/*#include "mbed.h"
#include "LCD_DISCO_F429ZI.h"
#include "ov7670.h"

LCD_DISCO_F429ZI lcd;

DigitalOut led1(LED1);

OV7670 camera(
    PB_9,PB_8,       // SDA,SCL(I2C / SCCB)
    PB_7,PF_8,PA_10,   // VSYNC,HREF,WEN(FIFO)
    //PortD,0xFF000000,
    PE_6,PE_5,PB_6,PE_4,PC_9,PC_8,PC_7,PC_6, 
    PF_9,PF_7,PA_9) ; // RRST,OE,RCLK

Serial pc(USBTX,USBRX);

#define SIZEX (160)
#define SIZEY (120)
#define SIZE 19200
#define VGA     307200         //640*480
#define QVGA    76800          //320*240
#define QQVGA   19200          //160*120

unsigned char bank0 [SIZE];
unsigned char *bank1 = (unsigned char *)(0x2007C000);

int main()
{      
    led1 = 1;
  
    BSP_LCD_SetFont(&Font20);
    lcd.DisplayStringAt(0, LINE(1), (uint8_t *)"MBED EXAMPLE", CENTER_MODE);
    wait(1);
     
      lcd.Clear(LCD_COLOR_BLUE);
      lcd.SetBackColor(LCD_COLOR_BLUE);
      lcd.SetTextColor(LCD_COLOR_WHITE);
      wait(0.3);
      lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"DISCOVERY", CENTER_MODE);
      lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"STM32F429ZI", CENTER_MODE);
      wait(1);

      lcd.Clear(LCD_COLOR_GREEN);
      
      lcd.SetTextColor(LCD_COLOR_BLUE);
      lcd.FillRect(10, 20, 50, 50);
      wait(0.1);
      lcd.SetTextColor(LCD_COLOR_BROWN);
      lcd.FillCircle(80, 80, 50);
      wait(0.1);
      lcd.SetTextColor(LCD_COLOR_YELLOW);
      lcd.FillEllipse(150, 150, 50, 100);
      wait(0.1);
      lcd.SetTextColor(LCD_COLOR_RED);
      lcd.FillCircle(200, 200, 40);
      wait(1);

      lcd.SetBackColor(LCD_COLOR_ORANGE);
      lcd.SetTextColor(LCD_COLOR_CYAN);
      BSP_LCD_SetFont(&Font24);
      lcd.DisplayStringAt(0, LINE(7), (uint8_t *)"HAVE FUN !!!", CENTER_MODE);
      wait(1);

      led1 = !led1;
      wait(0.5);
     lcd.Clear(LCD_COLOR_WHITE);
      lcd.SetBackColor(LCD_COLOR_WHITE);
      lcd.SetTextColor(LCD_COLOR_WHITE);*/
      // Reset camera on power up
   /* camera.Reset() ;

    // Set up for 160*120 pixels RGB565
    camera.Init('r', QQVGA);
    
    // Print message to screen
    pc.printf("Hit Any Key to stop RGBx160x120 Capture Data.\r\n");
          
    // Kick things off by capturing an image
    camera.CaptureNext() ;
    while (camera.CaptureDone() == false) ;

    // Now enter the main loop
    while (!pc.readable()) {

        // Start reading in the image data from the camera hardware buffer                   
        camera.ReadStart();

        // Read in the first half of the image
        for (int i = 0; i < SIZE; i++) {
            bank0[i] =  camera.ReadOnebyte();
        }

        // Read in the second half of the image
        for (int i = 0; i < SIZE ; i++) {
            bank1[i] =  camera.ReadOnebyte();
        }  
        
        // Stop reading the image
        camera.ReadStop() ; 
        
        // Immediately request the next image to be captured (takes around 45ms)
        camera.CaptureNext() ;
        
        // Use this time to display the image on the screen
        // Display the top half
        lcd.DrawBitmap(0, 0, bank0);
        lcd.DrawBitmap(0, 60, bank1);
       // TFT.Bitmap(0,0,160,60,bank1);
        // Display the bottom half
       // TFT.Bitmap(0,60,160,60,bank0);
        

        
        // Now wait for the image to finish being captured
        while (camera.CaptureDone() == false) ;
    }
    pc.printf("Capture Stop.\r\n");*/
    
