//#include "Chassis.h"
#include <Arduino.h>
#include <HardwareSerial.h>


/// @brief Lit l'état du chassis en I2C
void Scan_chassis(struct chassis_reception_t * chassis_reception){
  unsigned char tampon2[14];
  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
  error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
  if (error !=0){
    Err_Chassi_com =1;IndexErr = 2;
    affiche_erreur("Scan_Chassi", "Erreur I2C");
    while(1);
  }else{
    Err_Chassi_com =0;
    IndexErr = 0;

    Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
    chassis_reception->status = tampon2[0];
  }
}

void send_Chassis(struct chassis_emission_t * chassis_emission){
  //if(nbr_essai<=10){
    // Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet
    int nb_pas_x, nb_pas_y, nb_pas_rot;

    // Conversion des mm ou radian en pas

    nb_pas_x = chassis_emission->translation_x_mm * 4.049;
    nb_pas_y = chassis_emission->translation_y_mm * 4.953;
    nb_pas_rot = chassis_emission->rotation_z_rad * 791.;

    Mot[0] = chassis_emission->status;
    //y*=-1;
    //y = y*direction;
    Mot[1] = nb_pas_x >>8; 
    Mot[2] = nb_pas_x;
    Mot[3] = nb_pas_y >>8;
    Mot[4] = nb_pas_y;
    //Serial.println(y);
    Mot[5] = nb_pas_rot >>8; 
    Mot[6] = nb_pas_rot;
    Mot[7] = chassis_emission->vitesse >>8;
    Mot[8] = chassis_emission->vitesse;
    Mot[9] = chassis_emission->acceleration >>8; 
    Mot[10] = chassis_emission->acceleration; 

    error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
    if (error !=0){Err_Cha_send =1; IndexErr = 5;}
    else{Err_Cha_send =0;IndexErr = 0;}
    if(error==0){   //si pas d'erreur de transmission alors RAZ des valeurs
      nbr_essai ++;
      cmd_chassi_x = 0;
      cmd_chassi_y = 0;
      Cmd_Angle = 0;
      vit = 0;
      acc = 0;
    }
}



void send_Chassis_RAZ(){
  uint8_t RAZ_Message[11]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

  error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, RAZ_Message, 11);
}