#include "eda_dog_movements.h"

#include <algorithm>
#include <cmath>

#include "oscillator.h"

static const char *TAG = "EDARobotDogMovements";

#define LEG_HOME_POSITION 90

EDARobotDog::EDARobotDog() {
  is_dog_resting_ = false;
  // 初始化所有舵机管脚为-1（未连接）
  for (int i = 0; i < SERVO_COUNT; i++) {
    servo_pins_[i] = -1;
    servo_trim_[i] = 0;
  }
}

EDARobotDog::~EDARobotDog() { DetachServos(); }

unsigned long IRAM_ATTR millis() {
  return (unsigned long)(esp_timer_get_time() / 1000ULL);
}

void EDARobotDog::Init(int left_front_leg, int left_rear_leg, int right_front_leg,
                  int right_rear_leg) {
  servo_pins_[LEFT_FRONT_LEG] = left_front_leg;
  servo_pins_[LEFT_REAR_LEG] = left_rear_leg;
  servo_pins_[RIGHT_FRONT_LEG] = right_front_leg;
  servo_pins_[RIGHT_REAR_LEG] = right_rear_leg;

  AttachServos();
  is_dog_resting_ = false;
}

///////////////////////////////////////////////////////////////////
//-- ATTACH & DETACH FUNCTIONS ----------------------------------//
///////////////////////////////////////////////////////////////////
void EDARobotDog::AttachServos() {
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      servo_[i].Attach(servo_pins_[i]);
    }
  }
}

void EDARobotDog::DetachServos() {
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      servo_[i].Detach();
    }
  }
}

///////////////////////////////////////////////////////////////////
//-- OSCILLATORS TRIMS ------------------------------------------//
///////////////////////////////////////////////////////////////////
void EDARobotDog::SetTrims(int left_front_leg, int left_rear_leg,
                      int right_front_leg, int right_rear_leg) {
  servo_trim_[LEFT_FRONT_LEG] = left_front_leg;
  servo_trim_[LEFT_REAR_LEG] = left_rear_leg;
  servo_trim_[RIGHT_FRONT_LEG] = right_front_leg;
  servo_trim_[RIGHT_REAR_LEG] = right_rear_leg;

  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      servo_[i].SetTrim(servo_trim_[i]);
    }
  }
}

///////////////////////////////////////////////////////////////////
//-- BASIC MOTION FUNCTIONS -------------------------------------//
///////////////////////////////////////////////////////////////////
void EDARobotDog::MoveServos(int time, int servo_target[]) {
  if (GetRestState() == true) {
    SetRestState(false);
  }

  final_time_ = millis() + time;
  if (time > 10) {
    for (int i = 0; i < SERVO_COUNT; i++) {
      if (servo_pins_[i] != -1) {
        increment_[i] =
            (servo_target[i] - servo_[i].GetPosition()) / (time / 10.0);
      }
    }

    for (int iteration = 1; millis() < final_time_; iteration++) {
      partial_time_ = millis() + 10;
      for (int i = 0; i < SERVO_COUNT; i++) {
        if (servo_pins_[i] != -1) {
          servo_[i].SetPosition(servo_[i].GetPosition() + increment_[i]);
        }
      }
      vTaskDelay(pdMS_TO_TICKS(10));
    }
  } else {
    for (int i = 0; i < SERVO_COUNT; i++) {
      if (servo_pins_[i] != -1) {
        servo_[i].SetPosition(servo_target[i]);
      }
    }
    vTaskDelay(pdMS_TO_TICKS(time));
  }

  // final adjustment to the target.
  bool f = true;
  int adjustment_count = 0;
  while (f && adjustment_count < 10) {
    f = false;
    for (int i = 0; i < SERVO_COUNT; i++) {
      if (servo_pins_[i] != -1 && servo_target[i] != servo_[i].GetPosition()) {
        f = true;
        break;
      }
    }
    if (f) {
      for (int i = 0; i < SERVO_COUNT; i++) {
        if (servo_pins_[i] != -1) {
          servo_[i].SetPosition(servo_target[i]);
        }
      }
      vTaskDelay(pdMS_TO_TICKS(10));
      adjustment_count++;
    }
  };
}

