/*
 * ENHANCED HUMANOID ROBOT CONTROL SYSTEM V2.0 (Patched)
 * Professional-Grade Street-Walking Robot with Advanced AI
 *
 * Major Improvements/Fixes in this version:
 * - Split vision sensors into analog + digital (pins 34/35 are digital on Mega)
 * - Inverse kinematics now outputs 6 angles (toe defined) and clamps asin input
 * - PID now uses real loop dt via set_dt()
 * - Float-safe math (fabsf instead of abs for floats)
 * - Guard against zero/negative dt
 *
 * Target board: Arduino Mega 2560 (48 servos supported by Servo library)
 */

#include <Servo.h>
#include <math.h>

// ============================================================================
// ENHANCED SYSTEM CONFIGURATION
// ============================================================================

#define NUM_JOINTS 24
#define CONTROL_FREQUENCY 100
#define SERIAL_BAUD_RATE 115200
#define MAX_JOINT_VELOCITY 180.0  // deg/s
#define MAX_JOINT_ACCELERATION 360.0  // deg/s²

// Joint limits (degrees)
const float JOINT_MIN[NUM_JOINTS] = {
  0, 0, 0, 0, 0, 0,     // Left arm
  0, 0, 0, 0, 0, 0,     // Right arm
  -45, -20, -135, -30, -45, -20,  // Left leg (hip pitch, roll, knee, ankle pitch, roll, toe)
  -45, -20, -135, -30, -45, -20   // Right leg
};

const float JOINT_MAX[NUM_JOINTS] = {
  180, 180, 180, 180, 180, 180,  // Left arm
  180, 180, 180, 180, 180, 180,  // Right arm
  45, 20, 0, 30, 45, 20,          // Left leg
  45, 20, 0, 30, 45, 20           // Right leg
};

// Pin assignments
int servo_pins[NUM_JOINTS] = {
  2, 3, 4, 5, 6, 7,      // Left arm
  8, 9, 10, 11, 12, 13,  // Right arm
  22, 23, 24, 25, 26, 27, // Left leg
  28, 29, 30, 31, 32, 33  // Right leg
};

int imu_pins[2] = {A0, A1};
int force_sensors[4] = {A2, A3, A4, A5};
int proximity_sensors[8] = {A6, A7, A8, A9, A10, A11, A12, A13};

// Vision: split into analog + digital (34/35 are digital-only on Mega)
int vision_analog[2]  = {A14, A15}; // [0]=traffic, [1]=pedestrian density
int vision_digital[2] = {34, 35};   // [0]=sidewalk_detected, [1]=crosswalk_detected

// ============================================================================
// ROBOT STATE MACHINE
// ============================================================================

enum RobotState {
  STATE_IDLE,
  STATE_STANDING,
  STATE_WALKING,
  STATE_TURNING,
  STATE_STOPPING,
  STATE_EMERGENCY,
  STATE_RECOVERING
};

enum GaitPhase {
  PHASE_DOUBLE_SUPPORT,
  PHASE_LEFT_SWING,
  PHASE_RIGHT_SWING,
  PHASE_TRANSITION
};

// ============================================================================
// ADVANCED DATA STRUCTURES
// ============================================================================

struct Vector3 {
  float x, y, z;

  Vector3(float x = 0, float y = 0, float z = 0) : x(x), y(y), z(z) {}

  float magnitude() const {
    return sqrtf(x*x + y*y + z*z);
  }

  Vector3 normalized() const {
    float mag = magnitude();
    return mag > 0.001f ? Vector3(x/mag, y/mag, z/mag) : Vector3(0, 0, 0);
  }

  Vector3 operator+(const Vector3& v) const { return Vector3(x+v.x, y+v.y, z+v.z); }
  Vector3 operator-(const Vector3& v) const { return Vector3(x-v.x, y-v.y, z-v.z); }
  Vector3 operator*(float s) const { return Vector3(x*s, y*s, z*s); }
};

struct JointState {
  float position;
  float velocity;
  float acceleration;
  float target;
  float error;
  bool fault;

  JointState() : position(90), velocity(0), acceleration(0), target(90), error(0), fault(false) {}
};

struct FootContact {
  bool left_heel, left_toe;
  bool right_heel, right_toe;
  float left_force, right_force;

  FootContact() : left_heel(true), left_toe(true), right_heel(true), right_toe(true),
                  left_force(0), right_force(0) {}
};

struct EnvironmentState {
  float traffic_light;      // 0=red, 1=yellow, 2=green
  float pedestrian_density;
  float terrain_slope;
  float terrain_roughness;
  float obstacle_distances[8];
  bool sidewalk_detected;
  bool crosswalk_detected;
  Vector3 wind_velocity;

  EnvironmentState() : traffic_light(2), pedestrian_density(0), terrain_slope(0),
                      terrain_roughness(0), sidewalk_detected(true), crosswalk_detected(false) {
    for (int i = 0; i < 8; i++) obstacle_distances[i] = 10.0f;
  }
};

// ============================================================================
// GLOBAL STATE
// ============================================================================

Servo joint_servos[NUM_JOINTS];
JointState joint_states[NUM_JOINTS];
RobotState current_state = STATE_IDLE;
GaitPhase current_gait_phase = PHASE_DOUBLE_SUPPORT;

Vector3 center_of_mass(0, 0, 0.8f);
Vector3 com_velocity(0, 0, 0);
Vector3 orientation(0, 0, 0);  // roll, pitch, yaw
FootContact foot_contact;
EnvironmentState environment;

