#include "arm0502_control.h"
#include "modules_common.h"
#include "common.h"

/* 舵机 ID 0~5：底座、肩、肘、腕、腕摆、夹爪（与产品图一致） */
#define ARM0502_SERVO_COUNT 6
#define ARM0502_SERVO_MODE_BASE 21
#define ARM0502_STEP_PWM 20
#define ARM0502_STEP_PWM_FAST 60
#define ARM0502_STEP_PWM_SERVO5 40
#define ARM0502_STEP_TIME_MS 100

static SerialPort s_arm_serial;
static int s_chassis_fb_mode = 0;   /* 0无/1前进/2后退 */
static int s_chassis_lr_mode = 0;   /* 0无/3左平移/4右平移 */
static int s_servo_pwm[ARM0502_SERVO_COUNT] = {1500, 1500, 1500, 1500, 1500, 1500};

static int arm0502_send_one(int index, int pwm, int time_ms);

static speed_t arm0502_baud_to_termios(int baud_rate)
{
    switch (baud_rate) {
        case 9600: return B9600;
        case 19200: return B19200;
        case 38400: return B38400;
        case 57600: return B57600;
        case 115200: return B115200;
        case 230400: return B230400;
        case 460800: return B460800;
        case 921600: return B921600;
        default: return B115200;
    }
}

static int arm0502_serial_open(SerialPort *sp, const char *port_name, int baud_rate)
{
    if (sp->is_open)
        return 1;

    strncpy(sp->port_name, port_name, sizeof(sp->port_name) - 1);
    sp->port_name[sizeof(sp->port_name) - 1] = '\0';

    sp->fd = open(sp->port_name, O_RDWR | O_NOCTTY | O_NDELAY);
    if (sp->fd == -1) {
        my_zlog_warn("arm0502: open %s failed: %s", sp->port_name, strerror(errno));
        return 0;
    }
    fcntl(sp->fd, F_SETFL, 0);

    struct termios options;
    if (tcgetattr(sp->fd, &options) != 0) {
        close(sp->fd);
        sp->fd = -1;
        return 0;
    }

    speed_t speed = arm0502_baud_to_termios(baud_rate);
    cfsetispeed(&options, speed);
    cfsetospeed(&options, speed);

    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_iflag &= ~(IXON | IXOFF | IXANY);
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 0;

    if (tcsetattr(sp->fd, TCSANOW, &options) != 0) {
        close(sp->fd);
        sp->fd = -1;
        return 0;
    }
    tcflush(sp->fd, TCIOFLUSH);
    sp->is_open = 1;
    my_zlog_info("arm0502: serial %s @ %d ok", sp->port_name, baud_rate);
    return 1;
}

static void arm0502_serial_close(SerialPort *sp)
{
    if (sp->is_open) {
        sp->is_open = 0;
        close(sp->fd);
        sp->fd = -1;
    }
}

static void arm0502_serial_init_struct(SerialPort *sp)
{
    sp->fd = -1;
    sp->is_open = 0;
    memset(sp->port_name, 0, sizeof(sp->port_name));
}

static const char *arm0502_device_path(void)
{
    const char *env_path = getenv("ARM0502_SERIAL");
    if (env_path != NULL && env_path[0] != '\0')
        return env_path;
    /* CH340 常见为 ttyUSB0；若多路 USB 可通过环境变量指定 */
    return "/dev/ttyUSB0";
}

static int arm0502_ensure_serial(void)
{
    if (s_arm_serial.is_open)
        return 0;
    arm0502_serial_init_struct(&s_arm_serial);
    if (!arm0502_serial_open(&s_arm_serial, arm0502_device_path(), 115200))
        return -1;
    return 0;
}

static int arm0502_send_raw(const char *ascii, int len)
{
    if (arm0502_ensure_serial() != 0)
        return -1;
    int n = (int)write(s_arm_serial.fd, ascii, (size_t)len);
    if (n < 0) {
        my_zlog_error("arm0502: write failed: %s", strerror(errno));
        arm0502_serial_close(&s_arm_serial);
        return -1;
    }
    return n;
}

