//Main
void main(void) {
	initBoard();
	backlightOn();

	//Configure interrupts
	//Interrupt on RB1 = SW_E
	OpenRB1INT(PORTB_CHANGE_INT_ON & FALLING_EDGE_INT & PORTB_PULLUPS_OFF & PORTB_INT_PRIO_HIGH);

	//Interrupt on RB0 = SW_W
	OpenRB0INT(PORTB_CHANGE_INT_ON & FALLING_EDGE_INT & PORTB_PULLUPS_OFF & PORTB_INT_PRIO_LOW);

	//Enable button input => not needed, see dwengoBoard.c->initBoard();
	TRISB = 0xFF;

	//Init motors
	initializeMotors();
	initializeSensors();
	
	while (TRUE) 
	{
		if(SW_N == 0)
		{
			mode = 0;
			clearLCD();
			printStringToLCD("Doing moves brah", 1, 0);
			leftMotor(700);
			rightMotor(700);
			delay_s(4);
			leftMotor(1000);
			rightMotor(700);
			delay_s(4);
			leftMotor(700);
			rightMotor(1000);
			delay_s(4);
			leftMotor(-700);
			rightMotor(700);
			delay_s(4);
		}
		else if(mode == 1)
		{
			//Light eating
			
		}
		else if(SW_S == 0)
		{
			//Start light eating
			//mode = 1;
			clearLCD();
			printStringToLCD("Going to send string", 0, 0);
			initializeRS232();
			sendData();
		}
		else
		{
			//printStringToLCD("Cool story bro", 0, 0);
		}
	}
}

void sendMyInfo() 
{ // if we pull the trigger... IDENITIFY OURSELF!
  clearLCD();
  delay(50);
  selectLineOne();
  Serial.print("Sending my info!");
  delay(1000);
  clearLCD();
  sendDelay = 0;
}

void main(void) {
  unsigned char counter;
  BYTE camera_on = FALSE;
  BYTE camera_rec = FALSE;
  BYTE sw_e_prev = FALSE;
  BYTE sw_w_prev = FALSE;
  TRISA = OUTPUT;
  initBoard();
  initLCD();
  clearLCD();
  backlightOn();
  setCursorLCD(0,0);
  appendStringToLCD("The Stalker");
  setCursorLCD(1, 0);
  appendStringToLCD("...........");
  while (TRUE) {
      if (SW_E == PRESSED) {
          sw_e_prev = TRUE;
      } else if (sw_e_prev == TRUE) {   //SW_E key up
            sw_e_prev = FALSE;
            clearLCD();
            setCursorLCD(0,0);
            if (camera_on == TRUE) {
                appendStringToLCD("Shut down cam");
                camera_on = FALSE;
            } else {
                appendStringToLCD("Power up cam");
                camera_on = TRUE;
            }
            switchPower();
      }

      if (SW_W == PRESSED) {
          sw_w_prev = TRUE;
      } else if (sw_w_prev == TRUE) {   //SW_E key up
            sw_w_prev = FALSE;
            clearLCD();
            setCursorLCD(0,0);
            if (camera_rec == TRUE) {
                appendStringToLCD("Stop rec");
                camera_rec = FALSE;
            } else {
                appendStringToLCD("Start rec");
                camera_rec = TRUE;
            }
            switchRec();
      }

      delay_ms(200);
  }
}

/*
 * This function is called in lab1p2.c for testing purposes.
 * If everything is working properly, you should get this to look like the video on D2L
 * However, it is suggested that you test more than just this one function.
 */
