#define ID_FEETECH_ASC_G 11
#define ID_FEETECH_ASC_D 10

#define ASC_G_SIGNE 1
#define ASC_D_SIGNE -1

enum etat_ascenseur_t{
  INIT,
  CHERCHE_BUTEE_INIT,
  CHERCHE_BUTEE,
  EA_POS_INIT,
  ACTIF
}etat_ascenseur=CHERCHE_BUTEE_INIT;

void Ascenseur_init(void){
  Ascenseur_config_servo(ID_FEETECH_ASC_D);
  Ascenseur_config_servo(ID_FEETECH_ASC_G);
}

void Ascenseur_config_servo(int id){
  sms_sts.unLockEprom(id); // unlock EPROM-SAFE
  sms_sts.writeByte(id, 30, 1); // Resolution à 1
  sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_L, 0); // Multitour 
  sms_sts.writeByte(id, SMS_STS_MAX_ANGLE_LIMIT_H, 0); // Multitour 

  sms_sts.LockEprom(id); // unlock EPROM-SAFE
  Serial.printf("Servo ID : %d\n",id);
  for(int i=0; i < 40; i++){
    Serial.printf("Memoire %d : %d\n", i, sms_sts.readByte(ID_Feetech, i));
  }

}

void Ascenseur_gestion(void){
  u8 asc_ID[2] = {ID_FEETECH_ASC_D, ID_FEETECH_ASC_G};
  static int step=0;
  s16 tab_vitesses[2], tab_position[2];
  u16 tab_vitesses_u[2];
  u8 tab_acc[2];
  tab_acc[0] = 50;
  tab_acc[1] = 50;
  bool erreur;
  static int position_basse_gauche, position_haute_gauche;
  static int position_basse_droit, position_haute_droit;
  static bool butee_g = 0, butee_d = 0;

  switch(etat_ascenseur){
    case INIT:
      // On vérifie que les deux servomoteurs répondent
      do{
        int ID = sms_sts.Ping(asc_ID[0]);
        erreur = sms_sts.getLastError();
        ID = sms_sts.Ping(asc_ID[1]);
        erreur +=  sms_sts.getLastError();
      }while(erreur != 0);
      break;

    case CHERCHE_BUTEE_INIT:
      tab_vitesses[0] = 500 * ASC_D_SIGNE;
      tab_vitesses[1] = 500 * ASC_G_SIGNE;
      sms_sts.WheelMode(ID_FEETECH_ASC_D);
      sms_sts.WheelMode(ID_FEETECH_ASC_G);
      sms_sts.SyncWriteSpe(asc_ID, 2, tab_vitesses, tab_acc);
      delay(200);
      etat_ascenseur = CHERCHE_BUTEE;
      break;

    case CHERCHE_BUTEE:
      {
      int my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
      if(abs(my_load) > 150 ){
        sms_sts.EnableTorque(ID_FEETECH_ASC_D, 0);
        position_basse_droit = sms_sts.ReadPos(ID_FEETECH_ASC_D);
        Serial.printf("position_basse_droit:%d\n", position_basse_droit);
        butee_d = true;
      }

      my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
      if(abs(my_load) > 150 ){
        sms_sts.EnableTorque(ID_FEETECH_ASC_G, 0);
        position_basse_gauche = sms_sts.ReadPos(ID_FEETECH_ASC_G);
        Serial.printf("position_basse_gauche:%d\n", position_basse_gauche);
        butee_g = true;
      }

      if(butee_d && butee_g){
        etat_ascenseur = EA_POS_INIT;
        delay(500);
      }
      break;}

    case EA_POS_INIT:
      sms_sts.ServoMode(ID_FEETECH_ASC_D);
      sms_sts.ServoMode(ID_FEETECH_ASC_G);
      tab_position[0] = position_basse_droit + (-250 * ASC_D_SIGNE); // Droit
      tab_position[1] = position_basse_gauche + (-250 * ASC_G_SIGNE); // Gauche
      position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE);
      position_haute_gauche = position_basse_gauche - (8100 * ASC_G_SIGNE);
      tab_vitesses_u[0] = 2000;
      tab_vitesses_u[1] = 2000;
      sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
      etat_ascenseur = ACTIF;
      break;

    case ACTIF:
      etat_ascenseur = ACTIF;
      if (Serial.available() > 0) {
        // get incoming byte:
        int inByte = 0;
        inByte = Serial.read();
        if(inByte == 'd'){
          step++;
        }
        if(inByte == 'u'){
          step--;
        }
        while(Serial.available() > 0){
          inByte = Serial.read();
        }
        tab_position[0] = position_basse_droit + (250 + step * 500)* (ASC_D_SIGNE); // Droit
        tab_position[1] = position_basse_gauche + (250 + step * 500)* (ASC_G_SIGNE); // Gauche
        Serial.printf("position_droit:%d\n", tab_position[0]);
        Serial.printf("position_gauche:%d\n", tab_position[1]);
        if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) > 1){
          tab_position[0] = position_haute_droit;
        }
        if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) < 0){
          tab_position[0] = position_basse_droit;
        }
        if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) > 1){
          tab_position[1] = position_haute_gauche;
        }
        if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) < 0){
          tab_position[1] = position_basse_gauche;
        }
        

        tab_vitesses_u[0] = 2000;
        tab_vitesses_u[1] = 2000;
        sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
      }
      break;

  }
}