// Bus pour le Dxl
#define DXL_BUS_SERIAL1 1  //Dynamixel on Serial1(USART1)  <-OpenCM9.04
#define DXL_BUS_SERIAL2 2  //Dynamixel on Serial2(USART2)  <-LN101,BT210
#define DXL_BUS_SERIAL3 3  //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP

// Nos quatre pattes, avec leur deux servomoteurs

// Arrière gauche
#define PATTE_1_SERVO1 7 // Proche corps
#define PATTE_1_SERVO2 9 // Proche sol

// Arrière droit
#define PATTE_2_SERVO1 8
#define PATTE_2_SERVO2 12

// Avant gauche
#define PATTE_3_SERVO1 16
#define PATTE_3_SERVO2 11

// Avant droit
#define PATTE_4_SERVO1 14
#define PATTE_4_SERVO2 18

// Positions initiales
#define POS_INITIALE_SERVO1_TYPE1 380
#define POS_INITIALE_SERVO1_TYPE2 POS_INITIALE_SERVO1_TYPE1 + 256

#define POS_INITIALE_SERVO2_TYPE1 280

// Positions avancement
#define ROTATION_SERVO1 150
#define ROTATION_SERVO2 120

// Vitesses servo
#define SPEED_SLOW 100
#define SPEED_RAPID 200

// On initialise notre bus dynamixel
Dynamixel Dxl(DXL_BUS_SERIAL1);

// Variables d'état de notre robot
int state_avg, state_avd, state_arg, state_ard;

void setup() 
{
  // Dynamixel 2.0 Baudrate -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps 
  Dxl.begin(3);
  
  // On passe nos 8 servo en mode position
  Dxl.jointMode(PATTE_1_SERVO1);
  Dxl.jointMode(PATTE_1_SERVO2);
  
  Dxl.jointMode(PATTE_2_SERVO1);
  Dxl.jointMode(PATTE_2_SERVO2);
  
  Dxl.jointMode(PATTE_3_SERVO1);
  Dxl.jointMode(PATTE_3_SERVO2);
  
  Dxl.jointMode(PATTE_4_SERVO1);
  Dxl.jointMode(PATTE_4_SERVO2);
  
  // On passe dans le premier état (-1 = lancement des moteurs; puis 0 = initialisation)
  state_avg = -1;
  state_avd = -1; 
  state_arg = -1;
  state_ard = -1;
  
  // On initialise notre communication série
  SerialUSB.println("Hi");
}

void loop() 
{
  // Gestion des différents états  
  
  // Patte avant droite (patte maitre)
  SerialUSB.print("Patte avant droite - état "); SerialUSB.println(state_avd);
  patteManager(&state_avd, &state_arg, PATTE_4_SERVO1, PATTE_4_SERVO2, 1, 0, 1);
  
  // Patte arrière gauche
  SerialUSB.print("Patte arrière gauche - état "); SerialUSB.println(state_arg);
  patteManager(&state_arg, &state_avg, PATTE_1_SERVO1, PATTE_1_SERVO2, 0, 1, 0);
  
  // Patte avant gauche
  SerialUSB.print("Patte avant gauche - état "); SerialUSB.println(state_avg);
  patteManager(&state_avg, &state_ard, PATTE_3_SERVO1, PATTE_3_SERVO2, 0, 1, 1);
  
  // Patte arrière droite
  SerialUSB.print("Patte arrière droite - état "); SerialUSB.println(state_ard);
  patteManager(&state_ard, &state_avd, PATTE_2_SERVO1, PATTE_2_SERVO2, 0, 0, 0);
  
  SerialUSB.println("###################");
}

/********************************
*     FONCTIONS D'ANIMATION     *
*********************************/

