#include <stdio.h>
#include "C8051F020.h"
#include "c8051F020_SFR16.h"
#include "FO_M2__Structures_COMMANDES_INFORMATIONS_Serializer.h"
#include "cmd_motor.h"

const float mm_to_tick = ((float)TICK_TOUR)/(2.0*M_PI*((float)DIAMETRE_ROUE)/2.0);

COMMANDES_SERIALIZER motors;
INFORMATIONS_SERIALIZER info_motors;
char str[30];
char recep_string[30];
//-----------------------------------------------------------------------------
// Config oscillateur - SYSCLK = 22,1184MHz - Oscillateur externe à quartz
//-----------------------------------------------------------------------------
void Oscillator_Init_Osc_Quartz(void) {

      int i = 0;
    OSCXCN = 0x67;  // Config de l'horloge externe - Quartz > 6,7 MHz
    for (i = 0; i < 3000; i++);  // attente stabilisation Fosc quartz
    while ((OSCXCN & 0x80) == 0); // validation stabilité du quartz
    OSCICN = 0x0C;  // Commutation sur oscillateur externe
                         // L'oscillateur n'est pas stopp
}

void initUART1(void) {
    //Timer 1 par Paul
    CKCON |= 0x10;
    TMOD |= 0x20;
    PCON &= ~(0x40);
    PCON |= 0x80;
    TCLK0=0; //Transmit Clock Flag for UART0. 0 car on utilise Timer 1
  RCLK0=0; //Receive Clock Flag for UART0.
  TH1=0xB8;
  TR1=1;
    // INIT XBAR
    XBR2 |= 0x44;
    P0MDOUT |= 0x40; //P0.6 TX1 pushpull

    // INIT UART1
    SCON1 |= 0x50; //SM11 = 1 et REN1 = 1 Mode 1
    //TH1 = 0xB8; //Rechargement suivant la formule 20.1 pour le baudrate 19200 bauds
    SCON1 &= ~(0x02); //TI1 = 0
}

void sendChar(char c) {
  SCON1 &= ~(0x02); //TI1 = 0
    SBUF1 = c;
    while((SCON1&0x02)==0);
}

char recepChar(void) {
    char c;
    while((SCON1&0x01)==0);
    c = SBUF1;
    SCON1 &= ~(0x01); //RI1 = 0
    return c;
}

void recepString(void) {
    char c;
    unsigned short i = 0;
    recep_string[0] = '\0';

    while(1) {
        c = recepChar();
        if((c=='\r')||(c=='\n')) {
            recep_string[i] = '\0';
            break;
        }
        recep_string[i] = c;
        i++;
    }
}

unsigned short charLength(char * ptr) {
    unsigned short offset = 0;
    unsigned short count = 0;
    while (*(ptr + offset) != '\0') {
         ++count;
         ++offset;
    }
    return count;
}

void sendString(char * s) {
  unsigned short i;
    unsigned short l = charLength(s);
  for(i=0; i<l; i++) {
    sendChar(s[i]);
  }
}

void mmToTick(int distMot1, int distMot2) {
    if(distMot1 > 0) {
        motors.Ticks_mot1 = (int)((float)distMot1*mm_to_tick);
    }
    if(distMot2 > 0) {
        motors.Ticks_mot2 = (int)((float)distMot2*mm_to_tick);
    }
}

void update(void) {
    switch(motors.Etat_Commande) {

        case Reset:
            sendString("reset\r");
            break;

        case Getenc_1:
            sendString("getenc 1\r");
            recepString();
            info_motors.Etat_Response = Rep_getenc;
            sscanf(recep_string, "%d", &info_motors.Read_Val_enc_1);
            break;

        case Getenc_2:
            sendString("getenc 2\r");
            recepString();
            info_motors.Etat_Response = Rep_getenc;
            sscanf(recep_string, "%d", &info_motors.Read_Val_enc_2);
            break;

        case Getenc_1_2:
            sendString("getenc 1 2\r");
            recepString();
            info_motors.Etat_Response = Rep_getenc;
            sscanf(recep_string, "%d %d", &info_motors.Read_Val_enc_1, &info_motors.Read_Val_enc_2);
            break;

        case Clrenc_1:
            sendString("clrenc 1\r");
            break;

        case Clrenc_2:
            sendString("clrenc 2\r");
            break;

        case mogo_1:
            str[0] = '\0';
            sprintf(str, "mogo 1:%d\r", motors.Vitesse_Mot1);
            sendString(str);
            break;

        case mogo_2:
            str[0] = '\0';
            sprintf(str, "mogo 2:%d\r", motors.Vitesse_Mot2);
            sendString(str);
            break;

        case mogo_1_2:
            str[0] = '\0';
            sprintf(str, "mogo 1:%d 2:%d\r", motors.Vitesse_Mot1, motors.Vitesse_Mot2);
            sendString(str);
            break;

        case Vpid_set:
            str[0] = '\0';
            sprintf(str, "vpid %d:%d:%d:%d\r", motors.Set_P, motors.Set_I, motors.Set_D, motors.Set_L_A);
            sendString(str);
            break;

        case Vpid_read:
            sendString("vpid\r");
            recepString();
            info_motors.Etat_Response = Rep_vpid;
            sscanf(recep_string, "P:%d I:%d D:%d L:%d", &info_motors.Read_P, &info_motors.Read_I, &info_motors.Read_D, &info_motors.Read_L_A);
            break;

        case digo_1:
            str[0] = '\0';
            sprintf(str, "digo 1:%d:%d\r", motors.Ticks_mot1, motors.Vitesse_Mot1);
            sendString(str);
            break;

        case digo_2:
            str[0] = '\0';
            sprintf(str, "digo 2:%d:%d\r", motors.Ticks_mot2, motors.Vitesse_Mot2);
            sendString(str);
            break;

        case digo_1_2:
            str[0] = '\0';
            sprintf(str, "digo 1:%d:%d 2:%d:%d\r", motors.Ticks_mot1, motors.Vitesse_Mot1 , motors.Ticks_mot2, motors.Vitesse_Mot2);
            sendString(str);
            break;

        case Dpid_set:
            str[0] = '\0';
            sprintf(str, "dpid %d:%d:%d:%d\r", motors.Set_P, motors.Set_I, motors.Set_D, motors.Set_L_A);
            sendString(str);
            break;

        case Dpid_read:
            sendString("dpid\r");
            recepString();
            info_motors.Etat_Response = Rep_dpid;
            sscanf(recep_string, "P:%d I:%d D:%d A:%d", &info_motors.Read_P, &info_motors.Read_I, &info_motors.Read_D, &info_motors.Read_L_A);
            break;

        case Rpid_Stinger:
            sendString("rpid s\r");
            break;

        case Pids:
            sendString("pids\r");
            recepString();
            info_motors.Etat_Response = Rep_pids;
            sscanf(recep_string, "%d", &info_motors.Read_Pids);
            break;

        case Stop:
            sendString("stop\r");
            break;

        case Vel:
            sendString("vel\r");
            recepString();
            info_motors.Etat_Response = Rep_vel;
            sscanf(recep_string, "%d %d", &info_motors.Read_Vitesse_mot1, &info_motors.Read_Vitesse_mot2);
            break;

        case Restore:
            sendString("restore\r");

        case Commande_non:
        default :
            break;

    }
}