float gait_cycle = 0.0f;
float step_length = 0.35f;      // meters
float step_height = 0.06f;      // meters
float step_time = 0.6f;         // seconds per step
float walking_speed = 0.58f;    // m/s (human average)

bool emergency_stop = false;
unsigned long state_change_time = 0;

// ============================================================================
// ENHANCED PID CONTROLLER
// ============================================================================

class EnhancedPID {
private:
  float kp, ki, kd, kf;  // kf = feedforward gain
  float integral, prev_error;
  float integral_limit;
  float output_min, output_max;
  float dt;

public:
  EnhancedPID(float p = 3.0, float i = 0.2, float d = 0.08, float f = 0.5) {
    kp = p; ki = i; kd = d; kf = f;
    integral = 0; prev_error = 0;
    integral_limit = 50.0f;
    output_min = -100; output_max = 100;
    dt = 0.01f;
  }

  float compute(float setpoint, float current, float velocity = 0, float feedforward = 0) {
    float error = setpoint - current;
    integral += error * dt;

    // Anti-windup with clamping
    if (integral >  integral_limit) integral =  integral_limit;
    if (integral < -integral_limit) integral = -integral_limit;

    float derivative = (error - prev_error) / dt;

    // PID + feedforward
    float output = kp * error + ki * integral + kd * derivative + kf * feedforward;

    if (output > output_max) output = output_max;
    if (output < output_min) output = output_min;

    prev_error = error;

    return output;
  }

  void reset() {
    integral = 0;
    prev_error = 0;
  }

  void set_limits(float min_out, float max_out) {
    output_min = min_out;
    output_max = max_out;
  }

  void set_dt(float new_dt) {
    if (new_dt > 0.0001f) dt = new_dt;
  }
};

EnhancedPID joint_controllers[NUM_JOINTS];

// ============================================================================
// INVERSE KINEMATICS ENGINE
// ============================================================================

class InverseKinematics {
private:
  static constexpr float UPPER_LEG = 0.42f;  // meters
  static constexpr float LOWER_LEG = 0.42f;
  static constexpr float FOOT_HEIGHT = 0.08f;

public:
  // Solve for leg joint angles given foot position
  // joint_angles: [0]=hip pitch, [1]=hip roll, [2]=knee, [3]=ankle pitch, [4]=ankle roll, [5]=toe
  bool solve_leg_ik(Vector3 foot_pos, int leg_base, float* joint_angles) {
    // Distance from hip to foot
    float dx = foot_pos.x;
    float dy = foot_pos.y;
    float dz = foot_pos.z;
    float dist = sqrtf(dx*dx + dy*dy + dz*dz);

    // Check if reachable
    if (dist > UPPER_LEG + LOWER_LEG || dist < fabsf(UPPER_LEG - LOWER_LEG)) {
      return false;
    }

    // Hip roll (side to side)
    joint_angles[1] = atan2f(dy, dz) * 180.0f / PI;

    // Knee angle using law of cosines
    float cos_knee = (UPPER_LEG*UPPER_LEG + LOWER_LEG*LOWER_LEG - dist*dist) /
                     (2.0f * UPPER_LEG * LOWER_LEG);
    if (cos_knee >  1.0f) cos_knee =  1.0f;
    if (cos_knee < -1.0f) cos_knee = -1.0f;
    joint_angles[2] = -(180.0f - acosf(cos_knee) * 180.0f / PI);  // Negative for knee bend

    // Hip pitch
    float alpha = atan2f(dx, sqrtf(dy*dy + dz*dz));
    float knee_rad = fabsf(joint_angles[2]) * PI / 180.0f;
    float s = (LOWER_LEG * sinf(knee_rad)) / dist;
    if (s >  1.0f) s =  1.0f;
    if (s < -1.0f) s = -1.0f;
    float beta = asinf(s);
    joint_angles[0] = (alpha - beta) * 180.0f / PI;

    // Ankle pitch (keep foot level)
    joint_angles[3] = -(joint_angles[0] + joint_angles[2]);

    // Ankle roll (keep foot level)
    joint_angles[4] = -joint_angles[1];

    // Toe (neutral for now; adjust if you add toe kinematics)
    joint_angles[5] = 0.0f;

    return true;
  }

  // Forward kinematics for verification
  Vector3 forward_leg_fk(int leg_base, float* joint_angles) {
    float hip_pitch = joint_angles[0] * PI / 180.0f;
    float hip_roll  = joint_angles[1] * PI / 180.0f;
    float knee      = joint_angles[2] * PI / 180.0f;

    Vector3 pos;
    pos.x = UPPER_LEG * sinf(hip_pitch) + LOWER_LEG * sinf(hip_pitch + knee);
    pos.y = UPPER_LEG * sinf(hip_roll);
    pos.z = -(UPPER_LEG * cosf(hip_pitch) + LOWER_LEG * cosf(hip_pitch + knee));

    return pos;
  }
};

InverseKinematics ik_solver;

// ============================================================================
/* TRAJECTORY GENERATOR WITH SMOOTH INTERPOLATION */
// ============================================================================

