#include "modules_common.h"
#include "ptz_driver.h"
#include "gpio_common.h"
#include "ptz_common.h"
#include "devcontrol_common.h"
#include <math.h>
#include <stdlib.h>
#include <errno.h>

/* 须与 ptz_driver_task_function 里 delay_ms 一致，否则角速度积分/斜坡会偏快或偏慢 */
#define CONTROL_PERIOD  0.01f
#define STOP_DEADZONE   0.2f
#define STEP_ANGLE      10.0f
/* 轮盘输入死区：略小更易「拖一点就有反应」，过大则像没动 */
#define JOY_INPUT_DEADZONE 0.014f
/* 仅轮盘生效：整体最大角速度比例（调大=最快更猛） */
#define JOY_SPEED_SCALE 0.64f
/* 目标角速度低通：加速跟手 / 减速快停（WebRTC+MQTT 延迟下，松手后慢衰减会「滑很多」） */
#define JOY_RATE_LP_ATTACK 0.34f
#define JOY_RATE_LP_DECAY  0.72f
/* 轮盘角速度斜坡：加速略提高，小目标角速度更快跟上 */
#define JOY_SLEW_ACCEL_SCALE  0.62f
#define JOY_SLEW_DECEL_SCALE  2.6f

static const ptz_config_t s_ptz_configs[] = {
    {
        .device_id = DEVICE_PAO_PTZ0401,
        .pan_max_speed = 120.0f, .pan_accel = 300.0f,
        .tilt_max_speed = 150.0f, .tilt_accel = 700.0f,
        .joy_pan_max_speed = 120.0f, .joy_pan_accel = 300.0f,
        .joy_tilt_max_speed = 150.0f, .joy_tilt_accel = 700.0f,
        .pan_initial = 135.0f, .pan_min = 75.0f, .pan_max = 195.0f,
        .tilt_initial = 90.0f, .tilt_min = 0.0f, .tilt_max = 130.0f,
    },
    {
        .device_id = DEVICE_PAO_PTZ0404,
        .pan_max_speed = 80.0f, .pan_accel = 180.0f,
        .tilt_max_speed = 50.0f, .tilt_accel = 80.0f,
        .joy_pan_max_speed = 160.0f, .joy_pan_accel = 180.0f,
        .joy_tilt_max_speed = 120.0f, .joy_tilt_accel = 80.0f,
        .pan_initial = 135.0f, .pan_min = 90.0f, .pan_max = 180.0f,
        .tilt_initial = 70.0f, .tilt_min = 35.0f, .tilt_max = 90.0f,
    },
    {
        .device_id = DEVICE_PAO_PTZ0405,
        .pan_max_speed = 150.0f, .pan_accel = 180.0f,
        .tilt_max_speed = 100.0f, .tilt_accel = 80.0f,
        .joy_pan_max_speed = 75.0f, .joy_pan_accel = 110.0f,
        .joy_tilt_max_speed = 52.0f, .joy_tilt_accel = 90.0f,
        .pan_initial = 135.0f, .pan_min = 90.0f, .pan_max = 180.0f,
        .tilt_initial = 70.0f, .tilt_min = 10.0f, .tilt_max = 110.0f,
    },
    {
        .device_id = DEVICE_PAO_PTZ0406,
        .pan_max_speed = 150.0f, .pan_accel = 180.0f,
        .tilt_max_speed = 100.0f, .tilt_accel = 80.0f,
        .joy_pan_max_speed = 75.0f, .joy_pan_accel = 110.0f,
        .joy_tilt_max_speed = 52.0f, .joy_tilt_accel = 90.0f,
        .pan_initial = 135.0f, .pan_min = 90.0f, .pan_max = 180.0f,
        .tilt_initial = 70.0f, .tilt_min = 10.0f, .tilt_max = 110.0f,
    },
    { .device_id = -1 }
};

static const ptz_config_t *s_cfg = NULL;
static ServoAxis s_pan;
static ServoAxis s_tilt;
static float s_joy_x = 0.0f;
static float s_joy_y = 0.0f;
static float s_joy_r = 0.0f;
static bool s_joy_active = false;
/* 轮盘模式当前角速度（度/秒），仅 joy 分支使用；经 joy_*_accel 斜坡逼近目标 */
static float s_joy_pan_rate = 0.0f;
static float s_joy_tilt_rate = 0.0f;
/* 轮盘目标角速度低通状态，减轻手机采样抖动、速度阶跃感 */
static float s_joy_pan_tgt_lp = 0.0f;
static float s_joy_tilt_tgt_lp = 0.0f;

static float ptz_clampf(float value, float min_v, float max_v)
{
    if (value < min_v) return min_v;
    if (value > max_v) return max_v;
    return value;
}

