#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// Definiciones
#define NONE 0
#define GREEN 1
#define RED 2
#define YELLOW 3

const int PIN_POTENTIOMETER = 34;
const int PIN_LED_RED = 14;
const int PIN_LED_GREEN = 12;
const int PIN_LED_BLUE = 11;

const int FORCE_THRESHOLD_MIN = 400;
const int FORCE_THRESHOLD_MAX = 800;
const float ANGLE_MIN_DEG = 40.0;
const float ANGLE_MAX_DEG = 60.0;
const unsigned long FEEDBACK_DURATION = 500;
const bool TOUCH_SIMULATION = true;

// FSM States
enum State {
  STATE_GUIDE,
  STATE_STANDBY,
  STATE_EVALUATE,
  STATE_FEEDBACK
};
State actual_state = STATE_STANDBY;

// Eventos
enum EventType {
  EVENT_NONE,
  EVENT_FORCE_DETECTED,
  EVENT_TIMEOUT,    
  EVENT_CONTINUE,
  EVENT_FORCE_EVALUATE
};
struct Event {
  EventType type;
};
Event event;

// Variables
unsigned long feedbackStart = 0;
Adafruit_MPU6050 mpu;

int lastPotValue = -1; 
int currentPotValue = -1;
const int DEADZONE = 10;

// Prototipos
void initSensors();
void catch_event();
void read_event();
void fsm();
void enterStandby();
void evaluateManeuver();
void feedback(bool success, const char* reason, int force, bool touch, float angle);
float readAngle();
void update_led(int color);
void sendEvaluationResult(bool success, int force, bool touch, float angle, const char* failReason);
void sendEvaluationResultConsole(bool success, int force, bool touch, float angle, const char* failReason);

// Setup
void setup() {
  Serial.begin(115200);
  initSensors();
  actual_state = STATE_GUIDE;
  lastPotValue = analogRead(PIN_POTENTIOMETER);
}

// Loop
void loop() {
  catch_event();
  fsm();
}


// Leer evento según estado
void catch_event() {
   if (actual_state == STATE_FEEDBACK) {
    if (millis() - feedbackStart >= FEEDBACK_DURATION) {
      event.type = EVENT_TIMEOUT;
    }
    return; 
   }
  currentPotValue = analogRead(PIN_POTENTIOMETER);
  switch (actual_state) {
    case STATE_STANDBY:
      if(abs(currentPotValue - lastPotValue) > DEADZONE) {
        lastPotValue = currentPotValue;
        if (analogRead(PIN_POTENTIOMETER) >= FORCE_THRESHOLD_MIN) {
          event.type = EVENT_FORCE_DETECTED;
        }
      }
      break;

    case STATE_EVALUATE:
      event.type = EVENT_FORCE_EVALUATE;
      break;
    default:
      break;
  }
}

// Máquina de Estados
void fsm() {
  switch (actual_state) {
    case STATE_GUIDE:
        Serial.println("GUIA: Posiciona correctamente las manos.");
        delay(1500);
        Serial.println("GUIA: Aplica presión al torso.");
        delay(1500);
        Serial.println("GUIA: Mantén la inclinación adecuada.");
        delay(1500);
        Serial.println("GUIA: Esperando ejecución...");
        actual_state = STATE_STANDBY;
      break;
    case STATE_STANDBY:
      switch (event.type) {
        case EVENT_FORCE_DETECTED:
          //Serial.println("ESTADO: STATE_STANDBY -> EVENTO: EVENT_FORCE_DETECTED");
          actual_state = STATE_EVALUATE;
          break;
        case EVENT_CONTINUE:
          //Serial.println("ESTADO: STATE_STANDBY -> EVENTO: EVENT_CONTINUE");
          actual_state = STATE_STANDBY;
          break;
        default:
          break;
      }
      break;

    case STATE_EVALUATE:
      switch (event.type) {
        case EVENT_FORCE_EVALUATE:
          //Serial.println("ESTADO: STATE_EVALUATE -> EVENTO: EVENT_FORCE_EVALUATE");
          evaluateManeuver();
          actual_state = STATE_FEEDBACK;
          break;
        case EVENT_CONTINUE:
          //Serial.println("ESTADO: STATE_EVALUATE -> EVENTO: EVENT_CONTINUE");
          actual_state = STATE_EVALUATE;
          break;
        default:
          break;
      }
      break;

    case STATE_FEEDBACK:
      switch (event.type) {
        case EVENT_TIMEOUT:
          //Serial.println("ESTADO: STATE_FEEDBACK -> EVENTO: EVENT_TIMEOUT");
          enterStandby();
          actual_state = STATE_STANDBY;
          break;
        case EVENT_CONTINUE:
          //Serial.println("ESTADO: STATE_FEEDBACK -> EVENTO: EVENT_CONTINUE");
          actual_state = STATE_FEEDBACK;
          break;
        default:
          break;
      }
      break;
  }
}

