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

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

#define FOURCHE_TRANSPORT 5, 266
#define FOURCHE_LEVEE     5, 206
#define FOURCHE_PRISE     5, 308

#define DOIGT_PINCE_GAUCHE_FERME 3, 1530
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
#define DOIGT_PINCE_GAUCHE_SEUIL 1545

#define DOIGT_PINCE_DROIT_FERME 4, 2475
#define DOIGT_PINCE_DROIT_OUVRE 4, 2365
#define DOIGT_PINCE_DROIT_SEUIL 1575

#define AIMANT_PINCE_DROIT_TIENT 8, 176
#define AIMANT_PINCE_DROIT_LACHE 8, 366

#define AIMANT_PINCE_GAUCHE_TIENT 9, 366
#define AIMANT_PINCE_GAUCHE_LACHE 9, 236

#define I2C_CDE_DEMANDE 0x00
#define I2C_CDE_REALISE 0x01

volatile byte * I2C_memory;

SMS_STS sms_sts;

int ID_Feetech = 4;

enum etat_action_t Actionneur_deplacement(void);
enum etat_action_t Actionneur_prise_initiale(void);
enum etat_action_t Actionneur_empile(void);
enum etat_action_t Actionneur_deplacement(void);

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);
  

  //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

  I2C_Slave_init(0x20);
  I2C_memory = get_i2c_data();

/*
  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);

  }*/
  Ascenseur_init();  
}

void reglage_servo(int pin_servo){
  static int valeur=307; // Neutre
  printf("reglage_servo %d\n", pin_servo);
  while(1){
    if (Serial.available() > 0) {
      // get incoming byte:
      int inByte = 0;
      inByte = Serial.read();
      if(inByte == 'e'){
        valeur++;
      }
      if(inByte == 'z'){
        valeur+=10;
      }
      if(inByte == 'a'){
        valeur+=100;
      }
      if(inByte == 'd'){
        valeur--;
      }
      if(inByte == 's'){
        valeur-=10;
      }
      if(inByte == 'f'){
        valeur-=100;
      }
      if(inByte == 'q'){
        break;
      }
      printf("Valeur %d\n", valeur);
      ledcWrite(pin_servo, valeur);
    }
    Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
    Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
    delay(50);
  }

}



