#include <SCServo.h>
#include "Ascenseur.h"

#define SERVO_FOUCHE 5
#define SERVO_PINCE_GAUCHE 8
#define SERVO_PINCE_DROITE 9

#define FOURCHE_TRANSPORT 5, 307
#define FOURCHE_LEVEE     5, 227
#define FOURCHE_PRISE     5, 345

#define DOIGT_PINCE_GAUCHE_FERME 2080
#define DOIGT_PINCE_GAUCHE_OUVRE 2367
#define DOIGT_PINCE_GAUCHE_PARKING 1550

SMS_STS sms_sts;

int ID_Feetech = 4;

struct position_t{
  int nb_tour;
  uint position;
} cible_haute, cible_basse, actuelle;

enum etat_action_t{
  ACTION_EN_COURS,
  ACTION_TERMINEE,
  ACTION_ECHEC
};

enum translateur_action_t{
  TRANSLATEUR_INIT,
  TRANSLATEUR_AVANCE,
  TRANSLATEUR_RECULE,
}translateur_action = TRANSLATEUR_INIT;


/// @brief pilote la vitesse des moteurs
/// @param vitesse vitesse signée, sera saturée à 255
void Moteur_set(int vitesse){
  ledcWrite(3, 0);
  ledcWrite(4, 0);
  if(vitesse < 0){
    vitesse = -vitesse;
    if(vitesse > 255){
      vitesse = 255;
    }
    ledcWrite(3, vitesse);
  }else{
    if(vitesse > 255){
      vitesse = 255;
    }
    ledcWrite(4, vitesse);
  }
}

void Translateur_avance(){
  Moteur_set(-255);
  translateur_action = TRANSLATEUR_AVANCE;
}

void Translateur_recule(){
  Moteur_set(255);
  translateur_action = TRANSLATEUR_RECULE;
}

void setup()
{
  int liste_servo_id[4]= {3, 4, 10, 11};
  Serial.begin(115200);//sms舵机波特率115200
  Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
  sms_sts.pSerial = &Serial1;
  delay(5000);
  
  cible_haute.nb_tour = 0;
  cible_haute.position = 500;

  cible_basse.nb_tour = 2;
  cible_basse.position = 1500;

  actuelle.nb_tour = 0;

  //Servo fourche (12V)
  ledcAttach(5, 50, 12);
  // ledcWrite(5, 307); // Neutre
  ledcWrite(5, 307); // Position de transport
  //ledcWrite(5, 227); // Position levée (butée haute)
  //ledcWrite(5, 345); // Position de prise

/*
  while(1){
    ledcWrite(FOURCHE_PRISE);
    delay(1000);
    ledcWrite(FOURCHE_TRANSPORT);
    delay(1000);
    ledcWrite(FOURCHE_LEVEE);
    delay(1000);
    ledcWrite(FOURCHE_TRANSPORT);
    delay(5000);

  }*/
  int error = 0;
  do{
    error = 0;
    for (int i =0; i<4; i++){
      int ID = sms_sts.Ping(liste_servo_id[i]);
      printf("Servo %d: %d\n", liste_servo_id[i], ID );
      if( ID != liste_servo_id[i]){
        error = 1;
        delay(100);
      }
    }
  }while(error);


  // Servo pinces à aimant
  ledcAttach(8, 50, 12);
  ledcWrite(8, 307);

  ledcAttach(9, 50, 12);
  ledcWrite(9, 307);

  Translateur_init();
  

  /*while(1){

    ledcWrite(FOURCHE_PRISE);
    delay(1000);
    ledcWrite(FOURCHE_TRANSPORT);
    delay(1000);
    ledcWrite(FOURCHE_LEVEE);
    delay(1000);
    ledcWrite(FOURCHE_TRANSPORT);
    delay(1000);

    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_PARKING, 2400, 50);
    delay(1000);
    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_OUVRE, 2400, 50);
    delay(3000);
    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_FERME, 2400, 50);
    delay(5000);
    sms_sts.WritePosEx(3, DOIGT_PINCE_GAUCHE_OUVRE, 2400, 50);
    delay(5000);

  }*/
  /*
  sms_sts.WheelMode(10);
  sms_sts.WriteSpe(10, 2000, 50);
  while(1){
    printf(">load_d:%d\n", sms_sts.ReadLoad(10));
    delay(25);
  }*/




  if(sms_sts.FeedBack(ID_Feetech)!=-1){
    actuelle.position = sms_sts.ReadPos(-1);
  }else{
    do{
      Serial.println("Erreur lecture");
      delay(1000);
    }while(sms_sts.FeedBack(ID_Feetech) ==-1);
    actuelle.position = sms_sts.ReadPos(-1);
  }

  Ascenseur_init();  
}


