int main(void)
{
    init_ports();
    init_osc();
    startup(); // setup sequence
    TMR0_init(); // will use for 15 min timer

    int i;
    while(1)// main loop
    {
        while(!detect);// main loop, program spends majority here, detect active low

        for(i=5; i !=0; i--) // to separate beginning of trigger sequence
        {
            delayUs(50000);
        }
        trigger(); // take picture
        for(i=10; i !=0; i--) // delay after picture
        {
            delayUs(50000);
        }
    }

    return (EXIT_SUCCESS);
}

void writeFourBits(unsigned char word, unsigned int commandType, unsigned int delayAfter, unsigned int lower){
    //TODO:
    // set the commandType (RS value)
    // WTF is this here for we are supposed to use 4 bit mode and you have LATE 0, 2, 4, and 6 when you tell us to use the ODD registers
    //LATEbits.LATE0 = word&0x01; 
    //LATEbits.LATE2 = word&0x02;
    //LATEbits.LATE4 = word&0x04;
    //LATEbits.LATE6 = word&0x08;
    if(lower){ // set least sig bits
        TRIS_D4 = word&0x01;
        TRIS_D5 = word&0x02;
        TRIS_D6 = word&0x03;
        TRIS_D7 = word&0x04;
    }
    else{ // set most sig figs
        TRIS_D4 = word&0x01;
        TRIS_D5 = word&0x02;
        TRIS_D6 = word&0x03;
        TRIS_D7 = word&0x04;
    }
    LCD_RS = commandType; // 1 for write 0 for FUCKING USELESS we never read. and the ports would have to be changed to inputs instead of outputs to read anyways
    //enable
    LCD_E = 1; // This allows reading of data into LCD mem
    //delay
    delayUs(1);
    //disable
    LCD_E = 0; // Finishes writing data
    delayUs(1);
    delayUs(delayAfter);
}

CmdResult Command::IRQsend(CommandType cmd, unsigned int arg)
{
    unsigned char cc=static_cast<unsigned char>(cmd);
    //Handle ACMDxx as CMD55, CMDxx
    if(cc & 0x80)
    {
        CmdResult r=IRQsend(CMD55,(static_cast<unsigned int>(rca))<<16);
        if(r.IRQvalidateR1Response()==false)
            return CmdResult(cc & 0x3f,CmdResult::ACMDFail);
        //Bit 5 @ 1 = next command will be interpreted as ACMD
        if((r.getResponse() & (1<<5))==0)
            return CmdResult(cc & 0x3f,CmdResult::ACMDFail);
    }

    //Send command
    cc &= 0x3f;
    unsigned int command=SDIO_CMD_CPSMEN | static_cast<unsigned int>(cc);
    if(cc!=CMD0) command |= SDIO_CMD_WAITRESP_0; //CMD0 has no response
    if(cc==CMD2) command |= SDIO_CMD_WAITRESP_1; //CMD2 has long response
    if(cc==CMD9) command |= SDIO_CMD_WAITRESP_1; //CMD9 has long response
    SDIO->ARG=arg;
    SDIO->CMD=command;

    //CMD0 has no response, so wait until it is sent
    if(cc==CMD0)
    {
        for(int i=0;i<500;i++)
        {
            if(SDIO->STA & SDIO_STA_CMDSENT)
            {
                SDIO->ICR=0x7ff;//Clear flags
                return CmdResult(cc,CmdResult::Ok);
            }
            delayUs(1);
        }
        SDIO->ICR=0x7ff;//Clear flags
        return CmdResult(cc,CmdResult::Timeout);
    }

    //Command is not CMD0, so wait a reply
    for(int i=0;i<500;i++)
    {
        unsigned int status=SDIO->STA;
        if(status & SDIO_STA_CMDREND)
        {
            SDIO->ICR=0x7ff;//Clear flags
            if(SDIO->RESPCMD==cc) return CmdResult(cc,CmdResult::Ok);
            else return CmdResult(cc,CmdResult::RespNotMatch);
        }
        if(status & SDIO_STA_CCRCFAIL)
        {
            SDIO->ICR=SDIO_ICR_CCRCFAILC;
            return CmdResult(cc,CmdResult::CRCFail);
        }
        if(status & SDIO_STA_CTIMEOUT) break;
        delayUs(1);
    }
    SDIO->ICR=SDIO_ICR_CTIMEOUTC;
    return CmdResult(cc,CmdResult::Timeout);
}

