#include "movements.h"

#include <algorithm>
#include <cmath>

#include "oscillator.h"

namespace {
float EaseOutCubic(float t) {
    float inv = 1.0f - t;
    return 1.0f - inv * inv * inv;
}

int ServoMinAngle(int servo_index) {
    switch (servo_index) {
        case RIGHT_ROLL:
            return 100;
        case LEFT_ROLL:
            return 0;
        case BODY:
            return 30;
        case HEAD:
            return 75;
        default:
            return 0;
    }
}

int ServoMaxAngle(int servo_index) {
    switch (servo_index) {
        case RIGHT_ROLL:
            return 180;
        case LEFT_ROLL:
            return 80;
        case BODY:
            return 150;
        case HEAD:
            return 105;
        default:
            return 180;
    }
}

int ClampServoTarget(int servo_index, int position) {
    return std::clamp(position, ServoMinAngle(servo_index), ServoMaxAngle(servo_index));
}

int ClampServoAmplitude(int servo_index, int center, int amplitude) {
    int max_safe_amplitude =
        std::min(center - ServoMinAngle(servo_index), ServoMaxAngle(servo_index) - center);
    return std::clamp(amplitude, 0, std::max(0, max_safe_amplitude));
}
}  // namespace

Otto::Otto() {
    is_otto_resting_ = false;
    for (int i = 0; i < SERVO_COUNT; i++) {
        servo_pins_[i] = -1;
        servo_trim_[i] = 0;
    }
}

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

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

void Otto::Init(int right_pitch, int right_roll, int left_pitch, int left_roll, int body,
                int head) {
    servo_pins_[RIGHT_PITCH] = right_pitch;
    servo_pins_[RIGHT_ROLL] = right_roll;
    servo_pins_[LEFT_PITCH] = left_pitch;
    servo_pins_[LEFT_ROLL] = left_roll;
    servo_pins_[BODY] = body;
    servo_pins_[HEAD] = head;

    AttachServos();
    is_otto_resting_ = false;
}

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

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