void EDARobotDog::MoveSingle(int position, int servo_number) {
  if (position > 180)
    position = 90;
  if (position < 0)
    position = 90;

  if (GetRestState() == true) {
    SetRestState(false);
  }

  if (servo_number >= 0 && servo_number < SERVO_COUNT &&
      servo_pins_[servo_number] != -1) {
    servo_[servo_number].SetPosition(position);
  }
}

void EDARobotDog::OscillateServos(int amplitude[SERVO_COUNT],
                             int offset[SERVO_COUNT], int period,
                             double phase_diff[SERVO_COUNT], float cycle = 1) {
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      servo_[i].SetO(offset[i]);
      servo_[i].SetA(amplitude[i]);
      servo_[i].SetT(period);
      servo_[i].SetPh(phase_diff[i]);
    }
  }

  double ref = millis();
  double end_time = period * cycle + ref;

  while (millis() < end_time) {
    for (int i = 0; i < SERVO_COUNT; i++) {
      if (servo_pins_[i] != -1) {
        servo_[i].Refresh();
      }
    }
    vTaskDelay(5);
  }
  vTaskDelay(pdMS_TO_TICKS(10));
}

void EDARobotDog::Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT],
                     int period, double phase_diff[SERVO_COUNT],
                     float steps = 1.0) {
  if (GetRestState() == true) {
    SetRestState(false);
  }

  int cycles = (int)steps;

  //-- Execute complete cycles
  if (cycles >= 1)
    for (int i = 0; i < cycles; i++)
      OscillateServos(amplitude, offset, period, phase_diff);

  //-- Execute the final not complete cycle
  OscillateServos(amplitude, offset, period, phase_diff, (float)steps - cycles);
  vTaskDelay(pdMS_TO_TICKS(10));
}

///////////////////////////////////////////////////////////////////
//-- HOME = Dog at rest position --------------------------------//
///////////////////////////////////////////////////////////////////
void EDARobotDog::Home() {
  if (is_dog_resting_ == false) { // Go to rest position only if necessary
    int homes[SERVO_COUNT] = {LEG_HOME_POSITION, LEG_HOME_POSITION,
                              LEG_HOME_POSITION, LEG_HOME_POSITION};
    MoveServos(500, homes);
    is_dog_resting_ = true;
  }
  vTaskDelay(pdMS_TO_TICKS(200));
}

bool EDARobotDog::GetRestState() { return is_dog_resting_; }

void EDARobotDog::SetRestState(bool state) { is_dog_resting_ = state; }

///////////////////////////////////////////////////////////////////
//-- BASIC LEG MOVEMENTS ----------------------------------------//
///////////////////////////////////////////////////////////////////

void EDARobotDog::LiftLeftFrontLeg(int period, int height) {

  // 获取当前位置
  int current_pos[SERVO_COUNT];
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      current_pos[i] = servo_[i].GetPosition();
    } else {
      current_pos[i] = LEG_HOME_POSITION;
    }
  }

  // 重复3次摇摆动作
  for (int num = 0; num < 3; num++) {
    // servo1.write(180); delay(100);
    current_pos[LEFT_FRONT_LEG] = 0; // servo1
    MoveServos(100, current_pos);

    // servo1.write(150); delay(100);
    current_pos[LEFT_FRONT_LEG] = 30; // servo1
    MoveServos(100, current_pos);
  }

  // servo1.write(90);
  current_pos[LEFT_FRONT_LEG] = 90; // servo1
  MoveServos(100, current_pos);
}

void EDARobotDog::LiftLeftRearLeg(int period, int height) {

  // 获取当前位置
  int current_pos[SERVO_COUNT];
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      current_pos[i] = servo_[i].GetPosition();
    } else {
      current_pos[i] = LEG_HOME_POSITION;
    }
  }

  // 重复3次摇摆动作
  for (int num = 0; num < 3; num++) {
    // servo1.write(180); delay(100);
    current_pos[LEFT_REAR_LEG] = 180; // servo1
    MoveServos(100, current_pos);

    // servo1.write(150); delay(100);
    current_pos[LEFT_REAR_LEG] = 150; // servo1
    MoveServos(100, current_pos);
  }

  // servo1.write(90);
  current_pos[LEFT_REAR_LEG] = 90; // servo1
  MoveServos(100, current_pos);
}

