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

#define CONTROL_PERIOD  0.02f
#define STOP_DEADZONE   0.2f
#define STEP_ANGLE      10.0f

static const ptz_config_t s_ptz_configs[] = {
    {
        .device_id     = DEVICE_PAO_PTZ0401,
        .pan_max_speed = 120.0f, .pan_accel = 300.0f,
        .pan_initial   = 135.0f, .pan_min   = 75.0f,  .pan_max  = 195.0f,
        .tilt_max_speed= 150.0f, .tilt_accel= 700.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,
        .pan_initial   = 135.0f, .pan_min   = 90.0f,  .pan_max  = 180.0f,
        .tilt_max_speed= 50.0f,  .tilt_accel= 80.0f,
        .tilt_initial  = 70.0f,  .tilt_min  = 35.0f,  .tilt_max = 90.0f
    },
    {
        .device_id     = DEVICE_PAO_PTZ0405,
        .pan_max_speed = 80.0f,  .pan_accel = 180.0f,
        .pan_initial   = 135.0f, .pan_min   = 90.0f,  .pan_max  = 180.0f,
        .tilt_max_speed= 50.0f,  .tilt_accel= 80.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 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;
    }

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

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

    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_task(void)
{
    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);
    }
}
