#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>

// BLE UART service UUIDs
#define SERVICE_UUID        "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

// Global variables for accelerometer and gyroscope data
float ax = 0, ay = 0, az = 0;  // Accelerometer data
float gx = 0, gy = 0, gz = 0;  // Gyroscope data

// Other global variables
bool deviceFound = false;
BLEAdvertisedDevice* myDevice = nullptr;
BLEClient* pClient;
BLERemoteCharacteristic* pRemoteCharacteristic;

// Data processing variables
String packetBuffer = "";
bool expectingNewPacket = true;
const unsigned long PACKET_TIMEOUT = 100; // ms

// === Pin Definitions ===
const int in1Pin = 33;
const int in2Pin = 25;
const int enAPin = 32;
const int encoderPinA = 34;
const int encoderPinB = 35;
volatile long pulseCount = 0;
int lastEncoded = 0;

// Stepper
const int stepPin = 12;
const int dirPin = 14;

int pwmValue = 180;
const int limitSwitchPin = 26;  // Limit switch connected to pin 26

// Global variable to track the last detected face
int lastDetectedFace = -1;  // -1 means no face detected yet
bool motorMoved = false;    // Flag to track if motor has moved
bool motorDCUp = true;     // Flag for DC motor position (up or down)
unsigned long lastGyroCheck = 0;  // Time when the gyroscope was last checked
const unsigned long GYRO_CHECK_DELAY = 1000; // Delay to confirm gyro values

void updateEncoder() {
  int MSB = digitalRead(encoderPinA);
  int LSB = digitalRead(encoderPinB);
  int encoded = (MSB << 1) | LSB;
  int sum = (lastEncoded << 2) | encoded;

  static const int8_t encoderLookup[16] = {
     0, -1,  1,  0,
     1,  0,  0, -1,
    -1,  0,  0,  1,
     0,  1, -1,  0
  };

  pulseCount += encoderLookup[sum];
  lastEncoded = encoded;
}

class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    Serial.print("Found device: ");
    Serial.println(advertisedDevice.getName().c_str());
    
    if ((advertisedDevice.getName() == "Xiao-Accel" || 
         advertisedDevice.getName() == "XIAO nRF52840 Sense") && 
        !deviceFound) {
      advertisedDevice.getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      deviceFound = true;
      Serial.println("Device found, stopping scan.");
    }
  }
};

void analyzeAccelerometerData(float x, float y, float z, float gx, float gy, float gz) {
    // Check if the gyroscope values are within the stop condition (±3.5 for each axis)
    if (gx >= -3.5 && gx <= 3.5 && gy >= -3.5 && gy <= 3.5 && gz >= -3.5 && gz <= 3.5) {
        Serial.println("Gyroscope values are within range, checking accelerometer data...");
        
        int currentFace = -1;  // Variable to hold the current detected face
        
        // Now, only evaluate accelerometer data when the die is no longer moving
        if (x >= -0.3 && x <= 0.3 && y >= -0.3 && y <= 0.3 && z >= 0.7 && z <= 1.3) {
            currentFace = 1;  // Face #1
            Serial.println("The die landed on #1");
        } 
        else if (x >= -0.3 && x <= 0.3 && y >= 0.7 && y <= 1.3 && z >= -0.3 && z <= 0.3) {
            currentFace = 2;  // Face #2
            Serial.println("The die landed on #2");
        }
        else if (x >= -1.4 && x <= -0.6 && y >= -0.4 && y <= 0.4 && z >= -0.4 && z <= 0.4) {
            currentFace = 3;  // Face #3
            Serial.println("The die landed on #3");
        }
        else if (x >= 0.7 && x <= 1.3 && y >= -0.3 && y <= 0.3 && z >= -0.3 && z <= 0.3) {
            currentFace = 4;  // Face #4
            Serial.println("The die landed on #4");
        } 
        else if (x >= -0.3 && x <= 0.3 && y >= -1.3 && y <= -0.7 && z >= -0.3 && z <= 0.3) {
            currentFace = 5;  // Face #5
            Serial.println("The die landed on #5");
        }
        else if (x >= -0.3 && x <= 0.3 && y >= -0.3 && y <= 0.3 && z >= -1.3 && z <= -0.7) {
            currentFace = 6;  // Face #6
            Serial.println("The die landed on #6");
        }

        // Trigger motor movement only if the face has changed and motors have not moved yet
        if (currentFace != -1 && currentFace != lastDetectedFace && !motorMoved) {
            moveMotors(currentFace);
            lastDetectedFace = currentFace;
            // NO reiniciar motorMoved aquí todavía!
        }
    } else {
        Serial.println("Gyroscope values out of range, skipping accelerometer checks.");
    
    if (motorDCUp) {
        motorMoved = false; // Permitir nuevo movimiento cuando vuelva a detectar una cara
    }
  }
}