static void arm0502_chassis_send_by_state(void)
{
    const char *cmd = "$TZ!";
    int cmd_len = 4;

    if (s_chassis_fb_mode == 1 && s_chassis_lr_mode == 0) {
        cmd = "$QJ!";
        cmd_len = 4;
    } else if (s_chassis_fb_mode == 2 && s_chassis_lr_mode == 0) {
        cmd = "$HT!";
        cmd_len = 4;
    } else if (s_chassis_fb_mode == 0 && s_chassis_lr_mode == 3) {
        cmd = "$ZPY!";
        cmd_len = 5;
    } else if (s_chassis_fb_mode == 0 && s_chassis_lr_mode == 4) {
        cmd = "$YPY!";
        cmd_len = 5;
    } else if ((s_chassis_fb_mode == 1 && s_chassis_lr_mode == 3) ||
               (s_chassis_fb_mode == 2 && s_chassis_lr_mode == 4)) {
        /* 1+3 左转；2+4 约定为同向转 */
        cmd = "$ZZ!";
        cmd_len = 4;
    } else if ((s_chassis_fb_mode == 1 && s_chassis_lr_mode == 4) ||
               (s_chassis_fb_mode == 2 && s_chassis_lr_mode == 3)) {
        /* 1+4 右转；2+3 约定为同向转 */
        cmd = "$YZ!";
        cmd_len = 4;
    }

    if (arm0502_send_raw(cmd, cmd_len) < 0)
        my_zlog_warn("arm0502: chassis send failed fb=%d lr=%d", s_chassis_fb_mode, s_chassis_lr_mode);
}

static void arm0502_sync_servo_init_all(void)
{
    for (int i = 0; i < ARM0502_SERVO_COUNT; i++) {
        s_servo_pwm[i] = 1500;
        (void)arm0502_send_one(i, s_servo_pwm[i], 500);
        delay_ms(20);
    }
    my_zlog_info("arm0502: servo 0~5 init to 1500");
}

/* 单舵机 #idxPpwmTtime! 共 15 字节 */
static int arm0502_send_one(int index, int pwm, int time_ms)
{
    if (index < 0 || index > 254)
        return -1;
    if (pwm < 500)
        pwm = 500;
    if (pwm > 2500)
        pwm = 2500;
    if (time_ms < 0)
        time_ms = 0;
    if (time_ms > 9999)
        time_ms = 9999;

    char line[20];
    int len = snprintf(line, sizeof(line), "#%03dP%04dT%04d!", index, pwm, time_ms);
    if (len != 15) {
        my_zlog_error("arm0502: protocol pack error len=%d", len);
        return -1;
    }
    return arm0502_send_raw(line, len);
}

static int arm0502_send_link_123(int time_ms)
{
    int t = time_ms;
    if (t < 0)
        t = 0;
    if (t > 9999)
        t = 9999;

    char packet[80];
    int len = snprintf(packet, sizeof(packet),
                       "{#001P%04dT%04d!#002P%04dT%04d!#003P%04dT%04d!}",
                       s_servo_pwm[1], t,
                       s_servo_pwm[2], t,
                       s_servo_pwm[3], t);
    if (len <= 0 || len >= (int)sizeof(packet)) {
        my_zlog_error("arm0502: link123 pack error len=%d", len);
        return -1;
    }
    my_zlog_debug("arm0502 link123 send: %s", packet);
    return arm0502_send_raw(packet, len);
}

void arm0502_serial_stop_all(void)
{
    (void)arm0502_send_raw("$DST!", 5);
}

static void arm0502_stop_one(int servo_id)
{
    char buf[16];
    int len = snprintf(buf, sizeof(buf), "$DST:%d!", servo_id);
    if (len > 0 && len < (int)sizeof(buf))
        (void)arm0502_send_raw(buf, len);
}

void arm0502_control_stop_init(void)
{
    arm0502_serial_init_struct(&s_arm_serial);
    if (arm0502_serial_open(&s_arm_serial, arm0502_device_path(), 115200)) {
        arm0502_sync_servo_init_all();
        my_zlog_info("arm0502: init serial ready");
    } else {
        my_zlog_warn("arm0502: init serial not present (set ARM0502_SERIAL if needed)");
    }
}