//}
/*
OV7670 camera(
    p28,p27,       // SDA,SCL(I2C / SCCB)
    p21,p22,p20,   // VSYNC,HREF,WEN(FIFO)
    Port0,0x07800033,
    p23,p24,p25) ; // RRST,OE,RCLK

// the TFT is connected to SPI pin 5-7
SPI_TFT TFT(p5, p6, p7, p8, p14,"TFT"); // mosi, miso, sclk, cs, reset

Serial pc(USBTX,USBRX);

#define SIZEX (160)
#define SIZEY (120)
#define SIZE 19200

unsigned char bank0 [SIZE];
unsigned char *bank1 = (unsigned char *)(0x2007C000);

int main() {

    // Set up the TFT
    TFT.claim(stdout);                          // Send stdout to the TFT display
    TFT.background(Black);                      // Set background to black
    TFT.foreground(White);                      // Set chars to white
    TFT.cls();                                  // Clear the screen
    TFT.set_font((unsigned char*) Arial12x12);  // Select the font
    TFT.set_orientation(3);                     // Select orientation

    // Reset camera on power up
    camera.Reset() ;

    // Set up for 160*120 pixels RGB565
    camera.InitQQVGA() ;
    
    // Print message to screen
    pc.printf("Hit Any Key to stop RGBx160x120 Capture Data.\r\n");
          
    // Kick things off by capturing an image
    camera.CaptureNext() ;
    while (camera.CaptureDone() == false) ;

    // Now enter the main loop
    while (!pc.readable()) {

        // Start reading in the image data from the camera hardware buffer                   
        camera.ReadStart();

        // Read in the first half of the image
        for (int i = 0; i < SIZE; i++) {
            bank0[i] =  camera.ReadOneByte();
        }

        // Read in the second half of the image
        for (int i = 0; i < SIZE ; i++) {
            bank1[i] =  camera.ReadOneByte();
        }  
        
        // Stop reading the image
        camera.ReadStop() ; 
        
        // Immediately request the next image to be captured (takes around 45ms)
        camera.CaptureNext() ;
        
        // Use this time to display the image on the screen
        // Display the top half
        TFT.Bitmap(0,0,160,60,bank1);
        // Display the bottom half
        TFT.Bitmap(0,60,160,60,bank0);
        

        
        // Now wait for the image to finish being captured
        while (camera.CaptureDone() == false) ;
    }
}
*/



#include "mbed.h"
#include "ov7670.h"
#include "LCD_DISCO_F429ZI.h"
#include "string"
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <inttypes.h>


//Timer t;
bool new_send = false;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Serial pc(USBTX,USBRX);
LCD_DISCO_F429ZI lcd;
//Camera
OV7670 camera(
    PC_9,PA_8,       // SDA,SCL(I2C / SCCB)
    PB_3,PF_6,PG_9,   // VSYNC,HREF,WEN(FIFO)
    //PortD,0xFF000000,
    PB_7,PE_3,PE_5,PE_6,PB_2,PE_4,PA_5,PE_2, 
    PC_3,PA_0,PB_4) ; // RRST,OE,RCLK

void parse_cmd();
void CameraSnap();
void CameraGrab();

#define SIZEX (160)
#define SIZEY (120)
#define SIZE 19200

unsigned char bank0 [SIZE];
unsigned char *bank1 = (unsigned char *)(0x2007C000);
//RESET
//extern "C" void mbed_reset();

//Serial

int t1 = 0; 
int t2 = 0;
int t3 = 0;
char word[25];
int snap = 0;
//

 
#define VGA     307200         //640*480
#define QVGA    76800          //320*240
#define QQVGA   19200          //160*120

static char format = ' ';
static int resolution = 0;

void rxCallback() {
    char l;
   
    while ((l= pc.getc())!=-1){
        //read 1 character
               // wor[i] = l;
               // i++;
               // arr += l;
        pc.printf("%c", l);
    }
    pc.printf("c");
    /*if(strcmp("hello",wor) == 0)              
       {
                    pc.printf("HELLO\r\n");   
    }*/
     //pc.printf("\r\n");*/
    //pc.scanf("%s\n", &word);
     //pc.printf("HELLO\r\n"); 
}

void periodicCallback(void){

        pc.printf("Snap_done %d\r\n", snap); 
        }

