// 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 150
#define SPEED_RAPID 300

// Les 4 directions + le repos
#define DIRECTION_NONE 1
#define DIRECTION_FRONT 2
#define DIRECTION_BACK 3
#define DIRECTION_LEFT 4
#define DIRECTION_RIGHT 5

// 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;
int direction, wanted_direction = NULL;

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;

  // Direction par défaut
  direction = DIRECTION_NONE;
  
  // On lie une interruption sur le port série USB
  // Pour les commandes du robot
  SerialUSB.attachInterrupt(USBInterrupt);
}

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

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

// Mouvement d'une patte
int pawManager(int *state, int *next_paw_state, int patte_servo1, int patte_servo2, 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 = POS_INITIALE_SERVO2_TYPE1;
  
  int rotation_move_servo1 = position_intiale_servo1;
  int rotation_move_servo2 = position_intiale_servo2 + ROTATION_SERVO2;

  // Le sens de déplacement influence les ordres donnés aux servomoteurs
  switch (direction)
  {
      case DIRECTION_FRONT:
        rotation_move_servo1 += (is_on_left ? ROTATION_SERVO1 : -1*ROTATION_SERVO1);
        break;

      case DIRECTION_BACK:
        rotation_move_servo1 += (is_on_left ? -1*ROTATION_SERVO1 : ROTATION_SERVO1);
        break;

      case DIRECTION_LEFT:
        rotation_move_servo1 -= ROTATION_SERVO1;
        break;

      case DIRECTION_RIGHT:
        rotation_move_servo1 += ROTATION_SERVO1;
        break;
  }
  
  // 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 le robot est pret à bouger, elle passe dans l'état 1
      if(isReadyToBegin())
      {
        // Si la direction doit être changée, elle doit l'être à ce moment
        if(direction != wanted_direction && wanted_direction != NULL)
        {
            direction = wanted_direction;
            wanted_direction = NULL;
            break;
        }

        *state = 1;
        SerialUSB.println("Système pret");
      }
      else
      {
        SerialUSB.println("En attente des autres pattes");
      }

      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))
      )
      {
        // L'état suivant dépend de si le robot doit avancer
        if(direction == DIRECTION_NONE)
        {
          *state = 0;
        }
        else
        {
          *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
        // Sauf si un changement de direction est décidé : on bloque le mouvement
        if(wanted_direction == NULL)
        {
          *next_paw_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;
      
  }
}

// La direction est reçue par le port série USB
// Cette fonction gère l'interruption
void USBInterrupt(byte* buffer, byte nCount)  
{
    // Ici, la valeur reçue est un nombre [0;5]
    // Donc tenant sur 1 seul octet
    // Pas besoin de parcourir au delà de l'index 0 du buffer
    SerialUSB.print("Reçu (ascii ; int) : ");
    SerialUSB.print(buffer[0]);
    SerialUSB.print(" ; ");   

    // On stocke la valeur dans une variable temporaire, en convertissant le char en int
    // Sous condition que la valeur reçue soit un nombre ASCII
    if(buffer[0] > 47 && buffer[0] < 58)
    {
        int tmp_direction = buffer[0] - '0';
        SerialUSB.print(tmp_direction);

        // La valeur est-elle cohérente ?
        switch(tmp_direction)
        {
            case DIRECTION_NONE:
            case DIRECTION_FRONT:
            case DIRECTION_BACK:
            case DIRECTION_LEFT:
            case DIRECTION_RIGHT:
              wanted_direction = tmp_direction;
              break;
        }
    }

    SerialUSB.println();
}

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

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

// Vérifier que toutes les pattes sont en attente de démarrage
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);
}