#include <ESP32Servo.h>
#include "BluetoothSerial.h"

#define HALL_PIN 34
#define ESC1_PIN 18
#define ESC2_PIN 19

Servo esc1;
Servo esc2;
BluetoothSerial SerialBT;

unsigned long lastSerialTime = 0;                 // last time we received data
const unsigned long serialTimeoutMs = 2000;       // 2 seconds safety timeout

// Hall sensor mapping (sent to Unity)
int hallOutMin = 0;
int hallOutMax = 511;

// Hall raw ADC expected range (adjust if needed)
int hallRawMin = 2850;
int hallRawMax = 4095;

// ESC PWM range
int escMin = 1499;
int escMax = 2000;

// Ramping / smoothing settings
const int loopDelayMs = 20;   // update interval (20 ms → ~50 Hz)
const int rampStepUs = 40;    // microseconds change per update

// Current & target PWM values
int currentPwm1, currentPwm2;
int targetPwm1, targetPwm2;

// Helper: safer remap returning float with optional inversion
float remapf(float x, float in_min, float in_max, float out_min, float out_max, bool invert=false) {
    if (in_max == in_min) return out_min;
    float result = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    if (invert) result = out_max - (result - out_min); // invert mapping
    return result;
}

void setup() {
    Serial.begin(115200);
    SerialBT.begin("ESP32_ESC");   // Bluetooth name
    Serial.println("System Ready! Connect via Bluetooth: ESP32_ESC");

    pinMode(HALL_PIN, INPUT);

    // Attach ESCs
    esc1.attach(ESC1_PIN, escMin, escMax);
    esc2.attach(ESC2_PIN, escMin, escMax);

    // Initialize PWM variables to idle
    currentPwm1 = currentPwm2 = escMin;
    targetPwm1 = targetPwm2 = escMin;

    // Arm ESCs (send idle pulse)
    esc1.writeMicroseconds(escMin);
    esc2.writeMicroseconds(escMin);
    delay(3000);

    lastSerialTime = millis();  // avoid immediate timeout
}

void applyRampAndWrite() {
    // Ramp motor1
    if (currentPwm1 != targetPwm1) {
        int diff = targetPwm1 - currentPwm1;
        if (abs(diff) <= rampStepUs) currentPwm1 = targetPwm1;
        else currentPwm1 += (diff > 0) ? rampStepUs : -rampStepUs;
        esc1.writeMicroseconds(currentPwm1);
    }

    // Ramp motor2
    if (currentPwm2 != targetPwm2) {
        int diff = targetPwm2 - currentPwm2;
        if (abs(diff) <= rampStepUs) currentPwm2 = targetPwm2;
        else currentPwm2 += (diff > 0) ? rampStepUs : -rampStepUs;
        esc2.writeMicroseconds(currentPwm2);
    }
}

void loop() {
    // --- Read Hall sensor and send mapped value to Unity ---
    int hallValue = analogRead(HALL_PIN);

    // Invert mapping for mechanical setup
    float mappedFloat = remapf((float)hallValue, (float)hallRawMin, (float)hallRawMax,
                               (float)hallOutMin, (float)hallOutMax, true);
    int mappedInput = constrain((int)round(mappedFloat), hallOutMin, hallOutMax);

    // Send data over Bluetooth
    SerialBT.print("IN:");
    SerialBT.println(mappedInput);

    // Debug on USB
    Serial.print("Hall Raw: ");
    Serial.print(hallValue);
    Serial.print(" -> IN:");
    Serial.println(mappedInput);

    // --- Check for incoming Bluetooth data ---
    if (SerialBT.available()) {
        char buffer[64];
        int len = SerialBT.readBytesUntil('\n', buffer, sizeof(buffer) - 1);
        buffer[len] = '\0';
        String s = String(buffer);
        s.trim();

        lastSerialTime = millis(); // update when data received

        Serial.println("BT Received: " + s);

        // If input contains a comma -> hall mapping update: "min,max"
        if (s.indexOf(',') != -1) {
            int commaIdx = s.indexOf(',');
            String a = s.substring(0, commaIdx);
            String b = s.substring(commaIdx + 1);
            a.trim(); b.trim();
            int newMin = a.toInt();
            int newMax = b.toInt();

            if (newMin >= 0 && newMax >= newMin) {
                hallOutMin = newMin;
                hallOutMax = newMax;
                SerialBT.print("New Hall range (OUT): ");
                SerialBT.print(hallOutMin);
                SerialBT.print(" -> ");
                SerialBT.println(hallOutMax);
            } else {
                SerialBT.println("Invalid hall range. Use 'min,max'.");
            }
        } else {
            // Parse motor feedback values
            for (unsigned int i = 0; i < s.length(); ++i) if (s[i] == ',') s[i] = ' ';
            int feedback1 = -1, feedback2 = -1;
            int firstSpace = s.indexOf(' ');
            if (firstSpace == -1) {
                feedback1 = s.toInt();
                feedback2 = feedback1;
            } else {
                String t1 = s.substring(0, firstSpace);
                String t2 = s.substring(firstSpace + 1);
                t1.trim(); t2.trim();
                feedback1 = t1.toInt();
                feedback2 = t2.toInt();
            }

            // Clamp incoming feedback
            feedback1 = constrain(feedback1, 0, hallOutMax);
            feedback2 = constrain(feedback2, 0, hallOutMax);

            SerialBT.print("Unity Feedback -> Motor1: ");
            SerialBT.print(feedback1);
            SerialBT.print(" | Motor2: ");
            SerialBT.println(feedback2);

            // Map feedback (0..255) to ESC PWM range
            int newTarget1 = map(feedback1, 0, hallOutMax, escMin, escMax);
            int newTarget2 = map(feedback2, 0, hallOutMax, escMin, escMax);

            newTarget1 = constrain(newTarget1, escMin, escMax);
            newTarget2 = constrain(newTarget2, escMin, escMax);

            targetPwm1 = newTarget1;
            targetPwm2 = newTarget2;

            Serial.print("TARGET -> PWM1: ");
            Serial.print(targetPwm1);
            Serial.print(" | PWM2: ");
            Serial.println(targetPwm2);
        }
    }

    // --- Timeout safety check ---
    if (millis() - lastSerialTime > serialTimeoutMs) {
        if (targetPwm1 != escMin || targetPwm2 != escMin) {
            Serial.println("Serial timeout - motors idle.");
            SerialBT.println("Timeout - motors idle.");
        }
        targetPwm1 = escMin;
        targetPwm2 = escMin;
    }

    // Apply ramping & write to ESCs
    applyRampAndWrite();

    delay(loopDelayMs);
}