class TrajectoryGenerator {
private:
  float cubic_interpolate(float t, float p0, float p1, float v0, float v1) {
    // Cubic Hermite spline
    float t2 = t * t;
    float t3 = t2 * t;

    float h00 = 2*t3 - 3*t2 + 1;
    float h10 = t3 - 2*t2 + t;
    float h01 = -2*t3 + 3*t2;
    float h11 = t3 - t2;

    return h00 * p0 + h10 * v0 + h01 * p1 + h11 * v1;
  }

public:
  Vector3 generate_foot_trajectory(float phase, bool is_swing_foot) {
    Vector3 pos;

    if (!is_swing_foot) {
      // Stance foot (on ground)
      pos.x = -step_length * 0.5f;
      pos.y = 0;
      pos.z = -0.8f;  // Ground level
    } else {
      // Swing foot (in air)
      float t = phase;

      // Forward movement
      pos.x = cubic_interpolate(t, -step_length*0.5f, step_length*0.5f, 0, 0);

      // Vertical movement (parabolic)
      float height_factor = 4.0f * t * (1.0f - t);  // Peaks at t=0.5
      pos.z = -0.8f + step_height * height_factor;

      // Lateral stability
      pos.y = 0;
    }

    return pos;
  }

  float smooth_start_stop(float t, float duration = 0.2f) {
    // Smooth acceleration/deceleration using cosine ease
    if (t < duration) {
      return 0.5f * (1.0f - cosf(PI * t / duration));
    } else if (t > 1.0f - duration) {
      return 0.5f * (1.0f + cosf(PI * (t - (1.0f - duration)) / duration));
    }
    return 1.0f;
  }
};

TrajectoryGenerator trajectory_gen;

// ============================================================================
// BALANCE CONTROLLER WITH ZMP
// ============================================================================

class ZMPBalanceController {
private:
  Vector3 zmp_target;
  Vector3 zmp_current;
  float stability_margin = 0.03f;  // 3cm margin

public:
  bool check_stability() {
    compute_zmp();

    // Check if ZMP is within support polygon (simple bounds)
    return (fabsf(zmp_current.x) < 0.1f && fabsf(zmp_current.y) < 0.08f);
  }

  void compute_zmp() {
    // Simplified ZMP calculation
    float g = 9.81f;
    zmp_current.x = center_of_mass.x - (center_of_mass.z / g) * com_velocity.x;
    zmp_current.y = center_of_mass.y - (center_of_mass.z / g) * com_velocity.y;
  }

  Vector3 compute_balance_correction() {
    Vector3 correction(0, 0, 0);

    if (!check_stability()) {
      // Apply corrective torques (placeholder proportional gains)
      correction.x = -(zmp_current.x - zmp_target.x) * 5.0f;
      correction.y = -(zmp_current.y - zmp_target.y) * 5.0f;
    }

    return correction;
  }

  void update_com_estimate() {
    // Update center of mass based on joint positions (placeholder)
    center_of_mass.x = 0;
    center_of_mass.y = 0;
    center_of_mass.z = 0.8f;

    // Add velocity estimate from foot contacts (simple damping)
    if (foot_contact.left_force > 0 && foot_contact.right_force > 0) {
      com_velocity.x *= 0.95f;  // Damping
      com_velocity.y *= 0.95f;
    }
  }
};

ZMPBalanceController balance_controller;

// ============================================================================
// SENSOR FUSION MODULE
// ============================================================================

class SensorFusion {
private:
  float alpha_filter = 0.8f;  // Low-pass filter coefficient

public:
  void update_imu_data() {
    // Read IMU (simulated)
    float raw_pitch = (analogRead(imu_pins[0]) - 512) / 512.0f * 45.0f;
    float raw_roll  = (analogRead(imu_pins[1]) - 512) / 512.0f * 45.0f;

    // Apply complementary filter
    orientation.x = alpha_filter * orientation.x + (1.0f - alpha_filter) * raw_roll;
    orientation.y = alpha_filter * orientation.y + (1.0f - alpha_filter) * raw_pitch;
  }

  void update_force_sensors() {
    // Read foot pressure sensors
    int lh = analogRead(force_sensors[0]);
    int lt = analogRead(force_sensors[1]);
    int rh = analogRead(force_sensors[2]);
    int rt = analogRead(force_sensors[3]);

    foot_contact.left_heel  = (lh > 100);
    foot_contact.left_toe   = (lt > 100);
    foot_contact.right_heel = (rh > 100);
    foot_contact.right_toe  = (rt > 100);

    foot_contact.left_force  = (lh + lt) / 2046.0f;
    foot_contact.right_force = (rh + rt) / 2046.0f;
  }

  void update_proximity() {
    for (int i = 0; i < 8; i++) {
      int raw = analogRead(proximity_sensors[i]);
      float distance = (1023 - raw) / 1023.0f * 5.0f;  // 0-5 meters
      environment.obstacle_distances[i] = distance;
    }
  }

  void update_vision() {
    // Traffic light via analog (0..1023)
    int traffic = analogRead(vision_analog[0]);
    if (traffic > 800)      environment.traffic_light = 2;  // Green
    else if (traffic > 400) environment.traffic_light = 1;  // Yellow
    else                    environment.traffic_light = 0;  // Red

    // Sidewalk / crosswalk via digital
    environment.sidewalk_detected  = (digitalRead(vision_digital[0]) == HIGH);
    environment.crosswalk_detected = (digitalRead(vision_digital[1]) == HIGH);

    // Pedestrian density via analog
    float ped_sensor = analogRead(vision_analog[1]) / 1023.0f;
    environment.pedestrian_density = ped_sensor * 0.8f;
  }