///////////////////////////////////////////////////////////////////
//-- OSCILLATORS TRIMS ------------------------------------------//
///////////////////////////////////////////////////////////////////
void Otto::SetTrims(int right_pitch, int right_roll, int left_pitch, int left_roll, int body,
                    int head) {
    servo_trim_[RIGHT_PITCH] = right_pitch;
    servo_trim_[RIGHT_ROLL] = right_roll;
    servo_trim_[LEFT_PITCH] = left_pitch;
    servo_trim_[LEFT_ROLL] = left_roll;
    servo_trim_[BODY] = body;
    servo_trim_[HEAD] = head;

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

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

    int target[SERVO_COUNT];
    for (int i = 0; i < SERVO_COUNT; i++) {
        target[i] = ClampServoTarget(i, servo_target[i]);
    }

    if (time <= 10) {
        for (int i = 0; i < SERVO_COUNT; i++) {
            if (servo_pins_[i] != -1) {
                servo_[i].SetPosition(target[i]);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(time));
        return;
    }

    int start[SERVO_COUNT];
    for (int i = 0; i < SERVO_COUNT; i++) {
        start[i] = servo_[i].GetPosition();
    }

    int steps = std::max(1, time / 10);
    for (int step = 1; step <= steps; step++) {
        float t = static_cast<float>(step) / static_cast<float>(steps);
        float eased_t = EaseOutCubic(t);
        for (int i = 0; i < SERVO_COUNT; i++) {
            if (servo_pins_[i] != -1) {
                float interpolated = start[i] + (target[i] - start[i]) * eased_t;
                servo_[i].SetPosition(static_cast<int>(std::round(interpolated)));
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }

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

void Otto::MoveSingle(int position, int servo_number) {
    if (servo_number < 0 || servo_number >= SERVO_COUNT || servo_pins_[servo_number] == -1) {
        return;
    }

    position = ClampServoTarget(servo_number, position);

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

    servo_[servo_number].SetPosition(position);
}

void Otto::GetServoPositions(int positions[]) {
    for (int i = 0; i < SERVO_COUNT; i++) {
        positions[i] = (servo_pins_[i] != -1) ? servo_[i].GetPosition() : servo_initial_[i];
    }
}

void Otto::OscillateServos(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period,
                           double phase_diff[SERVO_COUNT], float cycle = 1) {
    int center[SERVO_COUNT];
    for (int i = 0; i < SERVO_COUNT; i++) {
        if (servo_pins_[i] != -1) {
            center[i] = ClampServoTarget(i, offset[i]);
            int safe_amplitude = ClampServoAmplitude(i, center[i], amplitude[i]);
            servo_[i].SetO(center[i] - 90);
            servo_[i].SetA(safe_amplitude);
            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);
    }
    for (int i = 0; i < SERVO_COUNT; i++) {
        if (servo_pins_[i] != -1) {
            servo_[i].SetPosition(center[i]);
        }
    }
    vTaskDelay(pdMS_TO_TICKS(10));
}

void Otto::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 = Otto at rest position -------------------------------//
///////////////////////////////////////////////////////////////////
void Otto::Home(bool hands_down) {
    if (is_otto_resting_ == false) {  // Go to rest position only if necessary
        int max_delta = 0;
        for (int i = 0; i < SERVO_COUNT; i++) {
            if (servo_pins_[i] != -1) {
                max_delta = std::max(max_delta, std::abs(servo_initial_[i] - servo_[i].GetPosition()));
            }
        }
        int home_time = std::clamp(500 + max_delta * 9, 500, 1700);
        MoveServos(home_time, servo_initial_);
        is_otto_resting_ = true;
    }

    vTaskDelay(pdMS_TO_TICKS(200));
}

bool Otto::GetRestState() {
    return is_otto_resting_;
}

void Otto::SetRestState(bool state) {
    is_otto_resting_ = state;
}

///////////////////////////////////////////////////////////////////
//-- PREDETERMINED MOTION SEQUENCES -----------------------------//
///////////////////////////////////////////////////////////////////

//---------------------------------------------------------
//-- 统一手部动作函数
//--  Parameters:
//--    action: 动作类型 1=举左手, 2=举右手, 3=举双手, 4=放左手, 5=放右手, 6=放双手,
//--            7=挥左手, 8=挥右手, 9=挥双手, 10=拍打左手, 11=拍打右手, 12=拍打双手
//--    times: 重复次数
//--    amount: 动作幅度 (10-50)
//--    period: 动作时间
//---------------------------------------------------------
void Otto::HandAction(int action, int times, int amount, int period) {
    // 限制参数范围
    times = 2 * std::max(3, std::min(100, times));
    amount = std::max(10, std::min(50, amount));
    period = std::max(100, std::min(1000, period));
    int flap_amount = std::min(amount, 40);
    const int left_flap_center = 40;
    const int right_flap_center = 140;

    int current_positions[SERVO_COUNT];
    for (int i = 0; i < SERVO_COUNT; i++) {
        current_positions[i] = (servo_pins_[i] != -1) ? servo_[i].GetPosition() : servo_initial_[i];
    }

    auto lower_left_hand = [&]() {
        current_positions[LEFT_PITCH] = servo_initial_[LEFT_PITCH];
        current_positions[LEFT_ROLL] = servo_initial_[LEFT_ROLL];
    };
    auto lower_right_hand = [&]() {
        current_positions[RIGHT_PITCH] = servo_initial_[RIGHT_PITCH];
        current_positions[RIGHT_ROLL] = servo_initial_[RIGHT_ROLL];
    };

    switch (action) {
        case 1:  // 举左手
            current_positions[LEFT_PITCH] = 180;
            MoveServos(period, current_positions);
            break;

        case 2:  // 举右手
            current_positions[RIGHT_PITCH] = 0;
            MoveServos(period, current_positions);
            break;

        case 3:  // 举双手
            current_positions[LEFT_PITCH] = 180;
            current_positions[RIGHT_PITCH] = 0;
            MoveServos(period, current_positions);
            break;

        case 4:  // 放左手
            lower_left_hand();
            MoveServos(period, current_positions);
            break;

        case 5:  // 放右手
            lower_right_hand();
            MoveServos(period, current_positions);
            break;

        case 6:  // 放双手
            lower_left_hand();
            lower_right_hand();
            MoveServos(period, current_positions);
            break;

        case 7:  // 挥左手
            current_positions[LEFT_PITCH] = 150;
            MoveServos(period, current_positions);
            for (int i = 0; i < times; i++) {
                current_positions[LEFT_PITCH] = 150 + (i % 2 == 0 ? -30 : 30);
                MoveServos(period / 10, current_positions);
                vTaskDelay(pdMS_TO_TICKS(period / 10));
            }
            current_positions[LEFT_PITCH] = servo_initial_[LEFT_PITCH];
            MoveServos(period, current_positions);
            break;

        case 8:  // 挥右手
            current_positions[RIGHT_PITCH] = 30;
            MoveServos(period, current_positions);
            for (int i = 0; i < times; i++) {
                current_positions[RIGHT_PITCH] = 30 + (i % 2 == 0 ? 30 : -30);
                MoveServos(period / 10, current_positions);
                vTaskDelay(pdMS_TO_TICKS(period / 10));
            }
            current_positions[RIGHT_PITCH] = servo_initial_[RIGHT_PITCH];
            MoveServos(period, current_positions);
            break;

        case 9:  // 挥双手
            current_positions[LEFT_PITCH] = 150;
            current_positions[RIGHT_PITCH] = 30;
            MoveServos(period, current_positions);
            for (int i = 0; i < times; i++) {
                current_positions[LEFT_PITCH] = 150 + (i % 2 == 0 ? -30 : 30);
                current_positions[RIGHT_PITCH] = 30 + (i % 2 == 0 ? 30 : -30);
                MoveServos(period / 10, current_positions);
                vTaskDelay(pdMS_TO_TICKS(period / 10));
            }
            current_positions[LEFT_PITCH] = servo_initial_[LEFT_PITCH];
            current_positions[RIGHT_PITCH] = servo_initial_[RIGHT_PITCH];
            MoveServos(period, current_positions);
            break;

        case 10:  // 拍打左手
            current_positions[LEFT_ROLL] = left_flap_center;
            MoveServos(period, current_positions);
            for (int i = 0; i < times; i++) {
                current_positions[LEFT_ROLL] = left_flap_center - flap_amount;
                MoveServos(period / 10, current_positions);
                current_positions[LEFT_ROLL] = left_flap_center + flap_amount;
                MoveServos(period / 10, current_positions);
            }
            current_positions[LEFT_ROLL] = servo_initial_[LEFT_ROLL];
            MoveServos(period, current_positions);
            break;

        case 11:  // 拍打右手
            current_positions[RIGHT_ROLL] = right_flap_center;
            MoveServos(period, current_positions);
            for (int i = 0; i < times; i++) {
                current_positions[RIGHT_ROLL] = right_flap_center + flap_amount;
                MoveServos(period / 10, current_positions);
                current_positions[RIGHT_ROLL] = right_flap_center - flap_amount;
                MoveServos(period / 10, current_positions);
            }
            current_positions[RIGHT_ROLL] = servo_initial_[RIGHT_ROLL];
            MoveServos(period, current_positions);
            break;

        case 12:  // 拍打双手
            current_positions[LEFT_ROLL] = left_flap_center;
            current_positions[RIGHT_ROLL] = right_flap_center;
            MoveServos(period, current_positions);
            for (int i = 0; i < times; i++) {
                current_positions[LEFT_ROLL] = left_flap_center - flap_amount;
                current_positions[RIGHT_ROLL] = right_flap_center + flap_amount;
                MoveServos(period / 10, current_positions);
                current_positions[LEFT_ROLL] = left_flap_center + flap_amount;
                current_positions[RIGHT_ROLL] = right_flap_center - flap_amount;
                MoveServos(period / 10, current_positions);
            }
            current_positions[LEFT_ROLL] = servo_initial_[LEFT_ROLL];
            current_positions[RIGHT_ROLL] = servo_initial_[RIGHT_ROLL];
            MoveServos(period, current_positions);
            break;
    }
}

//---------------------------------------------------------
//-- 统一身体动作函数
//--  Parameters:
//--    action: 动作类型 1=左转, 2=右转，3=回中心
//--    times: 转动次数
//--    amount: 旋转角度 (0-90度，以90度为中心左右旋转)
//--    period: 动作时间
//---------------------------------------------------------
void Otto::BodyAction(int action, int times, int amount, int period) {
    // 限制参数范围
    times = std::max(1, std::min(10, times));
    amount = std::max(0, std::min(90, amount));
    period = std::max(500, std::min(3000, period));

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

    int body_center = servo_initial_[BODY];
    int target_angle = body_center;

    switch (action) {
        case 1:  // 左转
            target_angle = body_center + amount;
            target_angle = std::min(180, target_angle);
            break;
        case 2:  // 右转
            target_angle = body_center - amount;
            target_angle = std::max(0, target_angle);
            break;
        case 3:  // 回中心
            target_angle = body_center;
            break;
        default:
            return;  // 无效动作
    }

    current_positions[BODY] = target_angle;
    MoveServos(period, current_positions);
    vTaskDelay(pdMS_TO_TICKS(100));
}

//---------------------------------------------------------
//-- 统一头部动作函数
//--  Parameters:
//--    action: 动作类型 1=抬头, 2=低头, 3=点头, 4=回中心, 5=连续点头
//--    times: 重复次数 (仅对连续点头有效)
//--    amount: 角度偏移 (1-15度范围内)
//--    period: 动作时间
//---------------------------------------------------------
void Otto::HeadAction(int action, int times, int amount, int period) {
    // 限制参数范围
    times = std::max(1, std::min(10, times));
    amount = std::max(1, std::min(15, abs(amount)));
    period = std::max(300, std::min(3000, period));

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

    int head_center = 90;  // 头部中心位置

    switch (action) {
        case 1:                                              // 抬头
            current_positions[HEAD] = head_center + amount;  // 抬头是增加角度
            MoveServos(period, current_positions);
            break;

        case 2:                                              // 低头
            current_positions[HEAD] = head_center - amount;  // 低头是减少角度
            MoveServos(period, current_positions);
            break;

        case 3:  // 点头 (上下运动)
            // 先抬头
            current_positions[HEAD] = head_center + amount;
            MoveServos(period / 3, current_positions);
            vTaskDelay(pdMS_TO_TICKS(period / 6));

            // 再低头
            current_positions[HEAD] = head_center - amount;
            MoveServos(period / 3, current_positions);
            vTaskDelay(pdMS_TO_TICKS(period / 6));

            // 回到中心
            current_positions[HEAD] = head_center;
            MoveServos(period / 3, current_positions);
            break;

        case 4:  // 回到中心位置
            current_positions[HEAD] = head_center;
            MoveServos(period, current_positions);
            break;

        case 5:  // 连续点头
            for (int i = 0; i < times; i++) {
                // 抬头
                current_positions[HEAD] = head_center + amount;
                MoveServos(period / 2, current_positions);

                // 低头
                current_positions[HEAD] = head_center - amount;
                MoveServos(period / 2, current_positions);

                vTaskDelay(pdMS_TO_TICKS(50));  // 短暂停顿
            }

            // 回到中心
            current_positions[HEAD] = head_center;
            MoveServos(period / 2, current_positions);
            break;

        default:
            // 无效动作，回到中心
            current_positions[HEAD] = head_center;
            MoveServos(period, current_positions);
            break;
    }
}
