#include <pico/stdlib.h>
#include <stdio.h>
#include "include/io.h"

#define NORMAL_SPEED 2000.0f
#define TURNING_SPEED 1300.0f
#define ON_STAGE_SPEED 1500.0f

#define RAMPE_ANGLE -7.82907651006f
#define RAMPE_ANGLE_OFFSET 1.0f
#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
 
// 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
typedef enum state_t {
  WaitingForTirette,
  WaitingTimer,
  ForwardToRamp,
  ForwardToScene,
  SceneTimeout,
  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
} state_t;

io_t io;

void main(void)
{
    stdio_init_all();

    // Initialise IO
    init_io();

    motor_control_activated(true);

    io.target_dir = 0.0f;
    io.target_speed = 0.0f;
    //my_IO.startDancingAction(15);

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

    printf("Rampe Angle         : %f\n", RAMPE_ANGLE);
    printf("Rampe Angle Offset  : %f\n", RAMPE_ANGLE_OFFSET);
    printf("Rampe Start Angle   : %f\n", RAMPE_START_ANGLE);
    printf("Rampe End Angle     : %f\n", RAMPE_END_ANGLE);

    state_t actual_state = WaitingForTirette;

    while(true)
    {
        update_io();

        switch(actual_state)
        {
          case WaitingForTirette:
            if(io.is_tirette_pulled)
            {
                actual_state = WaitingTimer;
            }
            break;

          case WaitingTimer:
            unsigned long initial_time = io.time_ms;

            if(io.time_ms - initial_time >= 87000)
            {
                actual_state = ForwardToRamp;
            }
            break;

          case ForwardToRamp:
            motor_control_activated(true);
            io.target_speed = NORMAL_SPEED;

            if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
            {
                actual_state = SceneTimeout;
            }
            break;

          case SceneTimeout:
            unsigned long initial_time = io.time_ms;

            if(io.time_ms - initial_time >= 500)
            {
                actual_state = ForwardToScene;
            }
            break;

          case ForwardToScene:
            if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
            {
                io.target_speed = TURNING_SPEED;
                actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
            }
            break;

          case Turn90Blue:
            set_dir_with_angular_speed(90.0f);

            if(io.gyro_data.z_angle >= 80.0f)
            {
                actual_state = ForwardToSceneEdge;
            }
            break;

          case Turn90Yellow:
            set_dir_with_angular_speed(-90.0f);

            if(io.gyro_data.z_angle <= -80.0f)
            {
                actual_state = ForwardToSceneEdge;
            }
            break;

          case ForwardToSceneEdge:
            io.target_speed = ON_STAGE_SPEED;

            if(io.gyro_data.y_angle > 3.0f)
            {
                actual_state = Dancing;
            }
            break;

          case Dancing:
            motor_control_activated(false);
            io.target_speed = 0.0f;
            io.is_dancing = true;
            break;
        }
    }
}