#include "common.h"
#include "gpio_init.h"
#include "ptz0401_control.h"

#define PWM_PIN_up 21
#define PWM_PIN_down 2

// 舵机最小脉冲宽度（ms）
#define MIN_PULSE_WIDTH 0.5
// 舵机最大脉冲宽度（ms）
#define MAX_PULSE_WIDTH 2.5

float Initial_value=90;

static int val_change = 90;
static int val_up_down = 90;
static unsigned char change_index=0;
static unsigned char up_down_index=0;


void pwm_PTZ_hz() {
    int pwm_clock = 24000000 / (50 * 1000);// 定义 PWM 频率为 50Hz
    pinMode(PWM_PIN_up,PWM_OUTPUT);
    pinMode(PWM_PIN_down,PWM_OUTPUT);
    pwmSetClock(PWM_PIN_up,pwm_clock);//=19200*1000/(hz*2000)
    pwmSetRange(PWM_PIN_up,1024);//占空比范围
    pwmSetClock(PWM_PIN_down,pwm_clock);//=19200*1000/(hz*2000)
    pwmSetRange(PWM_PIN_down,1024);//占空比范围
    
}

float calculate_pulse_width(int angle) {
    if (angle < 0) {
        angle = 0;
    } else if (angle > 270) {
        angle = 270;
    }
    return ((MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / 270.0) * angle + MIN_PULSE_WIDTH;
}

int calculate_duty_cycle(float pulse_width) {
    // 周期（ms）
    float period = 1000.0 / 50;
    return (int)((pulse_width / period) * 1024);
}

void PTZ_pwm_init() {
    float pulse_width = calculate_pulse_width(Initial_value);
    int pulse=calculate_duty_cycle(pulse_width);
    pwmWrite(PWM_PIN_up,pulse);
    pwmWrite(PWM_PIN_down,pulse);
}

void PTZ_pwm_left() {
    float pulse_width;
    int pulse; 
    change_index++; 
    if(change_index>=1) {
        change_index=0;
        val_change -= 3;
        if(val_change <= 30)val_change=30; 
        pulse_width = calculate_pulse_width(val_change);
        pulse=calculate_duty_cycle(pulse_width);
    }  
    printf("%d\n",pulse);
    pwmWrite(PWM_PIN_down,pulse);
}

void PTZ_pwm_right() {
    float pulse_width;
    int pulse;
    change_index++;
    if(change_index>=1) {
        change_index=0;
        val_change += 3;
        if(val_change >= 150)val_change=150; 
        pulse_width = calculate_pulse_width(val_change);
        pulse=calculate_duty_cycle(pulse_width);
    } 
    printf("%d\n",pulse);
    pwmWrite(PWM_PIN_down,pulse);
}

void PTZ_pwm_up() {
    float pulse_width;
    int pulse;
    up_down_index++;
    if(up_down_index>=1) {
        up_down_index = 0;
        val_up_down += 3;
        if(val_up_down >= 110) val_up_down=110;
        pulse_width = calculate_pulse_width(val_up_down);
        pulse=calculate_duty_cycle(pulse_width);
    }
    printf("%d\n",pulse); 
    pwmWrite(PWM_PIN_up,pulse);
}

void PTZ_pwm_down() {
    float pulse_width;
    int pulse;
    up_down_index++;
    if(up_down_index>=1) {
        up_down_index = 0;
        val_up_down -= 3;
        if(val_up_down <= 70) val_up_down=70;
        pulse_width = calculate_pulse_width(val_up_down);
        pulse=calculate_duty_cycle(pulse_width);
    }
    printf("%d\n",pulse);
    pwmWrite(PWM_PIN_up,pulse);
}

void PTZ_pwm_change(unsigned char *buf) {
    unsigned char mode=buf[1];
    unsigned char val=buf[2];
    if(val != 0) {
        switch(mode) {
            case 1:			
                PTZ_pwm_up();					 
                break;
            case 2:	
                PTZ_pwm_down();
                break;
            case 3:
                PTZ_pwm_right();
                break;
            case 4:
                PTZ_pwm_left();
                break; 
            default:
                break;
        } 
    }
}