void testLCD(){
    initLCD();
    int i = 0;
    while(1){
        printCharLCD('c');
        for(i = 0; i < 1000; i++) delayUs(1000);
        clearLCD();
        printStringLCD("Hello!");
        moveCursorLCD(1, 2);
        for(i = 0; i < 1000; i++) delayUs(1000);
        printStringLCD("Hello!");
        for(i = 0; i < 1000; i++) delayUs(1000);
        clearLCD();
    }
}

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...

	clearLCD();

	displayLCDCenteredString(0, autonNames[selectedAuton]);
	displayLCDCenteredString(1, "<  Enter   >");

	while(nLCDButtons != centerButton) // Center LCD button is not pressed
	{
		if(nLCDButtons == 0) // There are currently no buttons pressed
		{
			wait1Msec(10); // No need to waste processor cycles
		}
		else
		{
			if(nLCDButtons == leftButton && (selectedAuton - 1) >= 0) // The left button is pressed and a subtraction of 1 won't lead to a non-choice
			{
				selectedAuton--; // Decrease our current autonomous selection index by 1
			}

			if(nLCDButtons == rightButton && (selectedAuton + 1) <= totalAutons)// The right button is pressed and an addition of 1 won't lead to a non-choice
			{
				selectedAuton++; // Increase our current autonomous selection index by 1
			}

			while(nLCDButtons != 0) // Make sure we release the button to prevent it being impossible to use 1 step
			{
				wait1Msec(10); // No need to waste processor cycles
			}
		}

		// Add our selections to the display
		clearLCDLine(0);
		displayLCDCenteredString(0, autonNames[selectedAuton]);
	}

	clearLCD();

	displayLCDCenteredString(0, autonNames[selectedAuton]);
	displayLCDCenteredString(1, "Locked In");
}

void switchRec() {
    PORTAbits.RA1 = HIGH;
    delay_ms(1000);
    PORTAbits.RA1 = LOW;
    delay_ms(1000);
    clearLCD();
}

void loop() {
  if (Serial.available() > 0) {
    readInHit();
    
    selectLineOne();
    Serial.print("Input: ");
    Serial.print(inBit);
    Serial.print("        ");
    
    selectLineTwo();
    Serial.print("Hit by: ");
    Serial.print( playerNameString() );
    delay(5000);
    clearLCD();    
    flushSerialIn();
  }
  
  if ((digitalRead(triggerPin) == HIGH) && (sendDelay > 200))
    sendMyInfo();
  
  selectLineOne();
  Serial.print("No Hit...       ");
  selectLineTwo();
  Serial.print("Gun:");
  if (sendDelay < 200)
    Serial.print(" Charging.. ");
  else
    Serial.print(" READY!     ");
  delay(10);
  
  if (sendDelay == 32760) // prevent int overflow.
    sendDelay = 32760;
  else 
    sendDelay++;
}

void initLCD(void) {
	LCD_BACKLIGHT_DIR = OUTPUT;   // Set LCD backlight-pin as output
	ADCON1 = 0x0A;                // RE0-2 digital
	delay_ms(5);                  // wait 5ms

	LCD_DATA = 0;
	LCD_DATA_DIR = 0xff;

	LCD_RS_DIR = OUTPUT;
	LCD_RW_DIR = OUTPUT;
	LCD_EN_DIR = OUTPUT;

	LCD_RS = 0;
	LCD_RW = 0;
	LCD_EN = 0;

	delay_ms(15);

	LCD_DATA_DIR = 0;

	commandLCD(0b00110000);
	commandLCD(0b00110000);
	commandLCD(0b00110000);
	commandLCD(0b00111000);
	commandLCD(0b00000110);
	commandLCD(0b00011000);
	commandLCD(0b00001100);
	lcd_info.line = lcd_info.pos = 0;
	clearLCD();
}

void initBoard(void){

	//if LCD display is connected (default)
#ifndef	NO_LCD
	initLCD();
	clearLCD();
#endif

	//	Set buttons as inputs

	SW_C_IN;
	SW_N_IN;
	SW_E_IN;
	SW_S_IN;
	SW_W_IN;

	//	Enalbe pullups for the switches

	SW_C_HIGH;
	SW_N_HIGH;
	SW_E_HIGH;
	SW_S_HIGH;
	SW_W_HIGH;

	//	Set led pins as output

	LEDS_DIR = PORT_HIGH;	//LED pins as output
	LEDS = PORT_LOW;		//Turn LEDs off






}

void updateMainMenuText()
{
    if(buttonPressed)
    {
        buttonPressed = 0;
        delayMs(20);
        clearLCD();
        delayMs(20);
        if(cursorHighlightMainMenu == Messaging)
        {
            currentMenuState = Messaging;
        }
        else if(cursorHighlightMainMenu == Time)
        {
            currentMenuState = Time;
        }
        else if(cursorHighlightMainMenu == PowerOff)
        {
            currentMenuState = PowerOff;
        }
        else if(cursorHighlightMainMenu == GoBack)
        {
            currentMenuState = GoBack;
        }
    }
}

