#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <time.h>

#include "Messages.h"

  time_t rawtime;
  struct tm * timeinfo;


void envoi_données(uint8_t * données, unsigned int taille_données, FILE *fp){
    for(int i=0; i<taille_données; i++ ){
        fputc(données[i], fp);
        printf("0x%02x ", données[i]);
    }
}

void envoi_données_protocole(uint8_t * données, unsigned int taille_données, FILE *fp){
    fputc(0xFF, fp);printf("0xFF ");
    fputc(0xFF, fp);printf("0xFF ");
    fputc(taille_données + 1, fp);printf("%d ", taille_données + 1);
    envoi_données(données, taille_données,fp);
    fputc(0x00, fp); printf("0x00 ");
    
    fputc('\n', fp); // Besoin du '\n' pour que la trame soit réellement expédiée (avant d'avoir fini de remplir le tampon)

    printf("\n");

}

/// @brief Envoie des données, de l'entête et du caractère de fin du message
/// @param données 
/// @param taille_données 
/// @param fp 
void envoi_données_applicatives(uint8_t commande, uint8_t id_carte, uint8_t adresse_reg, uint8_t * données, unsigned int taille_données, FILE *fp){
    struct message_applicatif_t message_applicatif;
    message_applicatif.commande = commande;
    message_applicatif.id_carte = id_carte;
    message_applicatif.adresse_registre = adresse_reg;
    message_applicatif.taille_donnees = taille_données;
    memcpy(message_applicatif.donnees_applicative.donnees, données, taille_données);
    envoi_données_protocole((uint8_t *) &message_applicatif, taille_données + 4, fp);
}

int main(){
    FILE *fp;
    time_t rawtime, rawtime_old;
    time ( &rawtime );
    rawtime_old =0;
    char on=1;
    char filename[] = "/dev/ttyACM0";
    printf("Host cdc USB\n");
    fp = fopen(filename, "w");
    if(fp == NULL){
        printf("Open failed: %s\n", filename);
        return 1;
    }else{
        printf("Open success: %s\n", filename);;
    }
    int my_char;
    int type_message=0;

    // Données à envoyer
    struct msg_propulsion_position_t msg_propulsion_position;
    msg_propulsion_position.position_x_mm = 1.5;
    msg_propulsion_position.position_y_mm = 2.5;
    msg_propulsion_position.orientation_rad = -3;

    uint8_t mode = 2;

    struct msg_propulsion_pwm_t msg_propulsion_pwm;
    msg_propulsion_pwm.pwm_droit= -20;
    msg_propulsion_pwm.pwm_gauche= 20;

    struct msg_propulsion_vitesse_roues_t msg_propulsion_vitesse_roues;
    msg_propulsion_vitesse_roues.vitesse_droite_mm_s = 253.5;
    msg_propulsion_vitesse_roues.vitesse_gauche_mm_s = -153.2;

    struct msg_propulsion_vitesse_robot_t msg_propulsion_vitesse_robot;
    msg_propulsion_vitesse_robot.avance_mm_s = 354.3;
    msg_propulsion_vitesse_robot.rotation_rad_s = -2.3;
    
    while(1){
        time ( &rawtime );
        if(rawtime > rawtime_old){
            rawtime_old = rawtime;
            
            uint8_t données_led[10];
            données_led[0]='r';
            données_led[1]='D';
            données_led[2]=0x00;
            données_led[3]=0x01;
            if(on){
                données_led[4]=0x25;
            }else{
                données_led[4]=0x05;
            }
            on = !on;

            if (type_message>4) type_message=0;

            printf("envoi message\n");
            switch (type_message){
                case 0: 
                    printf("case 0\n");
                    envoi_données_applicatives('r', 'P', REG_PROPULSION_POSITION, (uint8_t *) &msg_propulsion_position, 
                    sizeof(msg_propulsion_position), fp); 
                    break;
                case 1:
                    printf("case 1\n");
                    envoi_données_applicatives('r', 'P', REG_PROPULSION_MODE, &mode, sizeof(mode), fp);
                    break;
                case 2: 
                    printf("case 2\n");
                    envoi_données_applicatives('r', 'P', REG_PROPULSION_PWM, (uint8_t *) &msg_propulsion_pwm, sizeof(msg_propulsion_pwm), fp);
                    break;
                case 3: 
                    printf("case 3\n");
                    envoi_données_applicatives('r', 'P', REG_PROPULSION_VITESSE_ROUES, (uint8_t *) &msg_propulsion_vitesse_roues, sizeof(msg_propulsion_vitesse_roues), fp);
                    break;
                case 4:
                    printf("case 2\n");
                    envoi_données_applicatives('r', 'P', REG_PROPULSION_VITESSE_ROBOT, (uint8_t *) &msg_propulsion_vitesse_robot, sizeof(msg_propulsion_vitesse_robot), fp);
                    break;

                default: type_message=0;

            }
            type_message++;

            //envoi_données_protocole(données_led, 5, fp);
            //envoi_données_applicatives('r', 'D', 0, &données_led[4], 1, fp);
            
            //envoi_données_applicatives('r', 'P', 0, (uint8_t *) &msg_propulsion_position, sizeof(msg_propulsion_position), fp);
        }
    }

    // Lecture de la liaison série (décodage hexa)
    do{
        my_char = fgetc(fp);
        printf("%02x ", my_char);
        if(my_char == '\n'){
            
            printf("\n");
        }
    }while(my_char != EOF);
    printf("fin de lecture\n");
    fclose(fp);

}