  void update_all() {
    update_imu_data();
    update_force_sensors();
    update_proximity();
    update_vision();
  }
};

SensorFusion sensor_fusion;

// ============================================================================
// INTELLIGENT DECISION MAKING
// ============================================================================

class DecisionMaker {
public:
  RobotState evaluate_state_transition() {
    // Emergency conditions
    if (emergency_stop) return STATE_EMERGENCY;

    float min_obstacle = 10.0f;
    for (int i = 0; i < 8; i++) {
      if (environment.obstacle_distances[i] < min_obstacle) {
        min_obstacle = environment.obstacle_distances[i];
      }
    }

    // Critical obstacle
    if (min_obstacle < 0.5f) return STATE_EMERGENCY;

    // Traffic light compliance
    if (environment.crosswalk_detected && environment.traffic_light == 0) {
      return STATE_STOPPING;
    }

    // High pedestrian density
    if (environment.pedestrian_density > 0.75f) {
      return STATE_STOPPING;
    }

    // Normal operation
    if (current_state == STATE_IDLE || current_state == STATE_STANDING) {
      return STATE_WALKING;
    }

    return current_state;
  }

  bool should_adjust_gait() {
    // Adjust gait for terrain
    return (environment.terrain_roughness > 0.6f);
  }

  float compute_safe_speed() {
    float base_speed = walking_speed;

    // Slow down for obstacles
    float min_dist = 10.0f;
    for (int i = 0; i < 8; i++) {
      if (environment.obstacle_distances[i] < min_dist) {
        min_dist = environment.obstacle_distances[i];
      }
    }
    if (min_dist < 2.0f) base_speed *= (min_dist / 2.0f);

    // Slow down for pedestrians
    if (environment.pedestrian_density > 0.5f) {
      base_speed *= (1.0f - environment.pedestrian_density * 0.5f);
    }

    // Slow down on rough terrain
    if (environment.terrain_roughness > 0.5f) {
      base_speed *= 0.7f;
    }

    if (base_speed < 0.1f) base_speed = 0.1f;
    return base_speed;
  }
};

DecisionMaker decision_maker;

// ============================================================================
// GAIT CONTROLLER
// ============================================================================

class GaitController {
private:
  float phase_left = 0.0f;
  float phase_right = 0.5f;  // 180° out of phase

public:
  void update_gait(float dt) {
    if (current_state != STATE_WALKING) return;

    float cycle_speed = decision_maker.compute_safe_speed() / step_length; // cycles per second
    gait_cycle += dt * cycle_speed;

    if (gait_cycle > 1.0f) {
      gait_cycle -= 1.0f;
    }

    phase_left  = gait_cycle;
    phase_right = fmod(gait_cycle + 0.5f, 1.0f);

    // Determine gait phase
    if (gait_cycle < 0.1f || gait_cycle > 0.9f) {
      current_gait_phase = PHASE_DOUBLE_SUPPORT;
    } else if (gait_cycle < 0.5f) {
      current_gait_phase = PHASE_RIGHT_SWING;
    } else {
      current_gait_phase = PHASE_LEFT_SWING;
    }
  }

  void generate_leg_targets() {
    // Left leg
    bool left_swing = (current_gait_phase == PHASE_LEFT_SWING);
    Vector3 left_foot = trajectory_gen.generate_foot_trajectory(phase_left, left_swing);

    float left_angles[6];
    if (ik_solver.solve_leg_ik(left_foot, 12, left_angles)) {
      for (int i = 0; i < 6; i++) {
        joint_states[12 + i].target = constrain(left_angles[i] + 90.0f,
                                                JOINT_MIN[12 + i],
                                                JOINT_MAX[12 + i]);
      }
    }

    // Right leg
    bool right_swing = (current_gait_phase == PHASE_RIGHT_SWING);
    Vector3 right_foot = trajectory_gen.generate_foot_trajectory(phase_right, right_swing);

    float right_angles[6];
    if (ik_solver.solve_leg_ik(right_foot, 18, right_angles)) {
      for (int i = 0; i < 6; i++) {
        joint_states[18 + i].target = constrain(right_angles[i] + 90.0f,
                                                JOINT_MIN[18 + i],
                                                JOINT_MAX[18 + i]);
      }
    }
  }

  void generate_arm_swing() {
    // Natural arm swing opposite to legs
    float left_arm_swing  = sinf(gait_cycle * 2.0f * PI) * 20.0f;
    float right_arm_swing = -left_arm_swing;

    joint_states[0].target = 90.0f + left_arm_swing;   // Left shoulder
    joint_states[6].target = 90.0f + right_arm_swing;  // Right shoulder
  }
};

GaitController gait_controller;

// ============================================================================
// MAIN CONTROL FUNCTIONS
// ============================================================================