void lcd_disp_sz_char_24(uchar cx,uchar cy,uchar* chr,uchar* buf)
{
	uchar *p,i,s,page;
	uchar port;
	if(chr!=0)
	{
		get24x24BytesFormGB2312s(chr,buf);
	}
	p=chrBuf_24;
	if(cx<2)
	{
		port=1;
		s=cx*24;
	}
	else
	{
		port=2;
		s=((cx-2)*24);
			
	}
	
	for(page=0;page<3;page++)
	{
		LCD_WrCmd(port,0xb8+cy*3+page);
		delayUs(100);
		LCD_WrCmd(port,0x40+s);
		delayUs(100);
		for(i=0;i<24;i++)
		{
			LCD_WrDat(port,*p);
			delayUs(10);
			p++;
		}
	}
}

//Initializes the LCD as described in the HD44780 datasheet.
//Normally called only by the initialize() function in utility.c.
void lcdInit()
{
	//configure LCD E (Enable) control pin as an output
	sbi(DDRD, 6);
	//configure LCD RS (Register Select) control pin as an output
	sbi(DDRD, 7);
	//set LCD E (Enable) line low inititally, so it can rise later
	cbi(PORTD, 6);

	//wait 15ms after power on
	delayMs(15);

	//Issue 'Function Set' commands to initialize LCD for 8-bit interface mode
	writeControl(0x38);
	delayUs(4900); //+100us in writeControl = 5000us or 5ms total
	writeControl(0x38);
	delayUs(50); //+100us in writeControl = 150us total
	writeControl(0x38);

	//Function Set command to specify 2 display lines and character font
	writeControl(0x38);

	//Display off
	lcdOff();

	//Clear display
	clearScreen();

	//Set entry mode
	writeControl(0x06);

	//Display on
	lcdOn();
}

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;
       }
    }
    
}

uint8_t getKeyColumnPattern()
{
  uint8_t result=0;

  PORTB&=~((1<<3)|(1<<2)|(1<<0)); // set not used multiplexer lines to 0

  DDRB|=((1<<3)|(1<<2)); // set multplexer lines as outputs
  DDRB&=~(1<<0); // set the selected line as input
  delayUs(KEYSCANDELAY);
  if(PINB&(1<<0)) result|=(1<<0) ;


  PORTB&=~((1<<3)|(1<<2)|(1<<0)); // set not used multiplexer lines to 0

  DDRB|=((1<<3)|(1<<0)); // set multplexer lines as outputs
  DDRB&=~(1<<2); // set the selected line as input
  delayUs(KEYSCANDELAY);
  if(PINB&(1<<2)) result|=(1<<1) ;


  PORTB&=~((1<<3)|(1<<2)|(1<<0)); // set not used multiplexer lines to 0

  DDRB|=((1<<2)|(1<<0)); // set multplexer lines as outputs
  DDRB&=~(1<<3); // set the selected line as input
  delayUs(KEYSCANDELAY);
  if(PINB&(1<<3)) result|=(1<<2) ;

  return result;
}

void ReadSensor(){
    int duration=0;
    int cm=0;
    Signal = OUTPUT; //Set Pin as an output
    LATBbits.LATB4 = 0; //Write a digital 0 to the pin to initialize the sensor
    delayUs(2);
    LATBbits.LATB4 = 1; //Write a digital 1 to the pin to initialize the sensor
    delayUs(3); //Wait for (aprox) 5 microseconds before turning the pin low
    LATBbits.LATB4 = 0; //Turns the pin low to a digital 0
    Signal = INPUT; // Sets the pin as an input to read the pulse from the Sensor
    //while (Signal == 0) {}
    T4CONbits.ON = 1; //Initiates the timer4
    delayUs(2);
    //while (Signal == 1) {}
    duration = TMR4;
    IFS0bits.AD1IF = 0;  //Turns the flag off
    T4CONbits.ON = 0;    //Turns the timer off
    
    TMR4 = 0;               //Clear TMR4
    duration = duration / 1000;
    // The speed of sound is 340 m/s or 29 microseconds per centimeter.
    // The ping travels out and back, so to find the distance of the
    // object we take half of the distance travelled.
    // cm = duration / 29 / 2, equals cm = duration / 58;
    cm = duration / 58;
    if (cm <= 30) {LATDbits.LATD2 = 1;}         // Turns the led ON
    else { LATDbits.LATD2 = 0;         // Turns the led OFF}
    }}