void lostBall() {
	//Unfortunately we lost track of where the ball was.
	//We could try to relocate it, we're going back to seek mode.
	setSpeed(0);
	clearLCD(LCD);
	sendStringToLCD(LCD, "Lost the ball!");
	_delay_ms(10);
	unsigned dirL = getDirL();
	unsigned dirR = getDirR();
	setDirL(-1);
	setDirR(-1);
	setSpeed(EVASIVE_SPEED);
	_delay_ms(EVASIVE_REV_DURATION);
	setSpeed(0);
	_delay_ms(250);
	setDir(1);
	setSpeed(0);
	setDirL(dirL);
	setDirR(dirR);
	//disableTBDetect();


	botState.phoneLooking = false;
	setBotMode(MODE_PICKUP);
}

	void eHealthDisplayClass::initECGScreen(void)
	{
		clearLCD();
		delay(200);

		timePrevious = 0;
		timePreviousMeassure = 0;
		time = 0;
		countPrevious = 0;

		//ECG LOGO
		line(20, 49, 21, 49, 1); 	line(22, 49, 24, 55, 1);
		line(24, 55, 28, 45, 1);	line(28, 45, 29, 49, 1);
		line(29, 49, 32, 49, 1);	delay(100);

		//LETRAS POR PANTALLA
		coordinates(42, 52);	writeLCD("ECG");
		coordinates(100, 52);	writeLCD( "cpm");

		delay(150);
		timePreviousMeassure = millis();
		count = 0;
		printHeart(1);
		x = 8;
	}

void init()
{
    WDTCTL = WDTPW + WDTHOLD; // disable WDT
    BCSCTL1 = CALBC1_1MHZ; // 1MHz clock
    DCOCTL = CALDCO_1MHZ;

    P1OUT = 0;
    P2OUT = 0;
    P1DIR = 0;
    P2DIR = 0;

    WDTCTL = WDT_ADLY_1000;                   // WDT 1s interval timer
    IE1 = WDTIE;                             // Enable WDT interrupt

    P1DIR |= LCD5110_SCE_PIN + LCD5110_DC_PIN + BACKLIGHT;
    P1OUT |= LCD5110_SCE_PIN + LCD5110_DC_PIN;

    SPI_init();
    __delay_cycles(50000);
    initLCD();
    clearLCD();

    defaultRX = 0;
    NRF_init(86);
    NRF_down();

    initKeyboard();
    // end init
};

int main(void)
{
	setClock();
	lcdPortConfig();
	lcdInit();
	
	clearLCD(black);
	
	
	while(1)
	{		
		if(boxes._1)
			makeOpenBox(220,80,yellow);
		else
			makeBox(220,80,yellow);	
		if(boxes._2)
			makeOpenBox(120,80,green);
		else
			makeBox(120,80,green);
		if(boxes._3)			
			makeOpenBox(20,80,red);
		else
			makeBox(20,80,red);
			
	}		
}

/*
 * Handle note-on messages
 * Precondition: 1 <= channel <= 16, 0 <= pitch < 128, 0 <= velocity < 128
 */
void HandleNoteOn(byte channel, byte pitch, byte velocity)
{
  //If zero velocity, turn the note off
  if (velocity == 0) {
    HandleNoteOff(channel, pitch, velocity);
    return;
  }
  
  double freq = frequencyTable[pitch];
  if (useSerial) {
    //Print a message to Serial Monitor
    Serial.println("NOTE ON");
    Serial.println("Channel: " + String(channel) + ", Pitch: " + String(pitch) + ", Velocity: " + String(velocity));
    Serial.println("Computed Frequency: "+String(freq)+"Hz");
  }
  if (useLCD) {
    //Print a message to the LCD
    clearLCD();
    printToLCD(0,0,"On F="+String(freq)+"Hz");
    printToLCD(0,1,"P="+String(pitch)+", V="+String(velocity));
  }
  ledOff(); //just to be safe
  
  DueTimer timer = Timer.getAvailable();
  timer.attachInterrupt(blinkLED);
  timer.setFrequency(freq);
  timer.start();
  activeTimers[pitch] = timer;
}

