#include "device_1201.h"
#include "rc_pwm_control.h"

static void device_1201_stop(void)
{
    rc_pwm_stop_all_drive_outputs();
}

static void device_1201_control(int mode, int val)
{
    if (mode == 1) {
        if (val < 50) {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
        } else {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, val / 2 - 10);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
        }
    } else if (mode == 2) {
        if (val < 50) {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
        } else {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, val / 2 - 10);
        }
    } else if (mode == 3) {
        if (val < 45) {
            rc_pwm_set_steering_angle_deg(90);
        } else if (val < 70) {
            rc_pwm_set_steering_angle_deg(50 + val + 7);
        } else {
            rc_pwm_set_steering_angle_deg(135);
        }
    } else if (mode == 4) {
        if (val < 45) {
            rc_pwm_set_steering_angle_deg(90);
        } else if (val < 70) {
            rc_pwm_set_steering_angle_deg(130 - val - 7);
        } else {
            rc_pwm_set_steering_angle_deg(45);
        }
    }
}

static void device_1201_shot(int pin, int val)
{
    (void)pin;
    (void)val;
}

static const device_drive_ops_t ops_s = {
    .name = "1201",
    .stop = device_1201_stop,
    .control = device_1201_control,
    .shot = device_1201_shot,
};

const device_drive_ops_t *device_1201_get_ops(void)
{
    return &ops_s;
}