void DisplayCGRAM(unsigned char cx,unsigned char cy)
{
	
	uchar *p,i,s,page;
	uchar port;
	cy--;
	
	p=lbatStateGraph;
	if(cx<4)
	{
		port=1;
		s=cx<<4;
	}
	else
	{
		port=2;
		s=((cx-4)<<4);
			
	}
	
	for(page=0;page<2;page++)
	{
		LCD_WrCmd(port,0xb8+cy*2+page);
		delayUs(100);
		LCD_WrCmd(port,0x40+s);
		delayUs(100);
		for(i=0;i<16;i++)
		{
			LCD_WrDat(port,*p);
			delayUs(10);
			p++;
		}
	}
} 

int main(void)
{
    SYSTEMConfigPerformance(10000000);
    initTimer1();
    initSW2();
    initLEDs();
    enableInterrupts();
    state = led1;

    while(1)
    {
        //TODO: Using a finite-state machine, define the behavior of the LEDs
        //Debounce the switch
        switch(state) {
        case led1:
            turnOnLED(1);
            stateNext = led2;
            break;

        case led2:
            turnOnLED(2);
            stateNext = led1;
            break;

        case deBounce1:
            delayUs(10000);
            break;
        case deBounce2:
            delayUs(10000);
            break;
        }
    }

    return 0;
}

void lcd_disp_sz_SingleBytechar(uchar cy,uchar cx,uchar* chr)
{
	uchar *p,i,s,page;
	uchar port;

	getBytesFormASCIIs(chr);
	p=chrBuf;
	if(cx<8)
	{
		port=1;
		s=cx<<3;
	}
	else
	{
		port=2;
		s=((cx-8)<<3);
			
	}
	
	for(page=0;page<2;page++)
	{
		LCD_WrCmd(port,0xb8+cy*2+page);
		delayUs(100);
		LCD_WrCmd(port,0x40+s);
		delayUs(100);
		for(i=0;i<8;i++)
		{
			LCD_WrDat(port,*p);
			delayUs(10);
			p++;
		}
	}
}

void LCD_PutChar(unsigned char cx,unsigned char cy,unsigned char  chr){
	uchar *p,i,s,page;
	uchar port;
	chr=0;//消除warning 该变量无用，仅为兼容之前接口
	cy--;

	p=arrow16x16;
	if(cx<4)
	{
		port=1;
		s=cx<<4;
	}
	else
	{
		port=2;
		s=((cx-4)<<4);
			
	}
	
	for(page=0;page<2;page++)
	{
		LCD_WrCmd(port,0xb8+cy*2+page);
		delayUs(100);
		LCD_WrCmd(port,0x40+s);
		delayUs(100);
		for(i=0;i<16;i++)
		{
			LCD_WrDat(port,*p);
			delayUs(10);
			p++;
		}
	}
}

void lcd_disp_sz_char_24_original_x(uchar x,uchar cy,uchar* chr,uchar* buf)
{
	uchar *p,i,s,page;
	uchar port;
	if(chr!=0)
	{
		get24x24BytesFormGB2312s(chr,buf);
	}
	p=buf;
	if(x<64)
	{
		port=1;
		s=x;
	}
	else
	{
		port=2;
		s=x-64;
			
	}
	
	for(page=0;page<3;page++)
	{
		LCD_WrCmd(port,0xb8+cy*3+page);
		delayUs(100);
		LCD_WrCmd(port,0x40+s);
		delayUs(100);
		for(i=0;i<24;i++)
		{
			LCD_WrDat(port,*p);
			delayUs(10);
			p++;
		}
	}
}