void setup() {
  Serial.begin(SERIAL_BAUD_RATE);
  Serial.println(F("╔════════════════════════════════════════════════════════════╗"));
  Serial.println(F("║  ENHANCED HUMANOID ROBOT CONTROL SYSTEM V2.0              ║"));
  Serial.println(F("║  Professional Street-Walking AI Robot                     ║"));
  Serial.println(F("╚════════════════════════════════════════════════════════════╝"));
  Serial.println();

  // Initialize servos
  for (int i = 0; i < NUM_JOINTS; i++) {
    joint_servos[i].attach(servo_pins[i]);
    joint_servos[i].write(90);
    joint_states[i].position = 90;
    joint_states[i].target = 90;

    // Configure PID for joint type
    if (i >= 12) {  // Legs need higher gains
      joint_controllers[i] = EnhancedPID(4.0f, 0.3f, 0.1f, 0.6f);
    } else {
      joint_controllers[i] = EnhancedPID(3.0f, 0.2f, 0.08f, 0.5f);
    }
  }

  // Digital vision pins
  pinMode(vision_digital[0], INPUT_PULLUP); // sidewalk_detected
  pinMode(vision_digital[1], INPUT_PULLUP); // crosswalk_detected

  current_state = STATE_STANDING;

  Serial.println(F("✅ System Initialized"));
  Serial.println(F("🤖 State: STANDING"));
  Serial.println(F("📡 All sensors online"));
  Serial.println(F("🧠 AI decision making active"));
  Serial.println();
  Serial.println(F("Commands: walk | stop | status | emergency | reset | help"));
  Serial.println();
}

void loop() {
  static unsigned long last_time = millis();
  unsigned long current_time = millis();
  float dt = (current_time - last_time) / 1000.0f;
  last_time = current_time;

  // Guard against zero/negative dt
  if (dt <= 0.0f) dt = 0.01f;

  // Sync PID dt (once per cycle is fine)
  for (int i = 0; i < NUM_JOINTS; i++) {
    joint_controllers[i].set_dt(dt);
  }

  // Sensor fusion
  sensor_fusion.update_all();

  // Balance control
  balance_controller.update_com_estimate();
  Vector3 balance_correction = balance_controller.compute_balance_correction();
  (void)balance_correction; // placeholder (not applied to joints in this simplified example)

  // Decision making
  RobotState new_state = decision_maker.evaluate_state_transition();
  if (new_state != current_state) {
    transition_state(new_state);
  }

  // Gait generation
  gait_controller.update_gait(dt);
  if (current_state == STATE_WALKING) {
    gait_controller.generate_leg_targets();
    gait_controller.generate_arm_swing();
  }

  // Joint control with velocity/acceleration limits
  for (int i = 0; i < NUM_JOINTS; i++) {
    float target   = joint_states[i].target;
    float current  = joint_states[i].position;
    float velocity = joint_states[i].velocity;

    // PID control
    float control = joint_controllers[i].compute(target, current, velocity);

    // Apply velocity limits
    float max_delta = MAX_JOINT_VELOCITY * dt;
    if (control >  max_delta) control =  max_delta;
    if (control < -max_delta) control = -max_delta;

    // Update state
    joint_states[i].velocity = control / dt;
    joint_states[i].position += control;
    if (joint_states[i].position > JOINT_MAX[i]) joint_states[i].position = JOINT_MAX[i];
    if (joint_states[i].position < JOINT_MIN[i]) joint_states[i].position = JOINT_MIN[i];
    joint_states[i].error = fabsf(target - current);

    // Write to servo
    joint_servos[i].write((int)joint_states[i].position);
  }

  // Status reporting
  static int status_counter = 0;
  if (++status_counter >= 200) {  // Every ~2 seconds at 100Hz
    print_status();
    status_counter = 0;
  }

  // Command handling
  handle_commands();

  delay(10);  // ~100Hz control loop
}

void transition_state(RobotState new_state) {
  Serial.print(F("🔄 State Transition: "));
  print_state_name(current_state);
  Serial.print(F(" → "));
  print_state_name(new_state);
  Serial.println();

  current_state = new_state;
  state_change_time = millis();

  if (new_state == STATE_EMERGENCY) {
    for (int i = 0; i < NUM_JOINTS; i++) {
      joint_controllers[i].reset();
    }
  }
}

void print_state_name(RobotState state) {
  switch(state) {
    case STATE_IDLE: Serial.print(F("IDLE")); break;
    case STATE_STANDING: Serial.print(F("STANDING")); break;
    case STATE_WALKING: Serial.print(F("WALKING")); break;
    case STATE_TURNING: Serial.print(F("TURNING")); break;
    case STATE_STOPPING: Serial.print(F("STOPPING")); break;
    case STATE_EMERGENCY: Serial.print(F("EMERGENCY")); break;
    case STATE_RECOVERING: Serial.print(F("RECOVERING")); break;
  }
}