void moveMotors(int face) {
    const int stepperSteps = 840;
    long dcPulses = -10000; // Motor hacia arriba (valor negativo)

    // Paso 1: Bajar motor DC solo si está arriba
    if (motorDCUp) {
        moveDC(10000);  // Valor positivo = hacia abajo
        // motorDCUp se actualiza automáticamente en moveDC()
    }

    // Paso 2: Mover motor paso a paso según cara
    switch (face) {
        case 1: moveStepper(-30); break;
        case 6: moveStepper(-30); break;
        case 2: moveStepper(-210); break;
        case 3: moveStepper(-420); break;
        case 4: moveStepper(-630); break;
        case 5: moveStepper(-840); break;
        default: break;
    }
    // Paso 3: Subir motor DC (siempre después del paso a paso)
    moveDC(dcPulses);  // Valor negativo = hacia arriba
    // motorDCUp se actualiza automáticamente en moveDC()

    // Modificación clave 3: NO reiniciar motorMoved aquí
}

void moveDC(long target) {
    Serial.print("Starting DC move to: ");
    Serial.println(target);

    long startPos = pulseCount;
    long targetAbs = abs(target);

    // Establecer dirección
    if (target > 0) {
        digitalWrite(in1Pin, HIGH);
        digitalWrite(in2Pin, LOW);
    } else if (target < 0) {
        digitalWrite(in1Pin, LOW);
        digitalWrite(in2Pin, HIGH);
    } else {
        Serial.println("Target is 0. No movement.");
        return;
    }

    analogWrite(enAPin, pwmValue);

    while (true) {
        long delta = abs(pulseCount - startPos);
        if (delta >= targetAbs) break;
        delay(1); // suavizado opcional
    }

    // Detener motor
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, LOW);
    analogWrite(enAPin, 0);

    // CORRECCIÓN CLAVE: Actualizar estado del motor correctamente
    if (target > 0) {
        motorDCUp = false;  // Acabamos de mover hacia abajo
    } else if (target < 0) {
        motorDCUp = true;   // Acabamos de mover hacia arriba
    }

    Serial.print("Final pulse count: ");
    Serial.println(pulseCount);
    Serial.println("DC move complete.\n");
}

void moveStepper(int steps) {
    Serial.print("Starting Stepper move: ");
    Serial.println(steps);
    
    // Paso 1: Homing - Mover hacia la derecha (dirección positiva) hasta activar el limit switch
    Serial.println("Iniciando homing hacia el origen...");
    digitalWrite(dirPin, HIGH);  // HIGH = Derecha (hacia el limit switch)
    
    while (digitalRead(limitSwitchPin) != LOW) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(500);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(500);
    }
    Serial.println("Origen encontrado (limit switch activado)");
    
    // Paso 2: Retroceder ligeramente para liberar el limit switch
    Serial.println("Retrocediendo ligeramente del limit switch");
    digitalWrite(dirPin, LOW);  // LOW = Izquierda (alejándose del switch)
    for (int i = 0; i < 5; i++) {  // 20 pasos de retroceso (ajustable)
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(500);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(500);
    }
    
    // Paso 3: Ahora movemos a la posición deseada
    if (steps != 0) {
        Serial.print("Moviendo a posición objetivo: ");
        Serial.println(steps);
        
        bool movingLeft = (steps < 0);
        digitalWrite(dirPin, movingLeft ? LOW : HIGH);
        int stepCount = abs(steps);
        
        for (int i = 0; i < stepCount; i++) {
            // Verificación de seguridad para movimiento hacia la derecha
            if (!movingLeft && digitalRead(limitSwitchPin) == LOW) {
                Serial.println("¡Alerta! Limit switch activado durante movimiento derecho");
                break;
            }
            
            digitalWrite(stepPin, HIGH);
            delayMicroseconds(500);
            digitalWrite(stepPin, LOW);
            delayMicroseconds(500);
        }
    }
    
    Serial.println("Stepper move complete.\n");
}
// void moveStepper(int steps) {
//     Serial.print("Starting Stepper move: ");
//     Serial.println(steps);
    
//     // Paso 1: Homing obligatorio al inicio
//     Serial.println("Buscando origen...");
//     digitalWrite(dirPin, HIGH);  // Derecha (hacia el limit switch)
    
//     while (digitalRead(limitSwitchPin) != LOW) {  // Mover hasta activar switch
//         digitalWrite(stepPin, HIGH);
//         delayMicroseconds(500);
//         digitalWrite(stepPin, LOW);
//         delayMicroseconds(500);
//     }
//     Serial.println("Origen encontrado");
    
//     // Paso 2: Retroceder para liberar el switch
//     Serial.println("Liberando switch");
//     digitalWrite(dirPin, LOW);  // Izquierda
//     for (int i = 0; i < 38; i++) {
//         digitalWrite(stepPin, HIGH);
//         delayMicroseconds(500);
//         digitalWrite(stepPin, LOW);
//         delayMicroseconds(500);
//     }
    
//     // Paso 3: Si steps = 0, terminamos aquí
//     if (steps == 0) {
//         Serial.println("Stepper en origen");
//         Serial.println("Stepper move complete.\n");
//         return;
//     }
    