/*******************************************************************************
* Description : 
* Syntax      : 
* Parameters I: 
* Parameters O: 
* return      : 
*******************************************************************************/
static void CC1101_reset(void)
{
    CSN_OUT_H;	    
    delayUs(200);
    CSN_OUT_L; 				
    delayUs(200);
    CSN_OUT_H; 				
    delayUs(200);
    CSN_OUT_L; 	
    
    if (TestSOMI())
    {    
        SPI_byte(SRES);	      // GDO2 _|-|_ 3.5us      
    }    
    else
    {
        RF.SOMI_false = 1u;       
    }          
    delayUs(200);
    if (TestSOMI())
    {
        NOP();
    }  
    else 
    {
         RF.SOMI_false = 1u;       
    }
    
    CSN_OUT_H; 	
}

void lcd_disp_sz_char(uchar cx,uchar cy,uchar* chr)
{
	uchar *p,i,s,page;
	uchar port;

	getBytesFormGB2312s(chr);
	p=chrBuf;
	if(cx<4)
	{
		port=1;
		s=cx<<4;
	}
	else
	{
		port=2;
		s=((cx-4)<<4);
			
	}
	
	for(page=0;page<2;page++)
	{
		LCD_WrCmd(port,0xb8+cy*2+page);
		delayUs(100);
		
		LCD_WrCmd(port,0x40+s);
		delayUs(100);
		for(i=0;i<16;i++)
		{
			LCD_WrDat(port,*p);
			delayUs(10);
			p++;
		}
	}
}

/*
void nRF24L01_TxPacket(uchar * addr,uchar addrLen,uchar *datas,uchar datasLen)
{   
	power_off();
    CE=0;
 //	CSN=1;  //SPI标止
 //   SCLK=0; //SPI时钟置低
//	delayUs(20);

    SPI_Write_Read_Register(WRITE_REG + EN_AA, 0x00);                   // 失能接收通道0自动应答
//  SPI_Write_Read_Register(WRITE_REG + EN_RXADDR, 0x00);               // 失能接收通道0
    SPI_Write_Read_Register(WRITE_REG + SETUP_RETR, 0x00);              // 失能自动重发
    SPI_Write_Read_Register(WRITE_REG + RF_CH, 0);                       // 选择射频通道0x00
    SPI_Write_Read_Register(WRITE_REG + RF_SETUP, 0x07);                // 数据传输率1Mbps，发射功率0dBm，低噪声放大器增益
//  SPI_Write_Read_Register(WRITE_REG + CONFIG, 0x7e);                  // CRC使能，16位CRC校验，上电
//  CE=1;
//  delayUs(200);   
////	CE=0;           //StandBy I模式 
    SPI_Write_Buffer(WRITE_REG + TX_ADDR, addr, addrLen);    // 写入发送地址              
    SPI_Write_Buffer(WR_TX_PLOAD,datas, datasLen);  // 装载数据	
    SPI_Write_Read_Register(WRITE_REG + CONFIG, 0x7e);            // IRQ收发完成中断响应，16位CRC，主发送
    CE=1;        //置高CE，激发数据发送
  //  delayUs(400);
	//delayUs(500);
	delayMs(50);
	CE=0; 

	SPI_Write_Read_Register(WRITE_REG + STATUS, 0xff); // 清除TX_DS或MAX_RT中断标志
	//SPI_Write_Read_Register(FLUSH_TX,0x0);
	clearTXFIFO();
}
*/
void nRF24L01_TxPacket(uchar * addr,uchar addrLen,uchar *datas,uchar datasLen)
{   
    CE=0;
 	CSN=1;  //SPI标止
    SCLK=0; //SPI时钟置低
	delayUs(20);
//	power_off();
//    SPI_Write_Buffer(WRITE_REG + TX_ADDR, TX_ADDRESS, TX_ADR_WIDTH);    // 写入发送地址
    SPI_Write_Read_Register(WRITE_REG + EN_AA, 0x00);                   // 失能接收通道0自动应答
//  SPI_Write_Read_Register(WRITE_REG + EN_RXADDR, 0x00);               // 失能接收通道0
    SPI_Write_Read_Register(WRITE_REG + SETUP_RETR, 0x00);              // 失能自动重发
    SPI_Write_Read_Register(WRITE_REG + RF_CH, 0x0);                       // 选择射频通道0x00
    SPI_Write_Read_Register(WRITE_REG + RF_SETUP, 0x07);                // 数据传输率1Mbps，发射功率0dBm，低噪声放大器增益
//  SPI_Write_Read_Register(WRITE_REG + CONFIG, 0x7e);                  // CRC使能，16位CRC校验，上电
//  CE=1;
//  delayUs(200);   
	CE=0;           //StandBy I模式 
    SPI_Write_Buffer(WRITE_REG + TX_ADDR, addr, addrLen);    // 写入发送地址              
    SPI_Write_Buffer(WR_TX_PLOAD,datas, datasLen);  // 装载数据	
    SPI_Write_Read_Register(WRITE_REG + CONFIG, 0x7e);            // IRQ收发完成中断响应，16位CRC，主发送
    CE=1;        //置高CE，激发数据发送
//    delayUs(600);
	delayUs(600);
	SPI_Write_Read(FLUSH_TX);
	SPI_Write_Read_Register(WRITE_REG + STATUS, 0xff); // 清除TX_DS或MAX_RT中断标志
}