void print_status() {
  Serial.println(F("━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"));
  Serial.print(F("🤖 State: ")); print_state_name(current_state);
  Serial.print(F(" | Phase: "));
  switch(current_gait_phase) {
    case PHASE_DOUBLE_SUPPORT: Serial.print(F("DOUBLE")); break;
    case PHASE_LEFT_SWING: Serial.print(F("L-SWING")); break;
    case PHASE_RIGHT_SWING: Serial.print(F("R-SWING")); break;
    case PHASE_TRANSITION: Serial.print(F("TRANS")); break;
  }
  Serial.print(F(" | Cycle: ")); Serial.print(gait_cycle * 100, 0); Serial.println(F("%"));

  Serial.print(F("⚖️  Balance: "));
  Serial.print(balance_controller.check_stability() ? F("STABLE") : F("UNSTABLE"));
  Serial.print(F(" | COM: (")); Serial.print(center_of_mass.x, 2);
  Serial.print(F(", ")); Serial.print(center_of_mass.z, 2); Serial.println(F(")"));

  Serial.print(F("👣 Contact: L["));
  Serial.print(foot_contact.left_heel ? F("H") : F("-"));
  Serial.print(foot_contact.left_toe ? F("T") : F("-"));
  Serial.print(F("] R["));
  Serial.print(foot_contact.right_heel ? F("H") : F("-"));
  Serial.print(foot_contact.right_toe ? F("T") : F("-"));
  Serial.print(F("] | Force: L=")); Serial.print(foot_contact.left_force, 2);
  Serial.print(F(" R=")); Serial.println(foot_contact.right_force, 2);

  Serial.print(F("🌍 Environment: "));
  Serial.print(F("Traffic="));
  if (environment.traffic_light == 0) Serial.print(F("🔴RED"));
  else if (environment.traffic_light == 1) Serial.print(F("🟡YELLOW"));
  else Serial.print(F("🟢GREEN"));
  Serial.print(F(" | Pedestrians=")); Serial.print(environment.pedestrian_density * 100, 0); Serial.print(F("%"));
  Serial.print(F(" | Terrain=")); Serial.print(environment.terrain_roughness * 100, 0); Serial.println(F("% rough"));

  Serial.print(F("🚧 Obstacles: "));
  float min_dist = 10.0f;
  for (int i = 0; i < 8; i++) {
    if (environment.obstacle_distances[i] < min_dist) {
      min_dist = environment.obstacle_distances[i];
    }
  }
  Serial.print(F("Closest=")); Serial.print(min_dist, 2); Serial.print(F("m"));
  Serial.print(F(" | Sidewalk=")); Serial.print(environment.sidewalk_detected ? F("YES") : F("NO"));
  Serial.print(F(" | Crosswalk=")); Serial.println(environment.crosswalk_detected ? F("YES") : F("NO"));

  Serial.print(F("🎯 Motion: Speed=")); Serial.print(decision_maker.compute_safe_speed(), 2);
  Serial.print(F("m/s | Orientation=("));
  Serial.print(orientation.x, 1); Serial.print(F("°, "));
  Serial.print(orientation.y, 1); Serial.println(F("°)"));

  // Joint health check
  int fault_count = 0;
  float max_error = 0;
  for (int i = 0; i < NUM_JOINTS; i++) {
    if (joint_states[i].fault) fault_count++;
    if (joint_states[i].error > max_error) max_error = joint_states[i].error;
  }
  Serial.print(F("🔧 Joints: MaxError=")); Serial.print(max_error, 1);
  Serial.print(F("° | Faults=")); Serial.println(fault_count);

  Serial.println(F("━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"));
  Serial.println();
}

