#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>

#define CONTROL_PERIOD  0.02f
#define STOP_DEADZONE   0.2f
#define STEP_ANGLE      10.0f
/* 轮盘输入死区：避免 0 附近噪声；每轴独立，全 0 时保持当前角度不回中位 */
#define JOY_INPUT_DEADZONE 0.05f

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 = 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 = 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;

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;
}

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;
}

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");
    }

    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;
}

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 表示立刻停住，但仍保持轮盘模式
    if (s_joy_r <= 0.0f) {
        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 {
            /* 线性角速度：rate = -stick * max_speed * r，与旧版 target 偏移符号一致 */
            float x_eff = (fabsf(s_joy_x) < JOY_INPUT_DEADZONE) ? 0.0f : s_joy_x;
            float y_eff = (fabsf(s_joy_y) < JOY_INPUT_DEADZONE) ? 0.0f : s_joy_y;

            s_pan.speed = 0.0f;
            s_tilt.speed = 0.0f;

            /* 角速度与 joy_pan_max_speed / joy_tilt_max_speed 线性相关；joy_*_accel 供表内单独配置，当前轮盘为直接跟手未做加减速 */
            if (x_eff != 0.0f || y_eff != 0.0f) {
                float pan_rate = -x_eff * s_cfg->joy_pan_max_speed * s_joy_r;
                float tilt_rate = -y_eff * s_cfg->joy_tilt_max_speed * s_joy_r;
                s_pan.angle += pan_rate * CONTROL_PERIOD;
                s_tilt.angle += 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_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(20);
    }
}
