#include "mbed.h"
#include <string>
#include <iostream>
#include "Huskylens.h"

Serial pc (PA_2, PA_3);
//DigitalOut led (LED_2);
Serial husky (PA_0,PA_1);

unsigned char reception_bidule[7]={0,0,0,0,0,0,'\n'};

unsigned char data_recept[50];
unsigned char cpt=0,flag_printf=0;

unsigned char data_taille=0,cpt_taille=0,chksum=0;

unsigned char tableau_COMMAND_REQUEST[4][16];

typedef enum 
{
    HEADER1,
    HEADER2,
    ADRESSE,
    TAILLE,
    CHECKSUM,
    DATA,
    CMD
}T_etat;

static T_etat etat = HEADER1;


void envoi_commande(unsigned char taille,unsigned char buffer[])        //Envoie la commande a la carte
{
        for(int i=0;i<taille;i++) 
        {
            husky.putc(buffer[i]);
        }
        pc.printf("Envoi commande\n");
    
}

void affichage_buffer_reception()                                       //Acquiere la donnée revoyé par la carte ex : [0x55]
{
    for(int i=0;i<data_taille+6;i++) //header + header2 + adress+ datataille + comande + data + checksum
        {
            pc.printf("%02x:",data_recept[i]);
        }
        pc.printf("\n");
}

void tram_creat(unsigned char tram)                                       
{
    for(int i=0;i<data_taille+6;i++) //header + header2 + adress+ datataille + comande + data + checksum
        {
            tableau_COMMAND_REQUEST[tram][i] = data_recept[i];
            pc.printf("%02x:",tableau_COMMAND_REQUEST[tram][i]);
        }
        pc.printf("\n");
}

void reception(void)                           //Automate de réception des données 
{       
        data_recept[cpt] = husky.getc();
        //flag_recept=1;
        switch(etat)
        {
            case HEADER1:chksum=0;  
                        cpt=0;
                        if(data_recept[cpt]==0x55){
                        etat=HEADER2;
                        }
                        break;
                        
            case HEADER2 : if(data_recept[cpt]==0xAA)
                                etat=ADRESSE;
                            else
                                etat=HEADER1;

                        break;
            case ADRESSE : if(data_recept[cpt]==0x11)
                            etat=TAILLE;
                            else
                            etat=HEADER1;
                        break;
            case TAILLE : data_taille=data_recept[cpt];
                        if(data_taille>=1)
                        etat=DATA;
                        else
                        etat=CMD;
                    break;
                
            case DATA:  cpt_taille++;
                        if(cpt_taille==data_taille)
                            etat=CMD;
                        break;
                        
            case CMD:      etat=CHECKSUM;               
                        break;
            
            
            case CHECKSUM :      etat=HEADER1;
                        
                        if(data_recept[cpt]==chksum)
                        {
                        flag_printf=1;
                        cpt_taille=0;
                        }
                        

                        break;
        }
        chksum=chksum+data_recept[cpt];
    cpt++;

}


int main()
{
    husky.attach(&reception,Serial::RxIrq);         //La fct réception est donc activé a chaque fois qu'une donnée passe dans le Rx
    husky.baud(9600);
    pc.printf("debut du programme\n");  
    envoi_commande(6,COMMAND_REQUEST_KNOCK);              //Test d'aqcuisiton avec commande_bidule
    
    bool flag_end = false;
    
    while(flag_end==false)
    {
        pc.printf("check:%02X\n",chksum);      
        if(flag_printf)
        {   
            affichage_buffer_reception();           //renvoie de données 
            flag_printf=0;
            flag_end=true;
        }
    }
    
    flag_end=false;
    envoi_commande(8,COMMAND_REQUEST_ALGORITHM_COLOR);
    
    while(flag_end==false)
    {
        pc.printf("check algo:%02X\n",chksum);
        if(flag_printf)
        {   
            affichage_buffer_reception();
            flag_printf=0;
            flag_end=true; 
        }
    }
    flag_end=false;
    
    envoi_commande(6,COMMAND_REQUEST);

    while(flag_end==false)        
    {
        pc.printf("check req:%02X\n",chksum);
        
        if(flag_printf)
        {   tram_creat(0);
//            affichage_buffer_reception();
            flag_printf=0;
         
            tram_creat(1);
  //          affichage_buffer_reception();
            flag_printf=0;

            tram_creat(2);
    //        affichage_buffer_reception();
            flag_printf=0;
         
            tram_creat(3);
      //      affichage_buffer_reception();
            flag_printf=0;
            flag_end=true;
        }
    }
    flag_end=false;
    
    envoi_commande(6,COMMAND_REQUEST_BLOCKS);

    while(1)        
    {
        pc.printf("check block:%02X\n",chksum);

        if(flag_printf)
        {   
        
            affichage_buffer_reception();
            flag_printf=0;

        }
    }
}
//### COMMAND_REQUEST_KNOCK(0x2C):

//Used for test connection with HUSKYLENS. When HUSKYLENS received this command, HUSKYLENS will return COMMAND_RETURN_OK.
//| Header | Header 2 | Address | Data Length | Command | Checksum |
//| :----- | :------- | :------ | :---------- | :------ | :------- |
//| 0x55   | 0xAA     | 0x11    | 0x00        | 0x2C    | 0x3C

//  0x55   | 0xAA     | 0x11    | 0x00        | 0x2E    | 0x3E