/* 过死区后 [|dz|,1] 线性映射到 [-1,1]：拖动量与目标角速度成正比，便于「拖少一点就慢慢动」 */
static float ptz_joy_axis_eff(float raw)
{
    float a = fabsf(raw);
    if (a < JOY_INPUT_DEADZONE) {
        return 0.0f;
    }
    float span = 1.0f - JOY_INPUT_DEADZONE;
    if (span <= 0.0f) {
        return 0.0f;
    }
    float t = (a - JOY_INPUT_DEADZONE) / span;
    if (t > 1.0f) {
        t = 1.0f;
    }
    /* 小行程略抬高有效量，避免「刚过死区×低速×低通」在画面上像完全没动 */
    if (t > 0.0f && t < 0.22f) {
        t = t * 1.28f + 0.012f;
        if (t > 1.0f) {
            t = 1.0f;
        }
    }
    return (raw >= 0.0f) ? t : -t;
}

static bool ptz_parse_float_item(cJSON *obj, const char *key, float *out)
{
    cJSON *item = cJSON_GetObjectItem(obj, key);
    if (item == NULL || out == NULL) {
        return false;
    }

    if (cJSON_IsNumber(item)) {
        *out = (float)item->valuedouble;
        return true;
    }

    if (cJSON_IsString(item) && item->valuestring != NULL) {
        char *endptr = NULL;
        errno = 0;
        float value = strtof(item->valuestring, &endptr);
        if (endptr == item->valuestring || (endptr && *endptr != '\0') || errno == ERANGE) {
            return false;
        }
        *out = value;
        return true;
    }

    return false;
}

static const ptz_config_t *ptz_find_config(int device_id)
{
    for (int i = 0; s_ptz_configs[i].device_id != -1; i++) {
        if (s_ptz_configs[i].device_id == device_id)
            return &s_ptz_configs[i];
    }
    return NULL;
}

/* 角速度向 target_rate 逼近；|目标|比当前小时用更大步长，松手/回拉更快停 */
static float ptz_joy_rate_slew(float current, float target_rate, float max_accel, float dt,
                               float decel_scale)
{
    bool fast_brake = fabsf(target_rate) < fabsf(current) - 0.3f;
    float step = max_accel * dt * (fast_brake ? decel_scale : 1.0f);
    if (current < target_rate) {
        current += step;
        if (current > target_rate) {
            current = target_rate;
        }
    } else if (current > target_rate) {
        current -= step;
        if (current < target_rate) {
            current = target_rate;
        }
    }
    return current;
}