// Mouvement d'une patte
int patteManager(int *state, int *next_state, int patte_servo1, int patte_servo2, int is_master, int is_on_left, int is_on_front)
{
  // En fonction des paramètres, on défini les bonnes valeurs à partir des constantes (avg = ard et inversement)
  int position_intiale_servo1 = (is_on_front == is_on_left ? POS_INITIALE_SERVO1_TYPE1 : POS_INITIALE_SERVO1_TYPE2);
  int position_intiale_servo2 = (is_on_front == is_on_left ? POS_INITIALE_SERVO2_TYPE1 : POS_INITIALE_SERVO2_TYPE1);
  
  int rotation_move_servo1 = (is_on_left ? position_intiale_servo1 + ROTATION_SERVO1 : position_intiale_servo1 - ROTATION_SERVO1);
  int rotation_move_servo2 = position_intiale_servo2 + ROTATION_SERVO2;
  
  // Switch d'état
  switch (*state)
  {
    // Les différents états de notre patte
      
    case -1: // Etat spécial pour l'initialisation des positions au démarrage
      Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_RAPID);
      Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID);
      
      *state = 0; // Pret à démarrer ! 
      
      SerialUSB.println("Pret a démarrer");
      
      break;
    
    case 0: // Initialisation, en attente - si la patte est maitre et que le robot est pret à bouger, elle passe dans l'état 1
      /*if(is_master && isReadyToBegin())
      {
        *state = 1;
        SerialUSB.println("Patte maitre, initialisation - Système pret");
      }
      else
      {
        SerialUSB.println("En attente du maitre");
      }*/
      
      if(isReadyToBegin())
      {
        *state = 1;
      }

      break;
    
    case 1: // La patte est posée à plat
      SerialUSB.print("Etat 1, patte à plat...");
      
      // On bouge la patte
      Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_SLOW);
      Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID);
      
      // On change d'état
      *state = 2;
      
      break;
      
    case 2: // Etat transitoire : on attend que la patte se pose
      // La position est-elle la bonne ?
      if(
        (comparefive(Dxl.getPosition(patte_servo1), position_intiale_servo1)) &&
        (comparefive(Dxl.getPosition(patte_servo2), position_intiale_servo2))
      )
      {
        *state = 3;
        SerialUSB.println("OK");
      }
      else
      {
        SerialUSB.println(Dxl.getPosition(patte_servo1));
        SerialUSB.println(Dxl.getPosition(patte_servo2));
      }
        
      break;
    
    case 3: // La patte se lève et s'avance rapidement
      SerialUSB.print("Etat 3, patte se lève et avance...");
      
      // On bouge la patte  
      Dxl.setPosition(patte_servo1, rotation_move_servo1, SPEED_RAPID);
      Dxl.setPosition(patte_servo2, rotation_move_servo2, SPEED_RAPID);
      
      // On change d'état
      *state = 4;
      
      break;
      
    case 4: // Etat transitoire : on attend que la patte se soit avancée
      // La position est-elle la bonne ?
      if(
        (comparefive(Dxl.getPosition(patte_servo1), rotation_move_servo1)) &&
        (comparefive(Dxl.getPosition(patte_servo2), rotation_move_servo2))
      )
      {
        SerialUSB.println("OK");
        
        // On change d'état
        *state = 5;
      }
      else
      {        
        SerialUSB.println(Dxl.getPosition(patte_servo1));
        SerialUSB.println(Dxl.getPosition(patte_servo2));
      }
      
      break;
      
    case 5: // Etat où la patte se repose sur le sol
      // On anime le servomoteur
      Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID);    
      
      // On change d'état
      *state = 6;
      
      SerialUSB.print("La patte se repose sur le sol... ");
      
      break;
      
    case 6: // Etat transitoire où l'on attend que la patte se repose
      // La position est-elle la bonne ?
      if(comparefive(Dxl.getPosition(patte_servo2), position_intiale_servo2))
      {
        *state = 7;
        
        // La patte reposée déclenche le mouvement de la suivante
        *next_state = 1;

        SerialUSB.println("OK");
      }
      else
      {
        SerialUSB.println(Dxl.getPosition(patte_servo2));
      }
      
      break;
      
    case 7: // Le corps pivote
      // Mouvement du servomoteur
      Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_SLOW);
      
      // Changement d'état
      *state = 0;
      
      SerialUSB.println("Pivot délenché, retour au début");  
      
      break;
      
    default:
      SerialUSB.println("Etat non supporté");
      break;
      
  }
}

/*****************
*     OUTILS     *
******************/

// Vérifier si deux nombres correspondent, à 5 près
int comparefive(int number1, int number2)
{
  return (abs(number1 - number2) <= 5);
}

// Vérifier que le système est dans un état d'interblocage pour démarrer : le maitre attend tous les esclaves, et les esclaves sont en attente du maitre
int isReadyToBegin()
{
  int statesInitCounter = 0;
  
  if(state_avg == 0) statesInitCounter++;
  if(state_avd == 0) statesInitCounter++;
  if(state_arg == 0) statesInitCounter++;
  if(state_ard == 0) statesInitCounter++;
  
  return (statesInitCounter == 4);
}