#include "IO.hpp"
#include "Images.hpp"

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

  my_IO.getScreen()->drawBitmap(0, 0, Imgs::riombotique, 52, 64, WHITE);
  my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);

  //my_IO.motorControlOn();

  Serial.println("Initalisation Finished");

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

#ifdef UNIT_TESTS

void loop()
{
  my_IO.update();

  Serial.print("Is Tirette Pulled : ");
  Serial.println(my_IO.isTirettePulled());

  Serial.print("Is Color Blue : ");
  Serial.println(my_IO.isColorBlue());

  my_IO.m_motors[0].setSpeed(3200);
  my_IO.m_motors[1].setSpeed(0);
  //my_IO.setDir(0);
  //my_IO.setSpeed(1024.0f);
}

#else

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

  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 >= 90000)
      {
        actual_state = State::ForwardToRamp;
      }
      break;
    
    case State::ForwardToRamp:
      my_IO.forward();

      if(my_IO.getAngle() > 9)
      {
        actual_state = State::ForwardToScene;
      }
      break;

    case State::ForwardToScene:
      IO::motors.forward();

      if(IO::gyro.getAngle() < 1)
      {
        actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
      }
      break;

    case State::Turn90Blue:
      IO::motors.turnLeft();

      if(IO::gyro.getAngle() > 90)
      {
        actual_state = State::ForwardToSceneEdge;
      }
      break;

    case State::Turn90Yellow:
      IO::motors.turnRight();

      if(IO::gyro.getAngle() > 90)
      {
        actual_state = State::ForwardToSceneEdge;
      }
      break;

    case State::ForwardToSceneEdge:
      IO::motors.forward();

      if(IO::gyro.getAngle() < 0)
      {
        actual_state = State::Dancing;
      }
      break;

    case State::Dancing:
      IO::ext_controls.startSpecialAction();
      break;
  }
}

#endif