void EDARobotDog::LiftRightFrontLeg(int period, int height) {
  
  // 获取当前位置
  int current_pos[SERVO_COUNT];
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      current_pos[i] = servo_[i].GetPosition();
    } else {
      current_pos[i] = LEG_HOME_POSITION;
    }
  }

  // 重复3次摇摆动作
  for (int num = 0; num < 3; num++) {
    // servo1.write(180); delay(100);
    current_pos[RIGHT_FRONT_LEG] = 180; // servo1
    MoveServos(100, current_pos);

    // servo1.write(150); delay(100);
    current_pos[RIGHT_FRONT_LEG] = 150; // servo1
    MoveServos(100, current_pos);
  }

  // servo1.write(90);
  current_pos[RIGHT_FRONT_LEG] = 90; // servo1
  MoveServos(100, current_pos);
}

void EDARobotDog::LiftRightRearLeg(int period, int height) {

  // 获取当前位置
  int current_pos[SERVO_COUNT];
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      current_pos[i] = servo_[i].GetPosition();
    } else {
      current_pos[i] = LEG_HOME_POSITION;
    }
  }

  // 重复3次摇摆动作
  for (int num = 0; num < 3; num++) {
    // servo1.write(180); delay(100);
    current_pos[RIGHT_REAR_LEG] = 0; // servo1
    MoveServos(100, current_pos);

    // servo1.write(150); delay(100);
    current_pos[RIGHT_REAR_LEG] = 30; // servo1
    MoveServos(100, current_pos);
  }

  // servo1.write(90);
  current_pos[RIGHT_FRONT_LEG] = 90; // servo1
  MoveServos(100, current_pos);
}

///////////////////////////////////////////////////////////////////
//-- DOG GAIT MOVEMENTS -----------------------------------------//
///////////////////////////////////////////////////////////////////

// 舵机方向分析（从 Stretch/Sleep 推断物理前后方向）：
//
//   Stretch: LF=0, LR=180, RF=180, RR=0
//   → 前腿向前伸，后腿向后伸
//
//   LEFT_FRONT_LEG  (0): 减小=向前, 增大=向后
//   LEFT_REAR_LEG   (1): 减小=向前, 增大=向后
//   RIGHT_FRONT_LEG (2): 增大=向前, 减小=向后
//   RIGHT_REAR_LEG  (3): 增大=向前, 减小=向后
//
//   即：左侧两腿同向（减=前），右侧两腿同向（增=前）
//   对角线A: 左前 + 右后 → 前进时左前减小, 右后减小（都是各自的"向前"）
//   对角线B: 左后 + 右前 → 前进时左后减小, 右前增大（都是各自的"向前"）
//
//   抬腿方向（从 Lift 函数）：
//   LEFT_FRONT_LEG:  0°   (减小=抬)
//   LEFT_REAR_LEG:   180° (增大=抬)
//   RIGHT_FRONT_LEG: 180° (增大=抬)
//   RIGHT_REAR_LEG:  0°   (减小=抬)

// 辅助：获取当前所有舵机位置
void EDARobotDog::GetCurrentPositions(int pos[SERVO_COUNT]) {
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      pos[i] = servo_[i].GetPosition();
    } else {
      pos[i] = LEG_HOME_POSITION;
    }
  }
}