void arm0502_control_change(int *buf)
{
    int type_field = buf[0];
    int mode = buf[1];
    int val = buf[2];

    if (mode == 10) {
        s_chassis_fb_mode = 0;
        s_chassis_lr_mode = 0;
        (void)arm0502_send_raw("$TZ!", 4);
        arm0502_serial_stop_all();
        return;
    }
    if (mode == 11) {
        int sid = val;
        if (sid < 0)
            sid = 0;
        if (sid > 9)
            sid = 9;
        arm0502_stop_one(sid);
        return;
    }

    /* 底盘控制新约定：mode=1前/2后/3左移/4右移，可组合 */
    if (mode >= 1 && mode <= 4) {
        if (val < 0) {
            my_zlog_debug("arm0502: chassis ignore negative val=%d", val);
            return;
        }
        if (val == 0) {
            s_chassis_fb_mode = 0;
            s_chassis_lr_mode = 0;
            (void)arm0502_send_raw("$TZ!", 4);
            return;
        }
        if (mode == 1 || mode == 2) {
            s_chassis_fb_mode = mode;
        } else {
            s_chassis_lr_mode = mode;
        }
        arm0502_chassis_send_by_state();
        return;
    }

    /* 舵机控制暂放到后续版本，当前改为 mode 21~26 */
    if (mode < ARM0502_SERVO_MODE_BASE || mode >= ARM0502_SERVO_MODE_BASE + ARM0502_SERVO_COUNT) {
        my_zlog_debug("arm0502: ignore mode=%d val=%d", mode, val);
        return;
    }

    int servo_index = mode - ARM0502_SERVO_MODE_BASE;
    if (val == 0) {
        arm0502_stop_one(servo_index);
        return;
    }
    if (val < 500 || val > 2500) {
        my_zlog_warn("arm0502: pwm out of range, mode=%d val=%d (need 500~2500 or 0 stop)", mode, val);
        return;
    }
    int pwm = val;
    s_servo_pwm[servo_index] = pwm;
    int t = type_field;
    if (t < 1 || t > 9999)
        t = 500;

    if (arm0502_send_one(servo_index, pwm, t) < 0)
        my_zlog_warn("arm0502: send failed (servo=%d pwm=%d t=%d)", servo_index, pwm, t);
}