/** LCD (Mode Select Menu) **/
void modeMenu(void){
    _INT1IF = LOW;                  //set INT1 flag back to zero 
    _INT2IF = LOW;                  //set INT2 flag back to zero 
    clearLCD();
    setCursor(1,1);
    putsLCD("Stir");
    setCursor(2,1);
    putsLCD("Shake");
    modeMenuSelect = 32766;         // this handles cursor position by means of modulus
    myBOOLs.mainMenuTrue = FALSE;   // these determine which menu for ext interrupt 2
    myBOOLs.modeMenuTrue = TRUE;    // these determine which menu for ext interrupt 2
    myBOOLs.rpmMenuTrue = FALSE;    // these determine which menu for ext interrupt 2
    while(myBOOLs.modeDecide){
        if(modeMenuSelect % 2 == 0){
            setCursor(1,0);
            putsLCD(">");
            setCursor(2,0);
            putsLCD(" ");
        }
        else{
            setCursor(1,0);
            putsLCD(" ");
            setCursor(2,0);
            putsLCD(">");
        }
    }
}

void UART_1602_Init()
{
	uart3_init(9600);
	backlightOn() ;
	clearLCD();
    lcdPosition(0,0);
}

int main(void) {
    initLCD();
	char chr1[] = {0x15, 0xa, 0x15, 0xa, 0x15, 0xa, 0x15, 0xa};
	char chr2[] = {0x1f, 0x11, 0xa, 0x4, 0x4, 0x4, 0x4, 0x4};
	char chr3[] = {0x0, 0x3, 0x0, 0x7, 0x0, 0xf, 0x0, 0x1f};
	char chr4[] = {0x4, 0xa, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0};
	char chr5[] = {0x0, 0xe, 0x11, 0x1b, 0x11, 0x15, 0xe, 0x0};
	char chr6[] = {0x0, 0x0, 0x0, 0x4, 0x4, 0x1f, 0xe, 0x4};
	char chr7[] = {0x0, 0x3, 0x4, 0xe, 0x1f, 0x17, 0x1b, 0xe};
	char chr8[] = {0x1f, 0x15, 0xe, 0x4, 0x1f, 0x4, 0xa, 0x11};
						
	uploadCustomCharacterAt(0, chr1);
	uploadCustomCharacterAt(1, chr2);
	uploadCustomCharacterAt(2, chr3);
	uploadCustomCharacterAt(3, chr4);
	uploadCustomCharacterAt(4, chr5);
	uploadCustomCharacterAt(5, chr6);
	uploadCustomCharacterAt(6, chr7);
	uploadCustomCharacterAt(7, chr8);
	
	clearLCD();
	printLCDXY("Custom Characters",0,0);
	gotoLCD(1,1);
	characterLCD(0);
	characterLCD(1);
	characterLCD(2);
	characterLCD(3);
	characterLCD(4);
	characterLCD(5);
	characterLCD(6);
	characterLCD(7);
}

	virtual void TeleopInit() {
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		if (_autonomousCommand)
		{
			_autonomousCommand->Cancel();
		}
		
		clearLCD();
		
		_teleopCommand = (Command*)_teleopChooser->GetSelected();
//		_teleopCommand = (Command*)_noOpCmd;

		printCommandToLCD(_teleopCommand->GetName());
		
//		if (! CommandBase::azimuthSubsystem->IsCalibrated())
//		{
//			CommandBase::azimuthSubsystem->CalibrateAzimuth();
//		}
		_teleopCommand->Start();
//		_testRobotCmd->Start();
//		_cmd = new TestBridgeArmCmd(CommandBase::oi->driveTrigger);
//		_cmd->Start();
	}

int main() 
{
    enableInterrupts();
    initTMR2();
    char key = 'x';
    
    while(1);
    {
       clearLCD();
       InitKeyPad();
       
       switch(state)
       {
           case findKey:
               ScanKeys(); //should i update the key value here?
               break;
           case debouncePress:
               delayUs(5000); //Proper Delay?
               break;
           case debounceRelease:
               delayUs(5000);
               break;
           case display:
               printCharLCD(key);
               break;
       }
    }
    
}