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

// I2C — shared by both IMUs
#define SDA_PIN  21
#define SCL_PIN  22

// Servos
#define SERVO_WRIST_PIN   18
#define SERVO_THUMB_PIN   19
#define SERVO_FINGER_PIN  15

// Potentiometers — input-only ADC pins
// [0]=wrist  [1]=thumb  [2]=finger  [3..5]=reserved
const int POT_PINS[6] = {34, 35, 32, 33, 25, 26};

#define WRIST_MIN   10
#define WRIST_MAX   170
#define THUMB_MIN   10
#define THUMB_MAX   170
#define FINGER_MIN  10
#define FINGER_MAX  170

Adafruit_MPU6050 imu_dorsal;
Adafruit_MPU6050 imu_forearm;

Servo servoWrist;
Servo servoThumb;
Servo servoFinger;

struct OrthosisState {
  int   potRaw[6];

  int   wristAngle;
  int   thumbAngle;
  int   fingerAngle;

  float dorsalAccX, dorsalAccY, dorsalAccZ;
  float dorsalGyrX, dorsalGyrY, dorsalGyrZ;

  float forearmAccX, forearmAccY, forearmAccZ;
};

OrthosisState state = {};

bool imu1OK = false;
bool imu2OK = false;

void setup() {
  Serial.begin(115200);
  delay(300);
  Serial.println("\n=== Hand Orthosis — Wokwi Simulation ===");

  Wire.begin(SDA_PIN, SCL_PIN);

  imu1OK = imu_dorsal.begin(0x68);
  Serial.println(imu1OK
    ? "[IMU] Dorsal  (0x68) OK"
    : "[IMU] Dorsal  (0x68) NOT FOUND — check SDA/SCL wiring");

  imu2OK = imu_forearm.begin(0x69);
  Serial.println(imu2OK
    ? "[IMU] Forearm (0x69) OK"
    : "[IMU] Forearm (0x69) NOT FOUND — set ad0:1 in diagram.json");

  if (imu1OK) {
    imu_dorsal.setAccelerometerRange(MPU6050_RANGE_4_G);
    imu_dorsal.setGyroRange(MPU6050_RANGE_500_DEG);
    imu_dorsal.setFilterBandwidth(MPU6050_BAND_21_HZ);
  }

  if (imu2OK) {
    imu_forearm.setAccelerometerRange(MPU6050_RANGE_4_G);
    imu_forearm.setGyroRange(MPU6050_RANGE_500_DEG);
    imu_forearm.setFilterBandwidth(MPU6050_BAND_21_HZ);
  }

  servoWrist.attach(SERVO_WRIST_PIN);
  servoThumb.attach(SERVO_THUMB_PIN);
  servoFinger.attach(SERVO_FINGER_PIN);
  servoWrist.write(90);
  servoThumb.write(90);
  servoFinger.write(90);
  Serial.println("[SERVO] Wrist + Thumb + Finger → 90° (neutral)");

  Serial.println("[INIT] Done.\n");
}

void loop() {
  readPots();
  readIMUs();
  computeTargets();
  driveActuators();
  printState();

  delay(20);
}

void readPots() {
  for (int i = 0; i < 6; i++) {
    state.potRaw[i] = analogRead(POT_PINS[i]);
  }
}

void readIMUs() {
  sensors_event_t a, g, t;

  if (imu1OK) {
    imu_dorsal.getEvent(&a, &g, &t);
    state.dorsalAccX = a.acceleration.x;
    state.dorsalAccY = a.acceleration.y;
    state.dorsalAccZ = a.acceleration.z;
    state.dorsalGyrX = g.gyro.x;
    state.dorsalGyrY = g.gyro.y;
    state.dorsalGyrZ = g.gyro.z;
  }

  if (imu2OK) {
    imu_forearm.getEvent(&a, &g, &t);
    state.forearmAccX = a.acceleration.x;
    state.forearmAccY = a.acceleration.y;
    state.forearmAccZ = a.acceleration.z;
  }
}

void computeTargets() {
  state.wristAngle = constrain(
    map(state.potRaw[0], 0, 4095, WRIST_MIN, WRIST_MAX),
    WRIST_MIN, WRIST_MAX
  );
  state.thumbAngle = constrain(
    map(state.potRaw[1], 0, 4095, THUMB_MIN, THUMB_MAX),
    THUMB_MIN, THUMB_MAX
  );
  state.fingerAngle = constrain(
    map(state.potRaw[2], 0, 4095, FINGER_MIN, FINGER_MAX),
    FINGER_MIN, FINGER_MAX
  );
}

void driveActuators() {
  servoWrist.write(state.wristAngle);
  servoThumb.write(state.thumbAngle);
  servoFinger.write(state.fingerAngle);
}

void printState() {
  Serial.printf(
    "W:%3d° Th:%3d° Fi:%3d° | D_A[%5.2f %5.2f %5.2f] D_G[%5.2f %5.2f] | F_A[%5.2f %5.2f %5.2f]\n",
    state.wristAngle,
    state.thumbAngle,
    state.fingerAngle,
    state.dorsalAccX, state.dorsalAccY, state.dorsalAccZ,
    state.dorsalGyrX, state.dorsalGyrY,
    state.forearmAccX, state.forearmAccY, state.forearmAccZ
  );
}