void handle_commands() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    cmd.toLowerCase();

    if (cmd == "walk") {
      if (current_state == STATE_STANDING || current_state == STATE_IDLE) {
        current_state = STATE_WALKING;
        gait_cycle = 0;
        Serial.println(F("✅ Walking initiated"));
      } else {
        Serial.println(F("⚠️  Already in motion or cannot walk from current state"));
      }
    }
    else if (cmd == "stop") {
      if (current_state == STATE_WALKING) {
        current_state = STATE_STOPPING;
        Serial.println(F("🛑 Stopping gait..."));
        delay(500);
        current_state = STATE_STANDING;
        Serial.println(F("✅ Standing"));
      }
    }
    else if (cmd == "emergency") {
      emergency_stop = true;
      current_state = STATE_EMERGENCY;
      Serial.println(F("🚨 EMERGENCY STOP ACTIVATED!"));
      Serial.println(F("   All systems halted. Send 'reset' to recover."));
    }
    else if (cmd == "reset") {
      emergency_stop = false;
      current_state = STATE_STANDING;
      gait_cycle = 0;

      // Reset all joint controllers
      for (int i = 0; i < NUM_JOINTS; i++) {
        joint_controllers[i].reset();
        joint_states[i].target = 90;
        joint_states[i].fault = false;
      }

      Serial.println(F("🔄 System reset complete"));
      Serial.println(F("✅ Ready for operation"));
    }
    else if (cmd == "status") {
      print_detailed_status();
    }
    else if (cmd == "balance") {
      Serial.print(F("⚖️  Balance Status: "));
      Serial.println(balance_controller.check_stability() ? F("STABLE ✅") : F("UNSTABLE ⚠️"));
      Serial.print(F("   COM Position: ("));
      Serial.print(center_of_mass.x, 3); Serial.print(F(", "));
      Serial.print(center_of_mass.y, 3); Serial.print(F(", "));
      Serial.print(center_of_mass.z, 3); Serial.println(F(")"));
      Serial.print(F("   Orientation: Roll=")); Serial.print(orientation.x, 2);
      Serial.print(F("° Pitch=")); Serial.print(orientation.y, 2); Serial.println(F("°"));
    }
    else if (cmd == "joints") {
      Serial.println(F("🔧 Joint Status:"));
      Serial.println(F("   ID  Pos    Tgt    Vel     Error  Status"));
      Serial.println(F("   ─────────────────────────────────────────"));
      for (int i = 0; i < NUM_JOINTS; i++) {
        Serial.print(F("   "));
        if (i < 10) Serial.print(F(" "));
        Serial.print(i);
        Serial.print(F("  "));
        Serial.print(joint_states[i].position, 1);
        Serial.print(F("  "));
        Serial.print(joint_states[i].target, 1);
        Serial.print(F("  "));
        Serial.print(joint_states[i].velocity, 1);
        Serial.print(F("  "));
        Serial.print(joint_states[i].error, 2);
        Serial.print(F("  "));
        Serial.println(joint_states[i].fault ? F("FAULT") : F("OK"));
      }
    }
    else if (cmd == "env") {
      Serial.println(F("🌍 Environmental Sensors:"));
      Serial.print(F("   Traffic Light: "));
      if (environment.traffic_light == 0) Serial.println(F("🔴 RED"));
      else if (environment.traffic_light == 1) Serial.println(F("🟡 YELLOW"));
      else Serial.println(F("🟢 GREEN"));

      Serial.print(F("   Pedestrian Density: "));
      Serial.print(environment.pedestrian_density * 100, 1); Serial.println(F("%"));

      Serial.print(F("   Terrain: Roughness="));
      Serial.print(environment.terrain_roughness * 100, 1);
      Serial.print(F("% Slope=")); Serial.print(environment.terrain_slope, 2); Serial.println(F("°"));

      Serial.println(F("   Obstacle Distances (m):"));
      Serial.print(F("     N=")); Serial.print(environment.obstacle_distances[0], 2);
      Serial.print(F(" NE=")); Serial.print(environment.obstacle_distances[1], 2);
      Serial.print(F(" E=")); Serial.print(environment.obstacle_distances[2], 2);
      Serial.print(F(" SE=")); Serial.println(environment.obstacle_distances[3], 2);
      Serial.print(F("     S=")); Serial.print(environment.obstacle_distances[4], 2);
      Serial.print(F(" SW=")); Serial.print(environment.obstacle_distances[5], 2);
      Serial.print(F(" W=")); Serial.print(environment.obstacle_distances[6], 2);
      Serial.print(F(" NW=")); Serial.println(environment.obstacle_distances[7], 2);

      Serial.print(F("   Sidewalk: ")); Serial.println(environment.sidewalk_detected ? F("✅ Detected") : F("❌ Not detected"));
      Serial.print(F("   Crosswalk: ")); Serial.println(environment.crosswalk_detected ? F("✅ Detected") : F("❌ Not detected"));
    }
    else if (cmd == "speed") {
      Serial.print(F("⚡ Current Safe Speed: "));
      Serial.print(decision_maker.compute_safe_speed(), 3);
      Serial.println(F(" m/s"));
      Serial.print(F("   Base Walking Speed: "));
      Serial.print(walking_speed, 2);
      Serial.println(F(" m/s"));
      Serial.print(F("   Step Length: "));
      Serial.print(step_length, 2);
      Serial.println(F(" m"));
      Serial.print(F("   Step Time: "));
      Serial.print(step_time, 2);
      Serial.println(F(" s"));
    }
    else if (cmd == "help") {
      Serial.println(F("╔════════════════════════════════════════════════════════════╗"));
      Serial.println(F("║  ENHANCED HUMANOID ROBOT - COMMAND REFERENCE              ║"));
      Serial.println(F("╚════════════════════════════════════════════════════════════╝"));
      Serial.println();
      Serial.println(F("🚶 MOTION COMMANDS:"));
      Serial.println(F("   walk       - Start walking gait"));
      Serial.println(F("   stop       - Stop walking and stand"));
      Serial.println(F("   emergency  - Emergency stop (halts all motion)"));
      Serial.println(F("   reset      - Reset system after emergency"));
      Serial.println();
      Serial.println(F("📊 STATUS COMMANDS:"));
      Serial.println(F("   status     - Show detailed system status"));
      Serial.println(F("   balance    - Check balance and COM position"));
      Serial.println(F("   joints     - Display all joint states"));
      Serial.println(F("   env        - Show environmental sensor data"));
      Serial.println(F("   speed      - Display speed and gait parameters"));
      Serial.println();
      Serial.println(F("ℹ️  INFORMATION:"));
      Serial.println(F("   help       - Show this help message"));
      Serial.println();
      Serial.println(F("🤖 FEATURES:"));
      Serial.println(F("   • Inverse kinematics for precise control"));
      Serial.println(F("   • ZMP-based balance control"));
      Serial.println(F("   • Multi-sensor fusion (IMU, force, proximity, vision)"));
      Serial.println(F("   • Adaptive gait generation"));
      Serial.println(F("   • Traffic light detection and compliance"));
      Serial.println(F("   • Pedestrian awareness"));
      Serial.println(F("   • Predictive collision avoidance"));
      Serial.println(F("   • Terrain adaptation"));
      Serial.println();
    }
    else if (cmd.length() > 0) {
      Serial.print(F("❌ Unknown command: '"));
      Serial.print(cmd);
      Serial.println(F("'"));
      Serial.println(F("   Type 'help' for available commands"));
    }
  }
}

