#include "pwm.h"

void PWM_Init_All(void) {
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
    
    HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2);
    
    HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_2);
}

/**
 * @brief 设置占空比
 * @param pulse: 0 到 20000 之间的值
 */
void PWM_Set_Duty(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t pulse) {
    __HAL_TIM_SET_COMPARE(htim, Channel, pulse);
}

// 假设 duty 范围是 -100 到 100
void motor_left_front(int16_t speed) {

    int16_t temp = speed;
	
	if(temp>4200)
		temp = 4200;
	if(temp<-4200)
		temp = -4200;

    if (temp >= 0) {
        PWM_Set_Duty(&htim1, TIM_CHANNEL_1, 4200);
        PWM_Set_Duty(&htim1, TIM_CHANNEL_2, (4200 - temp));
    } else {
        PWM_Set_Duty(&htim1, TIM_CHANNEL_1, (4200 + temp));
        PWM_Set_Duty(&htim1, TIM_CHANNEL_2, 4200);
    }
}

void motor_right_front(int16_t speed) {
    int16_t temp = speed;
	
	if(temp>4200)
		temp = 4200;
	if(temp<-4200)
		temp = -4200;

    if (temp >= 0) {
        PWM_Set_Duty(&htim1, TIM_CHANNEL_3, (4200 - temp));
        PWM_Set_Duty(&htim1, TIM_CHANNEL_4, 4200);
    } else {
        PWM_Set_Duty(&htim1, TIM_CHANNEL_3, 4200);
        PWM_Set_Duty(&htim1, TIM_CHANNEL_4, (4200 + temp));
    }
}

void motor_left_back(int16_t speed) {
    int16_t temp = speed;
	
	if(temp>4200)
		temp = 4200;
	if(temp<-4200)
		temp = -4200;

    if (temp >= 0) {
        PWM_Set_Duty(&htim9, TIM_CHANNEL_1, (4200 - temp));
        PWM_Set_Duty(&htim9, TIM_CHANNEL_2, 4200);
    } else {
        PWM_Set_Duty(&htim9, TIM_CHANNEL_1, 4200);
        PWM_Set_Duty(&htim9, TIM_CHANNEL_2, (4200 + temp));
    }
}

void motor_right_back(int16_t speed) {
    int16_t temp = speed;
	
	if(temp>4200)
		temp = 4200;
	if(temp<-4200)
		temp = -4200;

    if (temp >= 0) {
        PWM_Set_Duty(&htim12, TIM_CHANNEL_1, 4200);
        PWM_Set_Duty(&htim12, TIM_CHANNEL_2, (4200 - temp));
    } else {
        PWM_Set_Duty(&htim12, TIM_CHANNEL_1, (4200 + temp));
        PWM_Set_Duty(&htim12, TIM_CHANNEL_2, 4200);
    }
}


#define MAX_PWM 1000 // 这里的 1000 应改为你实际定时器的 Period (ARR) 值

void pwm_control_territory(uint16_t mode, uint16_t value) {
    // 将 0-100 的速度映射到实际 PWM
    int16_t speed = (int16_t)(value * MAX_PWM / 10);

    
    switch (mode) {
        case 0: // 停止
            motor_left_front(0);  motor_right_front(0);
            motor_left_back(0);   motor_right_back(0);
            break;

        case 1: // 前进
            motor_left_front(speed);  motor_right_front(speed);
            motor_left_back(speed);   motor_right_back(speed);
            break;

        case 2: // 后退
            motor_left_front(-speed); motor_right_front(-speed);
            motor_left_back(-speed);  motor_right_back(-speed);
            break;

        case 3: // 原地左转
            motor_left_front(-speed); motor_right_front(speed);
            motor_left_back(-speed);  motor_right_back(speed);
            break;

        case 4: // 原地右转
            motor_left_front(speed);  motor_right_front(-speed);
            motor_left_back(speed);   motor_right_back(-speed);
            break;

        case 5: // 左平移
            motor_left_front(-speed); motor_right_front(speed);
            motor_left_back(speed);   motor_right_back(-speed);
            break;

        case 6: // 右平移
            motor_left_front(speed);  motor_right_front(-speed);
            motor_left_back(-speed);  motor_right_back(speed);
            break;

        /* --- 新增：斜向运动 --- */

        case 7: // 左上 45° (Forward-Left)
            motor_left_front(0);      motor_right_front(speed);
            motor_left_back(speed);   motor_right_back(0);
            break;

        case 8: // 右上 45° (Forward-Right)
            motor_left_front(speed);  motor_right_front(0);
            motor_left_back(0);       motor_right_back(speed);
            break;

        case 9: // 左下 45° (Backward-Left)
            motor_left_front(-speed); motor_right_front(0);
            motor_left_back(0);       motor_right_back(-speed);
            break;

        case 10: // 右下 45° (Backward-Right)
            motor_left_front(0);      motor_right_front(-speed);
            motor_left_back(-speed);  motor_right_back(0);
            break;

        default:
            motor_left_front(0); motor_right_front(0);
            motor_left_back(0);  motor_right_back(0);
            break;
    }
}