void EDARobotDog::Turn(float steps, int period, int dir) {
  if (GetRestState() == true) {
    SetRestState(false);
  }

  // 转弯 = 左侧后退 + 右侧前进（左转），或反过来（右转）
  // 从 Walk 的已验证方向推导：
  //   前进: LF=90-s, LR=90-s, RF=90+s, RR=90+s
  //   后退: LF=90+s, LR=90+s, RF=90-s, RR=90-s
  //
  // 左转(d=1): 左侧后退+右侧前进
  //   LF=90+s, LR=90+s, RF=90+s, RR=90+s → 全部 90+s
  //   地面反推: 全部 90-s
  //
  // 右转(d=-1): 左侧前进+右侧后退
  //   LF=90-s, LR=90-s, RF=90-s, RR=90-s → 全部 90-s
  //   地面反推: 全部 90+s

  const int swing = 60;
  const int lift = 25;
  int t = period / 6;
  if (t < 50) t = 50;

  int d = (dir == LEFT) ? -1 : 1;

  for (int step = 0; step < (int)steps; step++) {
    int pos[SERVO_COUNT];
    GetCurrentPositions(pos);

    // 拍1: 抬A（LF+RR）
    pos[LEFT_FRONT_LEG]  = 90 - lift;
    pos[RIGHT_REAR_LEG]  = 90 - lift;
    MoveServos(t, pos);

    // 拍2: A空中摆 + B地面反推
    pos[LEFT_FRONT_LEG]  = 90 - lift + d * swing;  // LF空中
    pos[RIGHT_REAR_LEG]  = 90 - lift + d * swing;  // RR空中
    pos[LEFT_REAR_LEG]   = 90 - d * swing;         // LR地面反推
    pos[RIGHT_FRONT_LEG] = 90 - d * swing;         // RF地面反推
    MoveServos(t, pos);

    // 拍3: 放下A
    pos[LEFT_FRONT_LEG]  = 90 + d * swing;
    pos[RIGHT_REAR_LEG]  = 90 + d * swing;
    MoveServos(t / 2, pos);

    // 拍4: 抬B（LR+RF）
    pos[LEFT_REAR_LEG]   = 90 + lift;
    pos[RIGHT_FRONT_LEG] = 90 + lift;
    MoveServos(t, pos);

    // 拍5: B空中摆 + A地面反推
    pos[LEFT_REAR_LEG]   = 90 + lift + d * swing;  // LR空中
    pos[RIGHT_FRONT_LEG] = 90 + lift + d * swing;  // RF空中
    pos[LEFT_FRONT_LEG]  = 90 - d * swing;         // LF地面反推
    pos[RIGHT_REAR_LEG]  = 90 - d * swing;         // RR地面反推
    MoveServos(t, pos);

    // 拍6: 放下B
    pos[LEFT_REAR_LEG]   = 90 + d * swing;
    pos[RIGHT_FRONT_LEG] = 90 + d * swing;
    MoveServos(t / 2, pos);
  }

  // 结束回中
  int home[SERVO_COUNT] = {90, 90, 90, 90};
  MoveServos(150, home);
}

void EDARobotDog::Walk(float steps, int period, int dir) {
  if (GetRestState() == true) {
    SetRestState(false);
  }

  // Trot 对角步态
  // 方向: LF减=前, LR减=前, RF增=前, RR增=前
  // 抬腿: LF减=抬, LR增=抬, RF增=抬, RR减=抬

  const int lift = 25;
  const int swing = 30;
  int t = period / 6;
  if (t < 50) t = 50;

  int fwd = (dir == FORWARD) ? 1 : -1;

  for (int step = 0; step < (int)steps; step++) {
    int pos[SERVO_COUNT];
    GetCurrentPositions(pos);

    // 拍1: 抬起对角线A（LF+RR）
    pos[LEFT_FRONT_LEG]  = 90 - lift;
    pos[RIGHT_REAR_LEG]  = 90 - lift;
    MoveServos(t, pos);

    // 拍2: A空中前摆 + B地面后推（同时）
    pos[LEFT_FRONT_LEG]  = 90 - lift - fwd * swing;  // LF: 保持抬 + 前摆
    pos[RIGHT_REAR_LEG]  = 90 - lift + fwd * swing;  // RR: 保持抬 + 前摆(增=前)
    pos[LEFT_REAR_LEG]   = 90 + fwd * swing;         // LR: 地面后推(增=后)
    pos[RIGHT_FRONT_LEG] = 90 - fwd * swing;         // RF: 地面后推(减=后)
    MoveServos(t, pos);

    // 拍3: 放下A（着地到前摆位置）
    pos[LEFT_FRONT_LEG]  = 90 - fwd * swing;
    pos[RIGHT_REAR_LEG]  = 90 + fwd * swing;
    MoveServos(t / 2, pos);

    // 拍4: 抬起对角线B（LR+RF）
    pos[LEFT_REAR_LEG]   = 90 + lift;
    pos[RIGHT_FRONT_LEG] = 90 + lift;
    MoveServos(t, pos);

    // 拍5: B空中前摆 + A地面后推（同时）
    pos[LEFT_REAR_LEG]   = 90 + lift - fwd * swing;  // LR: 保持抬 + 前摆(减=前)
    pos[RIGHT_FRONT_LEG] = 90 + lift + fwd * swing;  // RF: 保持抬 + 前摆(增=前)
    pos[LEFT_FRONT_LEG]  = 90 + fwd * swing;         // LF: 地面后推(增=后)
    pos[RIGHT_REAR_LEG]  = 90 - fwd * swing;         // RR: 地面后推(减=后)
    MoveServos(t, pos);

    // 拍6: 放下B（不回中，直接衔接下一步）
    pos[LEFT_REAR_LEG]   = 90 - fwd * swing;
    pos[RIGHT_FRONT_LEG] = 90 + fwd * swing;
    MoveServos(t / 2, pos);
  }

  // 结束回中
  int home[SERVO_COUNT] = {90, 90, 90, 90};
  MoveServos(150, home);
}