//     // Paso 4: Movimiento a posición solicitada
//     bool movingLeft = (steps < 0);
//     digitalWrite(dirPin, movingLeft ? LOW : HIGH);
//     int stepCount = abs(steps);
    
//     Serial.print("Moviendo ");
//     Serial.print(stepCount);
//     Serial.println(movingLeft ? " pasos izquierda" : " pasos derecha");
    
//     for (int i = 0; i < stepCount; i++) {
//         // Seguridad para movimiento a derecha
//         if (!movingLeft && digitalRead(limitSwitchPin) == LOW) {
//             Serial.println("¡Alerta! Switch activado durante movimiento");
//             break;
//         }
        
//         digitalWrite(stepPin, HIGH);
//         delayMicroseconds(500);
//         digitalWrite(stepPin, LOW);
//         delayMicroseconds(500);
//     }
    
//     Serial.println("Stepper move complete.\n");
// }

void processCompletePacket(String packet) {
  if (packet.length() > 0) {
      // parsing accelerometer and gyroscope data from the string
      if (packet.startsWith("Accelerometer data:")) {
        if (sscanf(packet.c_str(), "Accelerometer data: X:%f Y:%f Z:%f", &ax, &ay, &az) == 3) {
          Serial.println("Accelerometer Data:");
          Serial.print("X: "); Serial.print(ax); Serial.print(" Y: "); Serial.print(ay); Serial.print(" Z: "); Serial.println(az);
        }
      } 
      else if (packet.startsWith("Gyroscope data:")) {
        if (sscanf(packet.c_str(), "Gyroscope data: X:%f Y:%f Z:%f", &gx, &gy, &gz) == 3) {
          Serial.println("Gyroscope Data:");
          Serial.print("X: "); Serial.print(gx); Serial.print(" Y: "); Serial.print(gy); Serial.print(" Z: "); Serial.println(gz);
        }
      }
      // Once both accelerometer and gyroscope data is available, call analyzeAccelerometerData
      if (ax != 0 && ay != 0 && az != 0 && gx != 0 && gy != 0 && gz != 0) {
        analyzeAccelerometerData(ax, ay, az, gx, gy, gz);
      }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Starting BLE Client for Xiao nRF52840 Sense...");
  pinMode(limitSwitchPin, INPUT_PULLUP);  // Assuming the limit switch is active low

  // DC motor
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(enAPin, OUTPUT);
  analogWrite(enAPin, pwmValue);

  pinMode(encoderPinA, INPUT_PULLUP);
  pinMode(encoderPinB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoderPinA), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinB), updateEncoder, CHANGE);

  // Stepper motor
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  BLEDevice::init("ESP32-BLE-Client");

  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setActiveScan(true);
  pBLEScan->setInterval(100);
  pBLEScan->setWindow(99);
  pBLEScan->start(5, false);
}

void loop() {
  static unsigned long lastPacketTime = 0;
  
  if (deviceFound) {
    if (!pClient || !pClient->isConnected()) {
      Serial.println("Attempting to connect to Xiao device...");
      
      pClient = BLEDevice::createClient();
      if (pClient->connect(myDevice)) {
        Serial.println("Connected to Xiao device!");

        BLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID);
        if (pRemoteService != nullptr) {
          pRemoteCharacteristic = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID_TX);
          if (pRemoteCharacteristic != nullptr && pRemoteCharacteristic->canNotify()) {
            pRemoteCharacteristic->registerForNotify([](BLERemoteCharacteristic* pBLERemoteCharacteristic, 
                                                      uint8_t* pData, 
                                                      size_t length, 
                                                      bool isNotify) {
              // Process received data
              for (int i = 0; i < length; i++) {
                char c = (char)pData[i];
                packetBuffer += c;
              }
              lastPacketTime = millis();
            });

            // Enable notifications
            uint8_t notificationOn[2] = {0x01, 0x00};
            pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue(notificationOn, 2, true);
            Serial.println("Enabled notifications, waiting for data...");
          }
        }
      }
    }

    // Check for complete packets
    if (packetBuffer.length() > 0) {
      // Look for complete lines
      int newlinePos = packetBuffer.indexOf('\n');  // Use '\n' as delimiter
      if (newlinePos >= 0) {
        String completePacket = packetBuffer.substring(0, newlinePos);  // Extract complete packet
        completePacket.trim();  // Remove any extra spaces/newlines
        processCompletePacket(completePacket);  // Process the full packet
        packetBuffer = packetBuffer.substring(newlinePos + 1);  // Remove the processed part of the buffer
      }
      // Timeout for incomplete data
      else if (millis() - lastPacketTime > PACKET_TIMEOUT && packetBuffer.length() > 0) {
        packetBuffer.trim();
        processCompletePacket(packetBuffer);  // Process remaining data
        packetBuffer = "";  // Reset buffer after processing
      }
    }
  } else {
    // Device not found, keep scanning
    if (millis() - lastPacketTime > 5000) {  // Rescan every 5 seconds if not connected
      BLEDevice::getScan()->start(5, false);
    }
  }

  delay(10);  // Small delay to prevent watchdog triggers
}