#include "car0106_control.h"
#include "common.h"
#include "gpio_common.h"

enum {
    CAR0106_PWM_FORWARD_PIN = 7,
    CAR0106_PWM_BACK_PIN = 5,
    CAR0106_PWM_LEFT_PIN = 24,
    CAR0106_PWM_RIGHT_PIN = 26,
    /* 前后：val>50 才驱动；刚过阈值起步约 20%，随 val→200 升到 60% */
    CAR0106_ACTIVE_THRESHOLD = 50,
    CAR0106_MAX_INPUT_SPEED = 200,
    CAR0106_DRIVE_MIN_DUTY = 20,
    CAR0106_DRIVE_MAX_DUTY = 60,
    /* 左右：val>=42 开始转向，输入上限 62（再大的包按 62 处理），PWM 固定 60% */
    CAR0106_STEER_ACTIVE_MIN = 42,
    CAR0106_STEER_INPUT_MAX = 62,
    CAR0106_STEER_FIXED_DUTY = 60,
    CAR0106_SERVO_CENTER_ANGLE = 90,
    CAR0106_SERVO_MAX_DELTA_ANGLE = 40,
    CAR0106_SERVO_MIN_ANGLE = CAR0106_SERVO_CENTER_ANGLE - CAR0106_SERVO_MAX_DELTA_ANGLE,
    CAR0106_SERVO_MAX_ANGLE = CAR0106_SERVO_CENTER_ANGLE + CAR0106_SERVO_MAX_DELTA_ANGLE
};

static int car0106_clamp(int value, int min, int max) {
    if (value < min) {
        return min;
    }
    if (value > max) {
        return max;
    }
    return value;
}

static int car0106_speed_to_duty(int val) {
    int limited = car0106_clamp(val, 0, CAR0106_MAX_INPUT_SPEED);
    const int first_step = CAR0106_ACTIVE_THRESHOLD + 1; /* 51 -> 30% */

    if (limited <= CAR0106_ACTIVE_THRESHOLD) return 0;
    if (limited >= CAR0106_MAX_INPUT_SPEED) return CAR0106_DRIVE_MAX_DUTY;

    return CAR0106_DRIVE_MIN_DUTY +
           (limited - first_step) *
               (CAR0106_DRIVE_MAX_DUTY - CAR0106_DRIVE_MIN_DUTY) /
               (CAR0106_MAX_INPUT_SPEED - first_step);
}

static void car0106_set_soft_pwm(int pin, int duty) {
    int limited = car0106_clamp(duty, 0, 100);

    if (limited > 0) {
        digitalWrite(pin, HIGH);
        softPwmWrite(pin, limited);
    } else {
        softPwmWrite(pin, 0);
        digitalWrite(pin, LOW);
    }
}

static void car0106_drive_set_duty(int forward_duty, int back_duty) {
    car0106_set_soft_pwm(CAR0106_PWM_FORWARD_PIN, forward_duty);
    car0106_set_soft_pwm(CAR0106_PWM_BACK_PIN, back_duty);
}

static void car0106_steer_set_duty(int left_duty, int right_duty) {
    car0106_set_soft_pwm(CAR0106_PWM_LEFT_PIN, left_duty);
    car0106_set_soft_pwm(CAR0106_PWM_RIGHT_PIN, right_duty);
}

/* 与0102一致：将角度转化为舵机 PWM 占空值（50Hz, range 1000） */
static void car0106_servo_set_angle(int angle) {
    float pulse_width;
    float period;
    int pwm_value;
    int limited_angle = car0106_clamp(angle, 0, 180);

    pulse_width = (2 / 180.0F) * limited_angle + 0.5F;
    period = 1000.0F / 50.0F;
    pwm_value = (int)((pulse_width / period) * 1000.0F);
    pwmWrite(PWM_PIN_CHANGE, pwm_value);
}

static int car0106_steer_value_to_angle_delta(int val) {
    int limited = car0106_clamp(val, 0, CAR0106_STEER_INPUT_MAX);

    if (limited < CAR0106_STEER_ACTIVE_MIN) {
        return 0;
    }

    if (CAR0106_STEER_INPUT_MAX == CAR0106_STEER_ACTIVE_MIN) {
        return CAR0106_SERVO_MAX_DELTA_ANGLE;
    }

    return (limited - CAR0106_STEER_ACTIVE_MIN) * CAR0106_SERVO_MAX_DELTA_ANGLE /
           (CAR0106_STEER_INPUT_MAX - CAR0106_STEER_ACTIVE_MIN);
}

void car0106_stop(void) {
    car0106_drive_set_duty(0, 0);
    car0106_steer_set_duty(0, 0);
    car0106_servo_set_angle(CAR0106_SERVO_CENTER_ANGLE);
}

void car0106_pwm_init_empty(void) {
    int pwm_clock = 24000000 / (50 * 1000);

    /* 舵机初始化与0102一致：50Hz + range 1000 */
    pinMode(PWM_PIN_CHANGE, PWM_OUTPUT);
    pwmSetClock(PWM_PIN_CHANGE, pwm_clock);
    pwmSetRange(PWM_PIN_CHANGE, 1000);
    car0106_servo_set_angle(CAR0106_SERVO_CENTER_ANGLE);
}

void car0106_control_change(int *buf) {
    int mode;
    int val;
    int duty;

    if (buf == NULL) {
        return;
    }

    mode = buf[1];
    val = buf[2];
    duty = car0106_speed_to_duty(val);

    switch (mode) {
        case 1: /* 前进 */
            car0106_drive_set_duty(duty, 0);
            break;
        case 2: /* 后退 */
            car0106_drive_set_duty(0, duty);
            break;
        case 3: /* 左转：仅左右逻辑单独阈值与上限 */
            if (car0106_clamp(val, 0, CAR0106_STEER_INPUT_MAX) >= CAR0106_STEER_ACTIVE_MIN) {
                int delta_angle = car0106_steer_value_to_angle_delta(val);
                car0106_steer_set_duty(CAR0106_STEER_FIXED_DUTY, 0);
                car0106_servo_set_angle(
                    car0106_clamp(CAR0106_SERVO_CENTER_ANGLE + delta_angle,
                                  CAR0106_SERVO_MIN_ANGLE,
                                  CAR0106_SERVO_MAX_ANGLE));
            } else {
                car0106_steer_set_duty(0, 0);
                car0106_servo_set_angle(CAR0106_SERVO_CENTER_ANGLE);
            }
            break;
        case 4: /* 右转 */
            if (car0106_clamp(val, 0, CAR0106_STEER_INPUT_MAX) >= CAR0106_STEER_ACTIVE_MIN) {
                int delta_angle = car0106_steer_value_to_angle_delta(val);
                car0106_steer_set_duty(0, CAR0106_STEER_FIXED_DUTY);
                car0106_servo_set_angle(
                    car0106_clamp(CAR0106_SERVO_CENTER_ANGLE - delta_angle,
                                  CAR0106_SERVO_MIN_ANGLE,
                                  CAR0106_SERVO_MAX_ANGLE));
            } else {
                car0106_steer_set_duty(0, 0);
                car0106_servo_set_angle(CAR0106_SERVO_CENTER_ANGLE);
            }
            break;
        default:
            break;
    }
}
