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


int Detect_gradin(struct detect_gradin_t * detect_gradin){
  return Detect_gradin(detect_gradin, true);
}

/// @brief Lit les capteurs VL53L1X
int Detect_gradin(struct detect_gradin_t * detect_gradin, bool continuous_try){
  unsigned char tampon[14];
  char chaine[200];
  int angle_mrad;
  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
  error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
  while (error !=0 && continuous_try){
    affiche_erreur("Detect_gradin", "Erreur I2C");
    error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
  }
  if(!error){
    detect_gradin->status = tampon[0];
    detect_gradin->centre_x_mm = tampon[1] << 24 | tampon[2] << 16 | tampon[3] << 8 | tampon[4];
    detect_gradin->centre_y_mm = tampon[5] << 24 | tampon[6] << 16 | tampon[7] << 8 | tampon[8];
    angle_mrad = tampon[9] << 24 | tampon[10] << 16 | tampon[11] << 8 | tampon[12];
    detect_gradin->angle_rad = angle_mrad / 1000.;
  }
  return error;
}

int lire_tirette(char * tirette){
  return lire_tirette(tirette, true);
}


int lire_tirette(char * tirette, bool continuous_try){
  unsigned char tampon;
  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
  error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
  while(error !=0 && continuous_try){
    affiche_erreur("lire_tirette", "Erreur I2C");
    error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
  }
  *tirette = tampon;
  Serial.printf(">tirette:%d", *tirette);
  return error;

}