/*
  Copyright 2025
              Thibaut Ferrand / Ulysse Cura
                                           */

#include "IO.hpp"

#define NORMAL_SPEED 2048.0f
#define TURNING_SPEED 1500.0f
#define ON_STAGE_SPEED 1120.0f
 
// Unit tests activation
//#define UNIT_TESTS

/*
  Etapes :
   - 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
   - 2eme etape : avancer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
   - 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
   - 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
   - 5eme etape : faire tourner actionneurs pour figurine
*/

/*
  Modules necessaires :
   - angle          // Asservissement et control du robot
   - choc           // Controle du robot (fin !)
(?)- accelerometre  // Asservissement
 */

// Movement states
enum class State {
  WaitingForTirette,
  WaitingTimer,
  ForwardToRamp,
  ForwardToScene,
  Turn90Blue, // If blue team
  Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
  ForwardToSceneEdge,
  Dancing
};

// CodeCell implementation
CodeCell code_cell;
IO my_IO {&code_cell};

void setup()
{
  // Initialise serial
  Serial.begin(115200);

  Serial.println("Starting Initialisation");

  // Initialise the CodeCell for angle and tap detectIOn
  code_cell.Init(MOTION_ROTATION);

  // Initialise IO
  int nb_errors = my_IO.init();

  if(nb_errors)
  {
    Serial.printf("%d errors occured during IO init.", nb_errors);
    while(true);
  }

  Serial.println("Initalisation Finished");

  my_IO.motorControlOff();

  my_IO.setDir(0);
  my_IO.setSpeed(0.0f);
  //my_IO.startDancingAction(15);

#ifdef UNIT_TESTS
  Serial.println("UNIT_TESTS");
#endif
}

#ifdef UNIT_TESTS

void loop()
{
  my_IO.update();

  //Serial.printf("Is Ta Mere Selected : %d\n", my_IO.isSelectedColorBlue());
  //Serial.printf("Is Tirette Pulled : %d\n", my_IO.isTirettePulled());

  //my_IO.startDancingAction();

  //my_IO.motorControlOn();
  //my_IO.setSpeed(2048);
  //my_IO.setDirWithAngularSpeed(90);
}

#else

void loop()
{
  static State actual_state {State::WaitingForTirette};

  my_IO.update();

  switch(actual_state)
  {
    case State::WaitingForTirette:
      if(my_IO.isTirettePulled())
      {
        actual_state = State::WaitingTimer;
      }
      break;

    case State::WaitingTimer:
      static unsigned long initial_time = millis();

      if(millis() - initial_time >= 87000)
      //if(millis() - initial_time >= 2000)
      {
        my_IO.initGyroscope();
        actual_state = State::ForwardToRamp;
      }
      break;
    
    case State::ForwardToRamp:
      my_IO.motorControlOn();
      my_IO.setSpeed(2000.0f);

      if(my_IO.getAngle(Axes::Y) < -8.0f)
      {
        actual_state = State::ForwardToScene;
      }
      break;

    case State::ForwardToScene:
      my_IO.setSpeed(2000.0f);
      if(my_IO.getAngle(Axes::Y) > -3.5f)
      {
        my_IO.setSpeed(1300.0f);
        actual_state = my_IO.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
        //actual_state = State::Dancing;
      }
      break;

    case State::Turn90Blue:
      my_IO.setDirWithAngularSpeed(90.0f);

      if(my_IO.getAngle(Axes::Z) >= 80.0f)
      {
        actual_state = State::ForwardToSceneEdge;
      }
      break;

    case State::Turn90Yellow:
      my_IO.setDirWithAngularSpeed(-90.0f);

      if(my_IO.getAngle(Axes::Z) <= -80.0f)
      {
        actual_state = State::ForwardToSceneEdge;
      }
      break;

    case State::ForwardToSceneEdge:
      my_IO.setSpeed(1100.0f);

      if(my_IO.getAngle(Axes::Y) > 3.0f)
      {
        actual_state = State::Dancing;
      }
      break;

    case State::Dancing:
      my_IO.motorControlOff();
      my_IO.setSpeed(0.0f);
      my_IO.startDancingAction();
      break;
  }
}

#endif