void EDARobotDog::Sit(int period) {


int current_pos[SERVO_COUNT];
      for (int i = 0; i < SERVO_COUNT; i++) {
        if (servo_pins_[i] != -1) {
          current_pos[i] = servo_[i].GetPosition();
        } else {
          current_pos[i] = LEG_HOME_POSITION;
        }
      }


  current_pos[LEFT_REAR_LEG] = 0;  // servo2
  current_pos[RIGHT_REAR_LEG] = 180; // servo4
  MoveServos(100, current_pos);
}

void EDARobotDog::Stand(int period) {
  // 站立：所有腿回到中立位置
  Home();
}

void EDARobotDog::Stretch(int period) {


  // 获取当前位置
int current_pos[SERVO_COUNT];
      for (int i = 0; i < SERVO_COUNT; i++) {
        if (servo_pins_[i] != -1) {
          current_pos[i] = servo_[i].GetPosition();
        } else {
          current_pos[i] = LEG_HOME_POSITION;
        }
      }


  current_pos[LEFT_FRONT_LEG] = 0;  // servo1
  current_pos[RIGHT_REAR_LEG] = 0;    // servo3
  current_pos[LEFT_REAR_LEG] = 180;     // servo2
  current_pos[RIGHT_FRONT_LEG] = 180; // servo4
  MoveServos(100, current_pos);
}

void EDARobotDog::Shake(int period) {
  // 摇摆：左右摇摆身体，左前腿和右后腿运动方向相反
  int A[SERVO_COUNT] = {20, 0, 20, 0}; // 只有前腿摇摆
  int O[SERVO_COUNT] = {0, LEG_HOME_POSITION, 0, LEG_HOME_POSITION};
  // 左前腿和右前腿反相摇摆
  double phase_diff[SERVO_COUNT] = {DEG2RAD(180), 0, DEG2RAD(0), 0};

  Execute(A, O, period, phase_diff, 3);
}

void EDARobotDog::EnableServoLimit(int diff_limit) {
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      servo_[i].SetLimiter(diff_limit);
    }
  }
}

void EDARobotDog::DisableServoLimit() {
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      servo_[i].DisableLimiter();
    }
  }
}

void EDARobotDog::Sleep() {

  int current_pos[SERVO_COUNT];
  for (int i = 0; i < SERVO_COUNT; i++) {
    if (servo_pins_[i] != -1) {
      current_pos[i] = servo_[i].GetPosition();
    } else {
      current_pos[i] = LEG_HOME_POSITION;
    }
  }

  // servo1.write(0); servo3.write(180); servo2.write(180); servo4.write(0);
  current_pos[LEFT_FRONT_LEG] = 0;   // servo1
  current_pos[RIGHT_REAR_LEG] = 180; // servo3
  current_pos[LEFT_REAR_LEG] = 180;  // servo2
  current_pos[RIGHT_FRONT_LEG] = 0;  // servo4
  MoveServos(100, current_pos);
}