static float ptz_calculate_pulse_width(float angle)
{
    if (angle < 0) angle = 0;
    if (angle > 270) angle = 270;
    return ((MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / 270.0f) * angle + MIN_PULSE_WIDTH;
}

static int ptz_calculate_duty_cycle(float pulse_width)
{
    float period = 1000.0f / SERVO_FREQ;
    return (int)((pulse_width / period) * PWM_RANGE);
}

static void ptz_servo_apply(int pin, float angle)
{
    float pulse_width = ptz_calculate_pulse_width(angle);
    int pulse = ptz_calculate_duty_cycle(pulse_width);
    pwmWrite(pin, pulse);
}

static void ptz_servo_update(ServoAxis *s)
{
    float error = s->target - s->angle;
    float abs_error = fabs(error);

    if (abs_error < STOP_DEADZONE && fabs(s->speed) < (s->accel_cfg * CONTROL_PERIOD)) {
        s->angle = s->target;
        s->speed = 0;
        return;
    }

    float braking_dist = (s->speed * s->speed) / (2.0f * s->accel_cfg);

    float desired_speed;
    if (abs_error <= braking_dist) {
        desired_speed = 0;
    } else {
        desired_speed = (error > 0) ? s->max_speed_cfg : -s->max_speed_cfg;
    }

    float speed_step = s->accel_cfg * CONTROL_PERIOD;

    if (s->speed < desired_speed) {
        s->speed += speed_step;
        if (s->speed > desired_speed && desired_speed >= 0) s->speed = desired_speed;
    } else if (s->speed > desired_speed) {
        s->speed -= speed_step;
        if (s->speed < desired_speed && desired_speed <= 0) s->speed = desired_speed;
    }

    s->angle += s->speed * CONTROL_PERIOD;

    if (s->angle < s->min_limit) {
        s->angle = s->min_limit;
        s->speed = 0;
    } else if (s->angle > s->max_limit) {
        s->angle = s->max_limit;
        s->speed = 0;
    }
}

void ptz_driver_pwm_hz(void)
{
    int pwm_clock = 24000000 / (SERVO_FREQ * 1000);

    pinMode(PWM_PIN_up, PWM_OUTPUT);
    pinMode(PWM_PIN_down, PWM_OUTPUT);

    pwmSetClock(PWM_PIN_up, pwm_clock);
    pwmSetRange(PWM_PIN_up, PWM_RANGE);

    pwmSetClock(PWM_PIN_down, pwm_clock);
    pwmSetRange(PWM_PIN_down, PWM_RANGE);
}

void ptz_driver_init(void)
{
    s_cfg = ptz_find_config(g_device_type);
    if (!s_cfg) {
        my_zlog_error("PTZ config not found for device %d", g_device_type);
        return;
    }

    if (g_device_type == DEVICE_PAO_PTZ0405) {
        pinMode(6, OUTPUT);
        digitalWrite(6, HIGH);
        my_zlog_info("PTZ0405 init: gpio6 set HIGH");
    }
    if (g_device_type == DEVICE_PAO_PTZ0406) {
        pinMode(6, OUTPUT);
        digitalWrite(6, HIGH);
        my_zlog_info("PTZ0406 init: gpio6 set HIGH");
    }

    s_pan = (ServoAxis){
        .angle = s_cfg->pan_initial, .target = s_cfg->pan_initial, .speed = 0.0f,
        .min_limit = s_cfg->pan_min, .max_limit = s_cfg->pan_max,
        .max_speed_cfg = s_cfg->pan_max_speed, .accel_cfg = s_cfg->pan_accel
    };
    s_tilt = (ServoAxis){
        .angle = s_cfg->tilt_initial, .target = s_cfg->tilt_initial, .speed = 0.0f,
        .min_limit = s_cfg->tilt_min, .max_limit = s_cfg->tilt_max,
        .max_speed_cfg = s_cfg->tilt_max_speed, .accel_cfg = s_cfg->tilt_accel
    };

    ptz_driver_pwm_hz();
    ptz_servo_apply(PWM_PIN_down, s_pan.angle);
    ptz_servo_apply(PWM_PIN_up, s_tilt.angle);

    device_ptz_pwm_task_threadpoll_init();
}

void ptz_driver_stop(void)
{
    s_pan.target = s_pan.angle;
    s_pan.speed  = 0.0f;
    s_tilt.target = s_tilt.angle;
    s_tilt.speed = 0.0f;
    s_joy_pan_rate = 0.0f;
    s_joy_tilt_rate = 0.0f;
    s_joy_pan_tgt_lp = 0.0f;
    s_joy_tilt_tgt_lp = 0.0f;
}

void ptz_driver_change(int *buf)
{
    if (!s_cfg) return;

    // 普通命令到达时，强制退出轮盘模式，恢复普通控制
    s_joy_active = false;
    s_pan.max_speed_cfg = s_cfg->pan_max_speed;
    s_tilt.max_speed_cfg = s_cfg->tilt_max_speed;

    if (buf[2] == 0) {
        ptz_driver_stop();
        return;
    }

    my_zlog_info("PTZ(%d) CMD: %d", s_cfg->device_id, buf[1]);

    switch (buf[1]) {
        case 1:
            s_tilt.target -= STEP_ANGLE;
            if (s_tilt.target < s_cfg->tilt_min) s_tilt.target = s_cfg->tilt_min;
            break;
        case 2:
            s_tilt.target += STEP_ANGLE;
            if (s_tilt.target > s_cfg->tilt_max) s_tilt.target = s_cfg->tilt_max;
            break;
        case 3:
            s_pan.target += STEP_ANGLE;
            if (s_pan.target > s_cfg->pan_max) s_pan.target = s_cfg->pan_max;
            break;
        case 4:
            s_pan.target -= STEP_ANGLE;
            if (s_pan.target < s_cfg->pan_min) s_pan.target = s_cfg->pan_min;
            break;
        default:
            break;
    }
}

static void ptz_driver_clear_joystick(void)
{
    s_joy_x = 0.0f;
    s_joy_y = 0.0f;
    s_joy_r = 0.0f;
    s_joy_active = false;
    ptz_driver_stop();
}

void ptz_driver_set_joystick(float x, float y, float r)
{
    s_joy_x = ptz_clampf(x, -1.0f, 1.0f);
    s_joy_y = ptz_clampf(y, -1.0f, 1.0f);
    s_joy_r = ptz_clampf(r, 0.0f, 1.0f);
    s_joy_active = true;

    // 约定：r=0 或 x/y 回中时立刻停住，但仍保持轮盘模式
    if (s_joy_r <= 0.0f ||
        (fabsf(s_joy_x) < JOY_INPUT_DEADZONE && fabsf(s_joy_y) < JOY_INPUT_DEADZONE)) {
        ptz_driver_stop();
    }
}

void ptz_driver_set_joystick_json(cJSON *joystick_ctrl)
{
    float x = 0.0f;
    float y = 0.0f;
    float r = 0.0f;
    bool ok_x = false;
    bool ok_y = false;
    bool ok_r = false;

    if (!cJSON_IsObject(joystick_ctrl)) {
        my_zlog_warn("ptz joystick json invalid");
        return;
    }

    ok_x = ptz_parse_float_item(joystick_ctrl, "x", &x);
    ok_y = ptz_parse_float_item(joystick_ctrl, "y", &y);
    ok_r = ptz_parse_float_item(joystick_ctrl, "r", &r);

    if (!ok_x || !ok_y || !ok_r) {
        my_zlog_warn("ptz joystick parse fail x:%d y:%d r:%d", ok_x, ok_y, ok_r);
        return;
    }

    ptz_driver_set_joystick(x, y, r);
}

static void ptz_driver_task(void)
{
    if (!s_cfg) {
        return;
    }

    bool joy_handled = false;

    if (s_joy_active) {
        joy_handled = true;
        /* r=0：立即保持当前位，不跟到中位 */
        if (s_joy_r <= 0.0f) {
            ptz_driver_stop();
        } else {
            /* 目标角速度 = -stick * max_speed * r；经 joy_*_accel 线性斜坡到目标，再积分到角度 */
            s_pan.speed = 0.0f;
            s_tilt.speed = 0.0f;

            float x_curve = ptz_joy_axis_eff(s_joy_x);
            float y_curve = ptz_joy_axis_eff(s_joy_y);

            if (x_curve == 0.0f && y_curve == 0.0f) {
                ptz_driver_stop();
            } else {
                float pan_target_rate = -x_curve * s_cfg->joy_pan_max_speed * s_joy_r * JOY_SPEED_SCALE;
                float tilt_target_rate = -y_curve * s_cfg->joy_tilt_max_speed * s_joy_r * JOY_SPEED_SCALE;

                float ap = (fabsf(pan_target_rate) < fabsf(s_joy_pan_tgt_lp) - 0.05f)
                               ? JOY_RATE_LP_DECAY
                               : JOY_RATE_LP_ATTACK;
                float at = (fabsf(tilt_target_rate) < fabsf(s_joy_tilt_tgt_lp) - 0.05f)
                               ? JOY_RATE_LP_DECAY
                               : JOY_RATE_LP_ATTACK;
                s_joy_pan_tgt_lp += ap * (pan_target_rate - s_joy_pan_tgt_lp);
                s_joy_tilt_tgt_lp += at * (tilt_target_rate - s_joy_tilt_tgt_lp);

                float a_pan = s_cfg->joy_pan_accel * JOY_SLEW_ACCEL_SCALE;
                float a_tilt = s_cfg->joy_tilt_accel * JOY_SLEW_ACCEL_SCALE;
                s_joy_pan_rate = ptz_joy_rate_slew(s_joy_pan_rate, s_joy_pan_tgt_lp, a_pan, CONTROL_PERIOD,
                                                   JOY_SLEW_DECEL_SCALE);
                s_joy_tilt_rate = ptz_joy_rate_slew(s_joy_tilt_rate, s_joy_tilt_tgt_lp, a_tilt, CONTROL_PERIOD,
                                                    JOY_SLEW_DECEL_SCALE);

                s_pan.angle += s_joy_pan_rate * CONTROL_PERIOD;
                s_tilt.angle += s_joy_tilt_rate * CONTROL_PERIOD;
                s_pan.angle = ptz_clampf(s_pan.angle, s_cfg->pan_min, s_cfg->pan_max);
                s_tilt.angle = ptz_clampf(s_tilt.angle, s_cfg->tilt_min, s_cfg->tilt_max);

                s_pan.target = s_pan.angle;
                s_tilt.target = s_tilt.angle;
            }
        }
    }

    if (!joy_handled) {
        s_joy_pan_rate = 0.0f;
        s_joy_tilt_rate = 0.0f;
        s_joy_pan_tgt_lp = 0.0f;
        s_joy_tilt_tgt_lp = 0.0f;
        s_pan.max_speed_cfg = s_cfg->pan_max_speed;
        s_tilt.max_speed_cfg = s_cfg->tilt_max_speed;
        ptz_servo_update(&s_pan);
        ptz_servo_update(&s_tilt);
    }

    ptz_servo_apply(PWM_PIN_down, s_pan.angle);
    ptz_servo_apply(PWM_PIN_up, s_tilt.angle);
}

void ptz_driver_task_function(void *arg)
{
    my_zlog_info("PTZ driver task start (device %d)", g_device_type);
    while (1) {
        ptz_driver_task();
        delay_ms(10);
    }
}