int main(void)
{
    //Initialize new interrupt fix
    SYSTEMConfigPerformance(40000000);
    
   
#ifdef part1
   initSW(); 
   initLED(RUN_LED);
   initLED(STOP_LED);
   initTimer2();
   enableInterrupts();
   
   // initialize the lights         
   state = runToggle; 
   turnOffLED(STOP_LED);
   turnOnLED(RUN_LED);
   
    while(1){
        
        switch(state){
            // the state that toggles the LEDs 
            case runToggle:
                // switch the led's
                toggleAllLEDs();
                prevState = runToggle;
                
                state = waitForPress; //Go to debounce press state
                break;
                
            // wait for user input i.e. button press
            case waitForPress:
                
                while (state == waitForPress);
                
                break;
                
            // once the button has been pressed 
            case dbPress:
                
                delayUs(DBdelayTime); // Delay for 5ms
              
                while(state == dbPress );
               
                break;
            
            // once the button has been released 
            case dbRelease:
                
                delayUs(DBdelayTime); //Delay for 5ms

                 state = runToggle;
                 
                break;
        } 
    }
#endif
    return 0;
}

int main(void) {
    
    SYSTEMConfigPerformance(40000000);
    initKeypad();
    enableEnterruptKeypad();
    initTimer2();
    initLCD();
    enableInterrupts();
    moveCursorLCD(0,0);
    state = Wait;

    while (1) {
        switch (state) {
            case Wait:
                break;

            case Scan:
                key = scanKeypad();
                state = MoveCursor;
                break;
                
            case MoveCursor:
                if(count == 0) moveCursorLCD(0,0);
                else if (count == 9) moveCursorLCD(1,0);
                state = Print;
                break;
                
            case debounce1:
                delayUs(500);
                state = Scan;
                break;
                
            case debounce2:
                delayUs(500);
                state = MoveCursor;
                break;

            case Print:
                delayUs(100);
                if(key == 0) printCharLCD('0');
                else if(key == 1) printCharLCD('1');
                else if(key == 2) printCharLCD('2');
                else if(key == 3) printCharLCD('3');
                else if(key == 4) printCharLCD('4');
                else if(key == 5) printCharLCD('5');
                else if(key == 6) printCharLCD('6');
                else if(key == 7) printCharLCD('7');
                else if(key == 8) printCharLCD('8');
                else if(key == 9) printCharLCD('9');
                else if(key == 10) printCharLCD('*');
                else if(key == 11) printCharLCD('#');
                state = Wait;
                break;

        }
    }

    return 0;
}

void toggleE() {
    delayUs(5);
    LATCbits.LAT_E = ENABLED;
    delayUs(E_DELAY);
    LATCbits.LAT_E = DISABLED;
    delayUs(E_DELAY);
    return;
}

void rightcircle(){
    sendCommand("LVM 75000");
    delayUs(1000000);
    sendCommand("RMR -300000");
    sendCommand("LMR 150000");
    delayUs(8500000);
    sendCommand("LVM 155000");
    delayUs(100000);
    
}