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

#define SERVO_ID 1

SMS_STS sms_sts;

enum etat_t{
  EN_COURS,
  TERMINE,
  ECHEC
};

void gestion_servo(void);
enum etat_t test_ping(void);
enum etat_t configure_servomoteur(void);
enum etat_t mouvement_servomoteur(void);

void lire_tous_les_registres(int servo_id);

// the setup routine runs once when you press reset:
void setup() {
  // initialize the digital pin as an output.
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);//sts舵机波特率1000000
  Serial1.begin(1000000);//sts舵机波特率1000000
  sms_sts.pSerial = &Serial1;
  delay(5000);

}

// the loop routine runs over and over again forever:
void loop() {
  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
  gestion_servo();
  digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
  gestion_servo();
}

void gestion_servo(){
  static enum {
    PING_SERVO,
    CONFIG_SERVO,
    MOUVEMENT_SERVO
  } etat_gestion_servo = PING_SERVO;

  enum etat_t etat_action;

  switch(etat_gestion_servo){
    case PING_SERVO:
      if(test_ping() == TERMINE){
//        etat_gestion_servo = CONFIG_SERVO;
      }
      break;

    case CONFIG_SERVO:
      etat_action = configure_servomoteur();
      if(etat_action == TERMINE){
        etat_gestion_servo = MOUVEMENT_SERVO;
      }
      if(etat_action == ECHEC){
        etat_gestion_servo = PING_SERVO;
      }
      break;

    case MOUVEMENT_SERVO:
      etat_action = mouvement_servomoteur();

      if(etat_action == ECHEC){
        etat_gestion_servo = PING_SERVO;
      }
      break;
      
      

  }
}

enum etat_t test_ping()
{
  int ID = sms_sts.Ping(1);
  static int found = 0;
  if(ID!=-1){
    Serial.print("Servo ID:");
    Serial.println(ID);
    if(found == 0){
      found = 1;
      lire_tous_les_registres(ID);
      return TERMINE;
    }
    delay(100);
  }else{
    Serial.print("Ping servo ID error!\n");
    found = 0;
    delay(2000);
  }
  return EN_COURS;
}

enum etat_t configure_servomoteur(){
  sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 3);		
  sms_sts.writeByte(SERVO_ID, 0x09, 0); // Butée min à 0
  sms_sts.writeByte(SERVO_ID, 0x0A, 0); // Butée min à 0
  sms_sts.writeByte(SERVO_ID, 0x0B, 0); // Buté max à 0, désactivation de la butée
  sms_sts.writeByte(SERVO_ID, 0x0C, 0); // Buté max à 0, désactivation de la butée

  sms_sts.writeByte(SERVO_ID, 0x1E, 3);	
  sms_sts.writeByte(SERVO_ID, 0x12, 0x6C);	// Registre de "Phase", valeur par défaut 108 (0x6C)


  return TERMINE;
}

enum etat_t mouvement_servomoteur(){
  static int position=0;
  static int temps_pas_ms = 0;
  static int temps_aff_ms = 0;
  static int pas_servo = 2048;
  int position_lue;
  char tampon[200];
  /// Toutes les 500 ms
  if(millis() - temps_pas_ms > 5000 ){
    temps_pas_ms = millis();
    // On avance ou recule de d'un pas
    position += pas_servo;
    sms_sts.WritePosEx(SERVO_ID, pas_servo, 2000);
    // si position > 5000 ou position < 0
    if(position > 35100 || position < 0){
      pas_servo = -pas_servo;
    }
    
  }

  if(millis() - temps_aff_ms > 10 ){
    temps_aff_ms = millis();
    sprintf(tampon, ">pos_consigne:%d\n", position);
    Serial.print(tampon);
    position_lue = sms_sts.ReadPos(SERVO_ID);
    if(position_lue != 32769){
      sprintf(tampon, ">pos_actuelle:%d\n", position_lue);
    }
    Serial.print(tampon);
  }
  
  return EN_COURS;
}

int lire_registre(int servo_id, int registre_adresse){
  return sms_sts.readByte(servo_id, registre_adresse); // Resolution à 1
}

void lire_tous_les_registres(int servo_id){
  char message[200]="";
  for (int i=0; i<0x46; i++){
    sprintf(message, "registre 0x%x: %d\n", i, lire_registre(servo_id, i));
    Serial.print(message);
  }

}