#include <Arduino.h>
#include <ESP32Servo.h>

// ==== CONFIGURATION ==== //
// Change these for your wiring/setup
#define SERVO_PIN_1 17     // Pin for servo 1
#define SERVO_PIN_2 16     // Pin for servo 2
#define JOYSTICK_X_PIN 4  // Simulated analog input from BLE (replace with actual BLE later)
#define JOYSTICK_Y_PIN 0

// ==== SERVO SETUP ==== //
Servo servo1;
Servo servo2;

// Range of analog input
const int analogMin = 0;
const int analogMax = 4095;

// Servo angle range (used as proxy for speed display)
const int servoMinAngle = 90;  // Center (no movement)
const int servoMaxAngle = 0;   // Full reverse
const int servoMinAngle2 = 180; // Full forward

// ==== BLE INCLUDES AND SETUP (COMMENTED OUT) ==== //
/*
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

// Custom BLE Service and Characteristic UUIDs
#define SERVICE_UUID        "12345678-1234-1234-1234-123456789abc"
#define CHARACTERISTIC_UUID "abcd1234-5678-90ab-cdef-123456789abc"

BLECharacteristic *pCharacteristic;
bool deviceConnected = false;

// Callback for BLE connection state
class MyServerCallbacks: public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {
    deviceConnected = true;
  }
  void onDisconnect(BLEServer* pServer) {
    deviceConnected = false;
  }
};

// Start BLE server and characteristic
void setupBLE() {
  BLEDevice::init("FloatingHouse_Server");
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService *pService = pServer->createService(SERVICE_UUID);

  pCharacteristic = pService->createCharacteristic(
                     CHARACTERISTIC_UUID,
                     BLECharacteristic::PROPERTY_WRITE
                   );

  pService->start();
  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->start();
}
*/

// ==== SETUP ==== //
void setup() {
  Serial.begin(115200);

  // Attach servos to pins
  servo1.attach(SERVO_PIN_1);
  servo2.attach(SERVO_PIN_2);

  // Move to neutral positions
  servo1.write(90);
  servo2.write(90);

  // Start BLE if needed
  // setupBLE();
}

// ==== MAP JOYSTICK TO SERVO ==== //
// This function maps joystick input (0–4095) to a servo angle
// that reflects the desired speed (centered at 90 = no movement)
int computeServoAngle(int analogValue, bool reverse = false) {
  int center = (analogMax + analogMin) / 2;
  int deadzone = 0; // Don't react to small jitters
  int angle;

  if (abs(analogValue - center) < deadzone) {
    angle = 90;  // Neutral position
  } else if (analogValue < center) {
    angle = map(analogValue, analogMin, center - deadzone, servoMaxAngle, 90);
  } else {
    angle = map(analogValue, center + deadzone, analogMax, 90, servoMinAngle2);
  }

  if (reverse) angle = 180 - angle;
  return constrain(angle, 0, 180);
}

// ==== MAIN LOOP ==== //
void loop() {
  // Simulate joystick values via analogRead (replace this with BLE values later)
  int joyX = analogRead(JOYSTICK_X_PIN); // Left/right
  int joyY = analogRead(JOYSTICK_Y_PIN); // Up/down

  // Compute servo positions (for now this just sets angle, but in the real system,
  // it would be used to set speed of continuous rotation servos)
  int angle1 = computeServoAngle(joyY);
  int angle2 = computeServoAngle(joyX);

  // Set servos
  servo1.write(angle1);
  servo2.write(angle2);

  // Debug output
  Serial.print("Joystick X: "); Serial.print(joyX);
  Serial.print(" | Angle2: "); Serial.print(angle2);
  Serial.print(" | Joystick Y: "); Serial.print(joyY);
  Serial.print(" | Angle1: "); Serial.println(angle1);

  delay(50);
}

/*
  === BLE RECEIVE HANDLER (Placeholder) ===

  Once BLE is enabled, you'll read joystick positions as a string or byte array.
  Format could be: "512,1023" for X and Y axes.
  In loop(), split that string, convert to ints, then call computeServoAngle().

  Example:
    String data = pCharacteristic->getValue(); // "1234,2048"
    int xVal = getFirstValue(data);
    int yVal = getSecondValue(data);
    int angle1 = computeServoAngle(yVal);
    int angle2 = computeServoAngle(xVal);
    servo1.write(angle1);
    servo2.write(angle2);
*/