// Estado Standby
void enterStandby() {
  actual_state = STATE_STANDBY;
  update_led(YELLOW);
  //Serial.println("Estado: STANDBY");
}

// Evaluación
void evaluateManeuver() {
  Serial.println("Estado: EVALUATE");

  //int potValue = analogRead(PIN_POTENTIOMETER);
  bool touch = TOUCH_SIMULATION;
  float angle = readAngle();

  bool forceOk = (currentPotValue >= FORCE_THRESHOLD_MIN && currentPotValue <= FORCE_THRESHOLD_MAX);
  bool touchOk = touch;
  bool angleOk = (angle >= ANGLE_MIN_DEG && angle <= ANGLE_MAX_DEG);

  bool success = (forceOk && touchOk && angleOk);
  const char* reason = "";

  if (!success) {
    if (!forceOk) reason = "Fuerza fuera de rango";
    else if (!touchOk) reason = "Sin contacto";
    else if (!angleOk) reason = "Ángulo incorrecto";
  }

  feedback(success, reason, currentPotValue, touch, angle);
}

// Feedback
void feedback(bool success, const char* reason, int force, bool touch, float angle) {
  //Serial.print("Feedback: ");
  //Serial.println(reason);

  update_led(success ? GREEN : RED);
  feedbackStart = millis();
  actual_state = STATE_FEEDBACK;
  sendEvaluationResultConsole(success, force, touch, angle, reason);
}

void sendEvaluationResult(bool success, int force, bool touch, float angle, const char* failReason) {
  String json = "{";
  json += "\"force_value\":" + String(force) + ",";
  json += "\"force_status\":\"" + String((force >= FORCE_THRESHOLD_MIN && force <= FORCE_THRESHOLD_MAX) ? "OK" : "OUT_OF_RANGE") + "\",";
  json += "\"touch_status\":\"" + String(touch ? "OK" : "NO_CONTACT") + "\",";
  json += "\"angle_deg\":" + String(angle, 1) + ",";
  json += "\"angle_status\":\"" + String((angle >= ANGLE_MIN_DEG && angle <= ANGLE_MAX_DEG) ? "OK" : "OUT_OF_RANGE") + "\",";
  json += "\"result\":\"" + String(success ? "CORRECT" : "INCORRECT") + "\",";
  json += "\"failure_reason\":\"" + String(failReason) + "\"";
  json += "}";

  //SerialBT.println(json);
}
void sendEvaluationResultConsole(bool success, int force, bool touch, float angle, const char* failReason) {
  Serial.println("{");
  Serial.print("  \"force_value\": ");
  Serial.print(force);
  Serial.println(",");

  Serial.print("  \"force_status\": \"");
  Serial.print((force >= FORCE_THRESHOLD_MIN && force <= FORCE_THRESHOLD_MAX) ? "OK" : "OUT_OF_RANGE");
  Serial.println("\",");

  Serial.print("  \"touch_status\": \"");
  Serial.print(touch ? "OK" : "NO_CONTACT");
  Serial.println("\",");

  Serial.print("  \"angle_deg\": ");
  Serial.print(angle, 1);
  Serial.println(",");

  Serial.print("  \"angle_status\": \"");
  Serial.print((angle >= ANGLE_MIN_DEG && angle <= ANGLE_MAX_DEG) ? "OK" : "OUT_OF_RANGE");
  Serial.println("\",");

  Serial.print("  \"result\": \"");
  Serial.print(success ? "CORRECT" : "INCORRECT");
  Serial.println("\",");

  Serial.print("  \"failure_reason\": \"");
  Serial.print(failReason);
  Serial.println("\"");

  Serial.println("}");
}
// Leer ángulo
float readAngle() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  float ax = accel.acceleration.x;
  float ay = accel.acceleration.y;
  float az = accel.acceleration.z;
  float pitch = atan2(ax, sqrt(ay * ay + az * az)) * 57.2958;

  //Serial.print("Ángulo: ");
  //Serial.println(pitch);
  return pitch;
}

// LED RGB
void update_led(int color) {
  digitalWrite(PIN_LED_RED, (color == RED || color == YELLOW) ? HIGH : LOW);
  digitalWrite(PIN_LED_GREEN, (color == GREEN || color == YELLOW) ? HIGH : LOW);
  digitalWrite(PIN_LED_BLUE, (color == NONE) ? LOW : (color == YELLOW) ? LOW : (color == GREEN) ? LOW : LOW);
}

// Inicialización sensores
void initSensors() {
  Wire.begin();
  if (!mpu.begin()) {
    Serial.println("Error al iniciar MPU6050");
    while (1) delay(10);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  pinMode(PIN_LED_RED, OUTPUT);
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_LED_BLUE, OUTPUT);

  update_led(YELLOW);
  Serial.println("MPU6050 iniciado");
}