// #include <Servo.h>

// Servo servos[17];
// int servoPins[17] = {
//   2, 3, 4, 5, 6, 7, 8, 9, 10,
//   11, 12, 13, 22, 23, 24, 25, 26
// };

// // Human-readable labels for each servo
// enum ServoIndex {
//   HEAD = 0,
//   R_SHOULDER,
//   R_ELBOW,
//   R_WRIST,
//   L_SHOULDER,
//   L_ELBOW,
//   L_WRIST,
//   R_HIP,
//   R_KNEE,
//   R_ANKLE,
//   L_HIP,
//   L_KNEE,
//   L_ANKLE,
//   WAIST_ROTATE,
//   TORSO_TILT,
//   TORSO_BEND,
//   BODY_BALANCE
// };

// void setup() {
//   Serial.begin(115200);
//   for (int i = 0; i < 17; i++) {
//     servos[i].attach(servoPins[i]);
//     servos[i].write(90);  // Neutral position
//   }
//   delay(1000);
// }

// // Leg stepping motion
// void moveLegs() {
//   Serial.println("Moving legs for stepping...");
//   servos[R_HIP].write(110);
//   servos[L_HIP].write(70);
//   servos[R_KNEE].write(60);
//   servos[L_KNEE].write(120);
//   delay(600);

//   servos[R_HIP].write(70);
//   servos[L_HIP].write(110);
//   servos[R_KNEE].write(120);
//   servos[L_KNEE].write(60);
//   delay(600);
// }

// // Right arm wave
// void waveRightHand() {
//   Serial.println("Waving right hand...");
//   servos[R_SHOULDER].write(60);
//   delay(400);
//   for (int i = 0; i < 3; i++) {
//     servos[R_ELBOW].write(60);
//     delay(300);
//     servos[R_ELBOW].write(120);
//     delay(300);
//   }
//   servos[R_SHOULDER].write(90);
// }

// // Left arm wave
// void waveLeftHand() {
//   Serial.println("Waving left hand...");
//   servos[L_SHOULDER].write(120);
//   delay(400);
//   for (int i = 0; i < 3; i++) {
//     servos[L_ELBOW].write(120);
//     delay(300);
//     servos[L_ELBOW].write(60);
//     delay(300);
//   }
//   servos[L_SHOULDER].write(90);
// }

// // One-step walking simulation
// void walkStep() {
//   Serial.println("Taking a step...");
//   moveLegs();
//   delay(200);
// }

// //  Simulate right-hand handshake
// void shakeHands() {
//   Serial.println(" Simulating handshake...");
//   servos[R_SHOULDER].write(60);
//   servos[R_ELBOW].write(90);
//   delay(300);
//   for (int i = 0; i < 3; i++) {
//     servos[R_WRIST].write(60);
//     delay(200);
//     servos[R_WRIST].write(120);
//     delay(200);
//   }
//   servos[R_SHOULDER].write(90);
//   servos[R_WRIST].write(90);
// }

// void loop() {
//   waveRightHand();
//   delay(1000);
//   waveLeftHand();
//   delay(1000);
//   walkStep();
//   delay(1000);
//   shakeHands();
//   delay(2000);
// }
#include <Servo.h>

// Simulated capacitor variables
float capacitorVoltage = 0.0;      // Start at 0 volts
bool isCharging = true;            // Start in charging mode
const float maxVoltage = 5.0;
const float chargeStep = 0.05;
const float dischargeStep = 0.02;

Servo servos[17];
int servoPins[17] = {
  2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
  22, 23, 24, 25, 26
};

// Servo indices
enum {
  HEAD_PAN = 0,
  HEAD_TILT,
  R_SHOULDER,
  R_ELBOW,
  R_WRIST,
  L_SHOULDER,
  L_ELBOW,
  L_WRIST,
  R_HIP,
  R_KNEE,
  R_ANKLE,
  L_HIP,
  L_KNEE,
  L_ANKLE,
  TORSO_TWIST,
  TORSO_BEND,
  WAIST_ROT
};

// IR sensor and LED pins
const int irPin = A3;     // Use analog pin to avoid conflict with servo pins
const int ledPin = 13;

