#include "car0106_control.h"
#include "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
};

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

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

void car0106_pwm_init_empty(void) {
    /* 0106 使用软件PWM，不需要物理PWM初始化 */
}

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) {
                car0106_steer_set_duty(CAR0106_STEER_FIXED_DUTY, 0);
            } else {
                car0106_steer_set_duty(0, 0);
            }
            break;
        case 4: /* 右转 */
            if (car0106_clamp(val, 0, CAR0106_STEER_INPUT_MAX) >= CAR0106_STEER_ACTIVE_MIN) {
                car0106_steer_set_duty(0, CAR0106_STEER_FIXED_DUTY);
            } else {
                car0106_steer_set_duty(0, 0);
            }
            break;
        default:
            break;
    }
}