void print_detailed_status() {
  Serial.println(F("╔════════════════════════════════════════════════════════════╗"));
  Serial.println(F("║           DETAILED SYSTEM STATUS REPORT                   ║"));
  Serial.println(F("╚════════════════════════════════════════════════════════════╝"));
  Serial.println();

  // System State
  Serial.println(F("🤖 SYSTEM STATE:"));
  Serial.print(F("   Current State: ")); print_state_name(current_state); Serial.println();
  Serial.print(F("   Gait Phase: "));
  switch(current_gait_phase) {
    case PHASE_DOUBLE_SUPPORT: Serial.println(F("Double Support")); break;
    case PHASE_LEFT_SWING: Serial.println(F("Left Swing")); break;
    case PHASE_RIGHT_SWING: Serial.println(F("Right Swing")); break;
    case PHASE_TRANSITION: Serial.println(F("Transition")); break;
  }
  Serial.print(F("   Gait Cycle Progress: ")); Serial.print(gait_cycle * 100, 1); Serial.println(F("%"));
  Serial.print(F("   Time in State: ")); Serial.print((millis() - state_change_time) / 1000.0, 1); Serial.println(F(" s"));
  Serial.print(F("   Emergency Stop: ")); Serial.println(emergency_stop ? F("⚠️  ACTIVE") : F("✅ Clear"));
  Serial.println();

  // Balance and Stability
  Serial.println(F("⚖️  BALANCE & STABILITY:"));
  Serial.print(F("   Status: ")); Serial.println(balance_controller.check_stability() ? F("✅ STABLE") : F("⚠️  UNSTABLE"));
  Serial.print(F("   Center of Mass: ("));
  Serial.print(center_of_mass.x, 3); Serial.print(F(", "));
  Serial.print(center_of_mass.y, 3); Serial.print(F(", "));
  Serial.print(center_of_mass.z, 3); Serial.println(F(") m"));
  Serial.print(F("   COM Velocity: ("));
  Serial.print(com_velocity.x, 3); Serial.print(F(", "));
  Serial.print(com_velocity.y, 3); Serial.println(F(") m/s"));
  Serial.print(F("   Orientation: Roll="));
  Serial.print(orientation.x, 2); Serial.print(F("° Pitch="));
  Serial.print(orientation.y, 2); Serial.print(F("° Yaw="));
  Serial.print(orientation.z, 2); Serial.println(F("°"));
  Serial.println();

  // Foot Contact
  Serial.println(F("👣 FOOT CONTACT:"));
  Serial.print(F("   Left Foot: "));
  if (foot_contact.left_heel) Serial.print(F("Heel✓ "));
  if (foot_contact.left_toe) Serial.print(F("Toe✓ "));
  Serial.print(F("| Force: ")); Serial.println(foot_contact.left_force, 2);
  Serial.print(F("   Right Foot: "));
  if (foot_contact.right_heel) Serial.print(F("Heel✓ "));
  if (foot_contact.right_toe) Serial.print(F("Toe✓ "));
  Serial.print(F("| Force: ")); Serial.println(foot_contact.right_force, 2);
  Serial.println();

  // Environmental Awareness
  Serial.println(F("🌍 ENVIRONMENT:"));
  Serial.print(F("   Traffic Light: "));
  if (environment.traffic_light == 0) Serial.println(F("🔴 RED - STOP"));
  else if (environment.traffic_light == 1) Serial.println(F("🟡 YELLOW - CAUTION"));
  else Serial.println(F("🟢 GREEN - GO"));
  Serial.print(F("   Pedestrian Density: ")); Serial.print(environment.pedestrian_density * 100, 1); Serial.println(F("%"));
  Serial.print(F("   Terrain Roughness: ")); Serial.print(environment.terrain_roughness * 100, 1); Serial.println(F("%"));
  Serial.print(F("   Sidewalk Detection: ")); Serial.println(environment.sidewalk_detected ? F("✅ Yes") : F("❌ No"));
  Serial.print(F("   Crosswalk Detection: ")); Serial.println(environment.crosswalk_detected ? F("✅ Yes") : F("❌ No"));

  float min_obstacle = 10.0f;
  int min_direction = 0;
  for (int i = 0; i < 8; i++) {
    if (environment.obstacle_distances[i] < min_obstacle) {
      min_obstacle = environment.obstacle_distances[i];
      min_direction = i;
    }
  }
  Serial.print(F("   Closest Obstacle: ")); Serial.print(min_obstacle, 2); Serial.print(F(" m at "));
  const char* directions[] = {"N", "NE", "E", "SE", "S", "SW", "W", "NW"};
  Serial.println(directions[min_direction]);
  Serial.println();

  // Motion Parameters
  Serial.println(F("⚡ MOTION:"));
  Serial.print(F("   Target Speed: ")); Serial.print(walking_speed, 2); Serial.println(F(" m/s"));
  Serial.print(F("   Safe Speed: ")); Serial.print(decision_maker.compute_safe_speed(), 2); Serial.println(F(" m/s"));
  Serial.print(F("   Step Length: ")); Serial.print(step_length, 2); Serial.println(F(" m"));
  Serial.print(F("   Step Height: ")); Serial.print(step_height, 3); Serial.println(F(" m"));
  Serial.print(F("   Step Time: ")); Serial.print(step_time, 2); Serial.println(F(" s"));
  Serial.println();

  // Joint Health
  Serial.println(F("🔧 JOINT HEALTH:"));
  int fault_count = 0;
  float total_error = 0;
  float max_error = 0;
  for (int i = 0; i < NUM_JOINTS; i++) {
    if (joint_states[i].fault) fault_count++;
    total_error += joint_states[i].error;
    if (joint_states[i].error > max_error) max_error = joint_states[i].error;
  }
  Serial.print(F("   Total Joints: ")); Serial.println(NUM_JOINTS);
  Serial.print(F("   Faults: ")); Serial.println(fault_count);
  Serial.print(F("   Average Error: ")); Serial.print(total_error / NUM_JOINTS, 2); Serial.println(F("°"));
  Serial.print(F("   Maximum Error: ")); Serial.print(max_error, 2); Serial.println(F("°"));
  Serial.println();

  Serial.println(F("════════════════════════════════════════════════════════════"));
  Serial.println();
}