void setup() {
  Serial.begin(115200);

  // Attach servos
  for (int i = 0; i < 17; i++) {
    servos[i].attach(servoPins[i]);
    servos[i].write(90);
  }

  // IR sensor setup
  pinMode(irPin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);

  Serial.println("Ready. Send ID,ANGLE or keyword commands.");
  Serial.println("Starting capacitor simulation...");
}

void loop() {
  // IR sensor handling
  int irValue = digitalRead(irPin);
  if (irValue == LOW) {
    digitalWrite(ledPin, HIGH);
    shakeHands();  // Trigger a movement
    delay(1000);   // Avoid rapid repeat
  } else {
    digitalWrite(ledPin, LOW);
  }

  // Serial command handling
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();

    if (input.equalsIgnoreCase("MOVE_FORWARD")) {
      moveLegsForward();
      return;
    }
    if (input.equalsIgnoreCase("SHAKE_HANDS")) {
      shakeHands();
      return;
    }
    if (input.equalsIgnoreCase("WAVE_RIGHT")) {
      waveRightHand();
      return;
    }
    if (input.equalsIgnoreCase("WAVE_LEFT")) {
      waveLeftHand();
      return;
    }

    int commaIndex = input.indexOf(',');
    if (commaIndex < 1) {
      Serial.println("Invalid command.");
      return;
    }

    int id = input.substring(0, commaIndex).toInt();
    int angle = input.substring(commaIndex + 1).toInt();

    if (id < 0 || id >= 17 || angle < 0 || angle > 180) {
      Serial.println("Invalid servo ID or angle.");
      return;
    }

    servos[id].write(angle);
    Serial.print("Servo ");
    Serial.print(id);
    Serial.print(" moved to ");
    Serial.println(angle);
  }

  // Simulate capacitor charge/discharge
  if (isCharging) {
    capacitorVoltage += chargeStep;
    if (capacitorVoltage >= maxVoltage) {
      capacitorVoltage = maxVoltage;
      isCharging = false;
    }
  } else {
    capacitorVoltage -= dischargeStep;
    if (capacitorVoltage <= 0) {
      capacitorVoltage = 0;
      isCharging = true;
    }
  }

  Serial.print("Simulated Capacitor Voltage: ");
  Serial.println(capacitorVoltage, 2);

  delay(100);  // Adjust speed of charging/discharging here
}

// === Movements ===

void moveLegsForward() {
  servos[R_HIP].write(110);
  servos[L_HIP].write(70);
  servos[R_KNEE].write(60);
  servos[L_KNEE].write(120);
  delay(500);

  servos[R_HIP].write(70);
  servos[L_HIP].write(110);
  servos[R_KNEE].write(120);
  servos[L_KNEE].write(60);
  delay(500);

  servos[R_HIP].write(90);
  servos[L_HIP].write(90);
  servos[R_KNEE].write(90);
  servos[L_KNEE].write(90);
  delay(200);
}

void shakeHands() {
  servos[R_SHOULDER].write(60);
  servos[R_ELBOW].write(90);
  delay(300);

  for (int i = 0; i < 3; i++) {
    servos[R_WRIST].write(60);
    delay(300);
    servos[R_WRIST].write(120);
    delay(300);
  }

  servos[R_SHOULDER].write(90);
  servos[R_ELBOW].write(90);
  servos[R_WRIST].write(90);
}

void waveRightHand() {
  servos[R_SHOULDER].write(60);
  delay(300);
  for (int i = 0; i < 3; i++) {
    servos[R_ELBOW].write(60);
    delay(300);
    servos[R_ELBOW].write(120);
    delay(300);
  }
  servos[R_SHOULDER].write(90);
  servos[R_ELBOW].write(90);
}

void waveLeftHand() {
  servos[L_SHOULDER].write(120);
  delay(300);
  for (int i = 0; i < 3; i++) {
    servos[L_ELBOW].write(120);
    delay(300);
    servos[L_ELBOW].write(60);
    delay(300);
  }
  servos[L_SHOULDER].write(90);
  servos[L_ELBOW].write(90);
}