#include <stdexcept>
#include <CodeCell.h>

#include "IO.hpp"

#define UNIT_TESTS

/*
  Etapes :
   - 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
   - 2eme etape : avncer 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 {};

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

  // Initialise the CodeCell for angle and tap detectIOn
  code_cell.Init(MOTION_GYRO + MOTION_TAP_DETECTOR + MOTION_ACCELEROMETER);

  Serial.println("CACA");

  // Initialise
  my_IO.init();
}

#ifdef UNIT_TESTS

void loop()
{
  if(code_cell.Run())
  {
    my_IO.update();

    my_IO.isTirettePulled();
  }
}

#else

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

  // Runs  every 100ms - Put your code here
  if(code_cell.Run())
  {
    switch(actual_state)
    {
      case State::WaitingForTirette:
        if(IO::ext_controls.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:
        IO::motors.forward();

        if(IO::gyro.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.tapDetected())
        {
          actual_state = State::Dancing;
        }
        break;

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

#endif