void arm0502_pwm_value(int pin, int value)
{
    if (pin != 24 && pin != 26 && pin != 5 && pin != 7 && pin != 9 && pin != 10 &&
        pin != 253 && pin != 254)
        return;

    my_zlog_debug("arm0502 pwm recv pin=%d val=%d current[0,1,2,3,4,5]=[%d,%d,%d,%d,%d,%d]",
                  pin, value, s_servo_pwm[0], s_servo_pwm[1], s_servo_pwm[2],
                  s_servo_pwm[3], s_servo_pwm[4], s_servo_pwm[5]);

    if (pin == 5 || pin == 7) {
        if (value == 0) {
            arm0502_stop_one(0);
            my_zlog_debug("arm0502 pwm stop servo0 by val=0");
            return;
        }
        if (value < 0)
            return;

        int delta0 = (pin == 7) ? ARM0502_STEP_PWM_FAST : -ARM0502_STEP_PWM_FAST; /* 7 加，5 减，步进60 */
        int old0 = s_servo_pwm[0];
        int next0 = old0 + delta0;
        if (next0 > 2500)
            next0 = 2500;
        if (next0 < 500)
            next0 = 500;
        s_servo_pwm[0] = next0;

        my_zlog_debug("arm0502 pwm servo0 pin=%d old=%d next=%d", pin, old0, next0);
        if (next0 == old0) {
            my_zlog_debug("arm0502 pwm skip send servo0: reached boundary");
            return;
        }
        if (arm0502_send_one(0, next0, ARM0502_STEP_TIME_MS) < 0)
            my_zlog_warn("arm0502: servo0 send failed pin=%d val=%d", pin, value);
        return;
    }

    if (pin == 9 || pin == 10) {
        if (value == 0) {
            arm0502_stop_one(5);
            my_zlog_debug("arm0502 pwm stop servo5 by val=0");
            return;
        }
        if (value < 0)
            return;

        /* 9: 舵机5从1500往最小700方向；10: 舵机5从1500往最大1800方向 */
        int delta5 = (pin == 10) ? ARM0502_STEP_PWM_SERVO5 : -ARM0502_STEP_PWM_SERVO5;
        int old5 = s_servo_pwm[5];
        int next5 = old5 + delta5;
        if (next5 > 1800)
            next5 = 1800;
        if (next5 < 900)
            next5 = 900;
        s_servo_pwm[5] = next5;

        my_zlog_debug("arm0502 pwm servo5 pin=%d old=%d next=%d", pin, old5, next5);
        if (next5 == old5) {
            my_zlog_debug("arm0502 pwm skip send servo5: reached boundary");
            return;
        }
        if (arm0502_send_one(5, next5, ARM0502_STEP_TIME_MS) < 0)
            my_zlog_warn("arm0502: servo5 send failed pin=%d val=%d", pin, value);
        return;
    }

    if (pin == 253 || pin == 254) {
        if (value == 0) {
            arm0502_stop_one(4);
            my_zlog_debug("arm0502 pwm stop servo4 by val=0");
            return;
        }
        if (value < 0)
            return;

        /* 254 加，253 减；步进 60 */
        int delta4 = (pin == 254) ? ARM0502_STEP_PWM_FAST : -ARM0502_STEP_PWM_FAST;
        int old4 = s_servo_pwm[4];
        int next4 = old4 + delta4;
        if (next4 > 2500)
            next4 = 2500;
        if (next4 < 500)
            next4 = 500;
        s_servo_pwm[4] = next4;

        my_zlog_debug("arm0502 pwm servo4 pin=%d old=%d next=%d", pin, old4, next4);
        if (next4 == old4) {
            my_zlog_debug("arm0502 pwm skip send servo4: reached boundary");
            return;
        }
        if (arm0502_send_one(4, next4, ARM0502_STEP_TIME_MS) < 0)
            my_zlog_warn("arm0502: servo4 send failed pin=%d val=%d", pin, value);
        return;
    }

    if (value == 0) {
        arm0502_stop_one(1);
        arm0502_stop_one(2);
        arm0502_stop_one(3);
        my_zlog_debug("arm0502 pwm stop link123 by val=0");
        return;
    }
    if (value < 0)
        return;

    int delta = (pin == 24) ? ARM0502_STEP_PWM : -ARM0502_STEP_PWM;
    int min_limit = 500;
    int max_limit = 2500;
    if (pin == 24) {
        /* 24 通道：以 1500 为中心，仅允许约 ±200 变化 */
        min_limit = 1300;
        max_limit = 1700;
    } else if (pin == 26) {
        /* 26 通道：以 1500 为中心，仅允许约 ±400 变化 */
        min_limit = 1100;
        max_limit = 1900;
    }
    int changed = 0;
    for (int sid = 1; sid <= 3; sid++) {
        /* 舵机编号2方向与其它舵机相反 */
        int eff_delta = (sid == 2) ? -delta : delta;
        int old_pwm = s_servo_pwm[sid];
        int next = s_servo_pwm[sid] + eff_delta;
        if (next > max_limit)
            next = max_limit;
        if (next < min_limit)
            next = min_limit;
        s_servo_pwm[sid] = next;
        if (next != old_pwm)
            changed = 1;
    }
    my_zlog_debug("arm0502 pwm pin=%d limit[%d,%d] next[1,2,3]=[%d,%d,%d] changed=%d",
                  pin, min_limit, max_limit, s_servo_pwm[1], s_servo_pwm[2], s_servo_pwm[3], changed);
    if (!changed) {
        my_zlog_debug("arm0502 pwm skip send: all servos reached boundary");
        return;
    }
    if (arm0502_send_link_123(ARM0502_STEP_TIME_MS) < 0)
        my_zlog_warn("arm0502: link123 send failed pin=%d val=%d", pin, value);
}

int arm0502_serial_run(void)
{
    int retries = 0;
    while (retries < 20) {
        if (arm0502_ensure_serial() == 0)
            return 0;
        retries++;
        delay_s(1);
    }
    my_zlog_warn("arm0502: serial_run could not open port");
    return -1;
}