void loop()
{
  static int m_pos=0;
  static unsigned long myTime=0;
  
  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;

  //Serial.printf("I2C: %d - %d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]);

  if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){
    
    switch (I2C_memory[I2C_CDE_DEMANDE]){
      case 0:
        // Aucune commande
        I2C_memory[I2C_CDE_REALISE] = 0;
        break;

      case 1:
        // Position de départ
        break;

      case 2: 
        // Position de déplacement
        // Fourche levée pour ne pas géner les capteurs
        if(Actionneur_deplacement() == ACTION_TERMINEE){
          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
        }
        break;
        
      case 3:
        // Position de pré-prise
        if(Actionneur_prepare_prise() == ACTION_TERMINEE){
          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
        }
        break;

      case 4:
        // Prise initiale 
        if(Actionneur_prise_initiale() == ACTION_TERMINEE){
          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
        }
        break;

      case 5:
        // Empile
        if(Actionneur_empile() == ACTION_TERMINEE){
          Serial.println("I2C : ACTION_TERMINEE !!!");
          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
        }
        break;
      

    }

  }


  if (Serial.available() > 0) {
    // get incoming byte:
    int inByte = 0;
    inByte = Serial.read();
    if(inByte == '?'){
      // Affiche le menu
      Serial.println("d: Ascenseur step down");
      Serial.println("u: Ascenseur step up");
      Serial.println("m: Ascenseur monte");
      Serial.println("l: Ascenseur descend");
      Serial.println("a: Translateur avance");
      Serial.println("z: Translateur recule");
      Serial.println("w: Doigt pince ouvre");
      Serial.println("x: Doigt pince ferme");
      Serial.println("1: Reglage servo 8");
      Serial.println("2: Reglage servo 9");
      Serial.println("3: Reglage servo fourche (5)");
      Serial.println("v: Cycle ascenseur");
      Serial.println("c: Cycle translateur");
      Serial.println("h: Actionneur, position de déplacement");
      Serial.println("n: Actionneur, prepare prise");
      Serial.println("j: Actionneur, prise initiale");
      Serial.println("k: Actionneur, empile");
      
      
      
    }
    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 == 'w'){
      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
      Servo_set(DOIGT_PINCE_DROIT_OUVRE);
    }
    if(inByte == 'x'){
      Servo_set(DOIGT_PINCE_GAUCHE_FERME);
      Servo_set(DOIGT_PINCE_DROIT_FERME);
    }
    if(inByte == '1'){
      reglage_servo(8);
    }
    if(inByte == '2'){
      reglage_servo(9);
    }
    if(inByte == '3'){
      reglage_servo(5);
    }
    if(inByte == 'h'){
      while(Actionneur_deplacement() == ACTION_EN_COURS);
    }
    if(inByte == 'j'){
      while(Actionneur_prise_initiale() == ACTION_EN_COURS);
    }
    if(inByte == 'k'){
      while(Actionneur_empile() == ACTION_EN_COURS);
    }
    if(inByte == 'n'){
      while(Actionneur_prepare_prise() == ACTION_EN_COURS);
    }
    /*
    if(inByte == 'u'){
      while(Actionneur_deplacement() == ACTION_EN_COURS);
      while(Actionneur_prise_initiale() == ACTION_EN_COURS);
      while(Actionneur_empile() == ACTION_EN_COURS);
    }*/
    if(inByte == 'v'){
      Ascenseur_cycle();
    }
    if(inByte == 'c'){
      Serial.println("Cycle translateur");
      while(Serial.available() > 0){
      inByte = Serial.read();
      }
      while(1){
        Translateur_cycle();
        if(Serial.available() > 0){
          break;
        }
        Serial.printf(">GPIO2:%d\n",digitalRead(2));
        Serial.printf(">GPIO10:%d\n",digitalRead(10));
        delay(25);
      }
    }


    while(Serial.available() > 0){
      inByte = Serial.read();
    }
  }
  Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
  Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
  Serial.printf(">GPIO2:%d\n",digitalRead(2));
  Serial.printf(">GPIO10:%d\n",digitalRead(10));
  Serial.printf(">OUT3:%d\n",digitalRead(3));
  Serial.printf(">OUT4:%d\n",digitalRead(4));

  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);
  }*/

  delay(25);
}

void Servo_set(int servo, int position){
  switch(servo){
    case 5:
    case 8:
    case 9:
      ledcWrite(servo, position);
      break;

    case 3:
    case 4:
      sms_sts.WritePosEx(servo, position, 2000);
      break;

  }
}

enum etat_action_t Actionneur_deplacement(void){
  Servo_set(FOURCHE_LEVEE);
  // Ascenseur en bas
  Ascenseur_descend();
  // Pinces internes fermées
  Servo_set(DOIGT_PINCE_DROIT_FERME);
  Servo_set(DOIGT_PINCE_GAUCHE_FERME);
  // Aimant rentrés
  Servo_set(AIMANT_PINCE_DROIT_LACHE);
  Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
  // Translateur à l'arrière
  Translateur_recule();

  return ACTION_TERMINEE;

}

enum etat_action_t Actionneur_prepare_prise(void){
  Servo_set(FOURCHE_PRISE);
  // Ascenseur en bas
  Ascenseur_descend();
  // Pinces internes fermées
  Servo_set(DOIGT_PINCE_DROIT_FERME);
  Servo_set(DOIGT_PINCE_GAUCHE_FERME);
  // Aimant rentrés
  Servo_set(AIMANT_PINCE_DROIT_TIENT);
  Servo_set(AIMANT_PINCE_GAUCHE_TIENT);
  // Translateur à l'arrière
  Translateur_recule();

  return ACTION_TERMINEE;

}

enum etat_action_t Actionneur_prise_initiale(void){
  static enum etat_actionneur_t{
    ACTIONNEUR_DEPILE,
    ACTIONNEUR_PRISE_INTERNE_1,
    ACTIONNEUR_PRISE_INTERNE_2,
    ACTIONNEUR_RECULE_PRISE_INTERNE_1,
    ACTIONNEUR_RECULE_PRISE_INTERNE_2
  } etat_actionneur=ACTIONNEUR_DEPILE;