int main() 
{  
   // pc.format(8, Serial::None, 1);
    pc.baud(250000);    
    //pc.attach(&rxCallback);  
    Ticker ticker;
    ticker.attach(periodicCallback, 10); 
    
    BSP_LCD_SetFont(&Font20);
    lcd.DisplayStringAt(0, LINE(1), (uint8_t *)"MBED EXAMPLE", CENTER_MODE);
    wait(1);
     
      lcd.Clear(LCD_COLOR_BLUE);
      lcd.SetBackColor(LCD_COLOR_BLUE);
      lcd.SetTextColor(LCD_COLOR_WHITE);
      wait(0.3);
      lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"DISCOVERY", CENTER_MODE);
      lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"STM32F429ZI", CENTER_MODE);
      wait(1);

     lcd.Clear(LCD_COLOR_WHITE);
      lcd.SetBackColor(LCD_COLOR_WHITE);
      lcd.SetTextColor(LCD_COLOR_WHITE);
    
    pc.printf("SystemCoreClock: %dMHz\r\n", SystemCoreClock/1000000);       // print the clock frequency

  
                    format = 'b';
                    resolution = QQVGA;
                    if(camera.Init('b', QQVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");

 

    while(1) 
    { 
        CameraSnap();
    }
}



void parse_cmd()
{
            new_send = false;
             
            if(strcmp("hello", word) == 0)              
            {
                    pc.printf("HELLO\r\n");    
            }else
            if(strcmp("snap", word) == 0)              
            {
                    CameraSnap();
                    memset(word, 0, sizeof(word));      
            }else
            if(strcmp("init_bw_VGA", word) == 0)                    // Set up for 640*480 pixels RAW    
            {
                    format = 'b';
                    resolution = VGA;
                    if(camera.Init('b', VGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else  
            if(strcmp("init_yuv_QVGA", word) == 0)                  // Set up for 320*240 pixels YUV422   
            {
                    format = 'y';
                    resolution = QVGA;
                    if(camera.Init('b', QVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else  
            if(strcmp("init_rgb_QVGA", word) == 0)                  // Set up for 320*240 pixels RGB565   
            {
                    format = 'r';
                    resolution = QVGA;
                    if(camera.Init('r', QVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else 
            if(strcmp("init_bw_QVGA", word) == 0)                  // Set up for 320*240 pixels YUV (Only Y)         
            {
                    format = 'b';
                    resolution = QVGA;
                    if(camera.Init('b', QVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else  
            if(strcmp("init_yuv_QQVGA", word) == 0)                 // Set up for 160*120 pixels YUV422
            {                            
                    format = 'y';
                    resolution = QQVGA;
                    if(camera.Init('b', QQVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else   
            if(strcmp("init_rgb_QQVGA", word) == 0)                 // Set up for 160*120 pixels RGB565
            {                            
                    format = 'r';
                    resolution = QQVGA;
                    if(camera.Init('r', QQVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else
            if(strcmp("init_bw_QQVGA", word) == 0)                 // Set up for 160*120 pixels YUV (Only Y)
            {                        
                    format = 'b';
                    resolution = QQVGA;
                    if(camera.Init('b', QQVGA) != 1)
                    {
                      pc.printf("Init Fail\r\n");
                    }
                    pc.printf("Initializing done\r\n");
                    memset(word, 0, sizeof(word));
            }else
            /*if(strcmp("reset", word) == 0)              
            {
                    mbed_reset();        
            }else*/
            if(strcmp("time", word) == 0)              
            {
                    pc.printf("Tot time acq + send (mbed): %dms\r\n", t2-t1);
                    memset(word, 0, sizeof(word));
            }else
            if(strcmp("reg_status", word) == 0)              
            {  
                    int i = 0;
                    pc.printf("AD : +0 +1 +2 +3 +4 +5 +6 +7 +8 +9 +A +B +C +D +E +F");
                    for (i=0;i<OV7670_REGMAX;i++) 
                    {
                        int data;
                        data = camera.ReadReg(i); // READ REG
                        if ((i & 0x0F) == 0) 
                        {
                            pc.printf("\r\n%02X : ",i);
                        }
                        pc.printf("%02X ",data);
                    }
                    pc.printf("\r\n");
            }
            
            memset(word, 0, sizeof(word));
            
}

uint32_t createRGB(int z, int r, int g, int b)
{   
    return ((z & 0xff) << 24)+((r & 0xff) << 16) + ((g & 0xff) << 8) + (b & 0xff);
}


void CameraSnap()
{

        camera.CaptureNext();
        while(camera.CaptureDone() == false);           
        camera.ReadStart();  
        int xx = 0;
        int yy = 0;
        for(int x = 0; x<resolution; x++)
        {
               // Read in the first half of the image
              /* if(format == 'b' && resolution != VGA)
               {
                    camera.ReadOnebyte();
               }else
               if(format == 'y' || format == 'r')
               {
                    pc.putc(camera.ReadOnebyte());
               }     */
               // Read in the Second half of the image
               //pc.putc(camera.ReadOnebyte());      // Y only   
               
               camera.ReadOnebyte();
               int l = camera.ReadOnebyte();
               uint32_t k = createRGB(255, l, l, l);
               lcd.DrawPixel(xx+10, yy+10, k);     
               xx++;
               if (xx >= 160) {
                   yy++;
                   xx =0;
                }
                   
        }
        camera.ReadStop();   
        snap++;
}