void Translateur_init(void){
// Moteur
  ledcAttach(3, 500, 8);
  ledcWrite(3, 0);
  ledcAttach(4, 500, 8);
  ledcWrite(4, 0);
  // Etat des contacteurs de fin de course
  // relecture de 3 sur 2 (Avant)
  // relecture de 4 sur 10 (Arrière)
  pinMode(2, INPUT);
  pinMode(10, INPUT);
}

enum etat_action_t Translateur_etat(void){
  if(translateur_action == TRANSLATEUR_AVANCE){
    if(digitalRead(2) == 0){
      return ACTION_TERMINEE;
    }
  }
  if(translateur_action == TRANSLATEUR_RECULE){
    if(digitalRead(10) == 0){
      return ACTION_TERMINEE;
    }
  }
  return ACTION_EN_COURS;
}

void Translateur_cycle(void){
  if(Translateur_etat() == ACTION_TERMINEE){
    if(translateur_action == TRANSLATEUR_RECULE){
      Translateur_avance();
    }else{
      Translateur_recule();
    }
  }
}



void loop()
{
  static int m_pos=0;
  static unsigned long myTime=0;
  static position_t cible;

  
  
  
  if(millis() > myTime + 100){
    myTime = millis();
    Serial.print(">millis:");
    Serial.println(millis());
  }


    int Pos;
  int Speed;
  int Load;
  int Voltage;
  int Temper;
  int Move;
  int Current;


  if (Serial.available() > 0) {
    // get incoming byte:
    int inByte = 0;
    inByte = Serial.read();
    if(inByte == 'd'){
      Ascenseur_step_down();
    }
    if(inByte == 'u'){
      Ascenseur_step_up();
    }
    if(inByte == 'm'){
      Ascenseur_monte();
    }
    if(inByte == 'l'){
      Ascenseur_descend();
    }
    if(inByte == 'a'){
      Translateur_avance();
    }
    if(inByte == 'z'){
      Translateur_recule();
    }
    if(inByte == 'c'){
      while(Serial.available() > 0){
      inByte = Serial.read();
      }
      while(1){
        Translateur_cycle();
        if(Serial.available() > 0){
          break;
        }
        printf(">GPIO2:%d\n",digitalRead(2));
        printf(">GPIO10:%d\n",digitalRead(10));
      }
    }


    while(Serial.available() > 0){
      inByte = Serial.read();
    }
  }

  Ascenseur_gestion();
  /*
  if(sms_sts.FeedBack(ID)!=-1){
    Pos = sms_sts.ReadPos(-1);
    Speed = sms_sts.ReadSpeed(-1);
    Load = sms_sts.ReadLoad(-1);
    Voltage = sms_sts.ReadVoltage(-1);
    Temper = sms_sts.ReadTemper(-1);
    Move = sms_sts.ReadMove(-1);
    Current = sms_sts.ReadCurrent(-1);
    Serial.print(">Position:");
    Serial.println(Pos);
    Serial.print(">Speed:");
    Serial.println(Speed);
    Serial.print(">Load:");
    Serial.println(Load);
    Serial.print(">Voltage:");
    Serial.println(Voltage);
    Serial.print(">Temper:");
    Serial.println(Temper);
    Serial.print(">Move:");
    Serial.println(Move);
    Serial.print(">Current:");
    Serial.println(Current);
    delay(50);
  }else{
    Serial.println("FeedBack err");
    delay(500);
  }*/
}