  switch(etat_actionneur){
    case ACTIONNEUR_DEPILE:
      Servo_set(FOURCHE_LEVEE);
      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_1;
      break;

    case ACTIONNEUR_PRISE_INTERNE_1:
      Serial.println("ACTIONNEUR_PRISE_INTERNE_1");
      Translateur_avance();
      
      Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
      Servo_set(DOIGT_PINCE_DROIT_OUVRE);
      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2;
      break;

    case ACTIONNEUR_PRISE_INTERNE_2:
      Serial.println("ACTIONNEUR_PRISE_INTERNE_2");
      if(Translateur_etat() == ACTION_TERMINEE){
        delay(500);
        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
        Servo_set(DOIGT_PINCE_DROIT_FERME);
        delay(500);
        etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_1;
      }
      break;

    case ACTIONNEUR_RECULE_PRISE_INTERNE_1:
      Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_1");
      Translateur_recule();
      etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_2;
      break;

    case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
      Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
      if(Translateur_etat() == ACTION_TERMINEE){
        Servo_set(FOURCHE_TRANSPORT);
        etat_actionneur=ACTIONNEUR_DEPILE;
        return ACTION_TERMINEE;
      }
      break;
  }
  Ascenseur_gestion(); 
  return ACTION_EN_COURS;

}

enum etat_action_t Actionneur_empile(){
  static enum etat_actionneur_t{
    ACTIONNEUR_LEVE,
    ACTIONNEUR_LEVE_1,
    ACTIONNEUR_AVANCE_PRISE_INTERNE,
    ACTIONNEUR_DEPOSE_PRISE_INTERNE,
    ACTIONNEUR_RANGE_TRANSLATEUR,
    ACTIONNEUR_DESCEND
  } etat_actionneur=ACTIONNEUR_LEVE;

  switch(etat_actionneur){
    case ACTIONNEUR_LEVE:
      Serial.println("ACTIONNEUR_LEVE");
      etat_actionneur = ACTIONNEUR_LEVE_1;
      Servo_set(FOURCHE_PRISE);
      Ascenseur_monte();
      break;

    case ACTIONNEUR_LEVE_1:
      Serial.println("ACTIONNEUR_LEVE_1");
      if(Ascenseur_get_etat() == ACTION_TERMINEE){
        etat_actionneur = ACTIONNEUR_AVANCE_PRISE_INTERNE;
        Translateur_avance();
      }
      break;

    case ACTIONNEUR_AVANCE_PRISE_INTERNE:
      Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE");
      if(Translateur_etat() == ACTION_TERMINEE){
        delay(500);
        Ascenseur_depose();
        Servo_set(DOIGT_PINCE_DROIT_OUVRE);
        Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
        etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
      }
      break;

    case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
      Serial.println("ACTIONNEUR_DEPOSE_PRISE_INTERNE");
      if(Ascenseur_get_etat() == ACTION_TERMINEE){        
        delay(500);
        Translateur_recule();
        etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
      }
      break;

    case ACTIONNEUR_RANGE_TRANSLATEUR:
      Serial.println("ACTIONNEUR_RANGE_TRANSLATEUR");
      if(Translateur_etat() == ACTION_TERMINEE){
        etat_actionneur = ACTIONNEUR_DESCEND;
        Servo_set(DOIGT_PINCE_DROIT_FERME);
        Servo_set(DOIGT_PINCE_GAUCHE_FERME);
        Servo_set(AIMANT_PINCE_DROIT_LACHE);
        Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
        Ascenseur_descend();
      }
      break;

    case ACTIONNEUR_DESCEND:
      Serial.println("ACTIONNEUR_DESCEND");
      if(Ascenseur_get_etat() == ACTION_TERMINEE){
        etat_actionneur = ACTIONNEUR_LEVE;
        return ACTION_TERMINEE;
      }
      break;
  }
  Ascenseur_gestion(); 
  return ACTION_EN_COURS;
}