#include "car0102_control.h"
#include "common.h"
#include "modules_common.h"
#include "gpio_common.h"

// 控制源: 谁在控制硬件
#define CTRL_NONE     0
#define CTRL_PHONE    1
#define CTRL_STEERING 2

static pthread_mutex_t s_car0102_hw_mutex = PTHREAD_MUTEX_INITIALIZER;
static int s_control_source = CTRL_NONE;

// 转向控制变量（方向盘专用）
static int s_target_angle = 90;
static int s_current_angle = 90;
static int s_steering_mode = 0;
static int s_steering_timeout = 0;
static int s_steering_cmd_timeout = 0; // 仅由转向消息续期，避免转向残留

// 速度控制变量（方向盘专用）
static int s_target_speed = 0;
static int s_current_speed = 0;
static int s_speed_mode = 0;       // 0=停止, 1=前进, 2=后退
static int s_speed_timeout = 0;

// ===== 方向盘调参区（只建议改这里4个参数） =====
#define STEER_STEP_TURN    60     // 转向时每次变化角度（越大越快）
#define STEER_STEP_RETURN  90     // 回正时每次变化角度（一般略大于转向）
#define REV_TARGET_SCALE_NUM 6   // 后退目标速度比例: 分子
#define REV_TARGET_SCALE_DEN 10  // 后退目标速度比例: 分母 => 6/10=0.6（慢40%）
#define REV_PWM_GAIN       2     // 后退PWM斜率（越小后退越柔和）

//将角度转化为对应的舵机pwm值
void car0102_calculate_L_R(int angle) {
    if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }
	float pulse_width=(2 / 180.0) * angle + 0.5;
	// 周期（ms）
    float period = 1000.0 / 50;
	int val = (int)((pulse_width / period) * 1000);
	pwmWrite(PWM_PIN_CHANGE,val);
}

void car0102_speed_stop() {
    pthread_mutex_lock(&s_car0102_hw_mutex);
    s_control_source = CTRL_NONE;
    s_target_speed = 0;
    s_current_speed = 0;
    s_speed_mode = 0;
    s_target_angle = 90;
    s_current_angle = 90;
    s_steering_mode = 0;
    s_steering_cmd_timeout = 0;
    s_speed_timeout = 0;
    s_steering_timeout = 0;
    pwmWrite(PWM_PIN_SPEED, 0);
    car0102_calculate_L_R(90);
    pthread_mutex_unlock(&s_car0102_hw_mutex);
}

void car0102_mode_1_flont(int gval) {
    device_gpio_control(g_device_type,26,1);
	if(gval == 0) {
		device_gpio_control(g_device_type,26,0);
		pwmWrite(PWM_PIN_SPEED, 0);
	}else if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 1000);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED, 1000-550);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 1000-550);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, 1000-600);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 1000-600);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, 1000-650);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 1000-650);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 1000-700);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, 1000-700);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, 1000-750);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, 1000-750);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, 1000-800);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, 1000-800);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, 1000-850);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, 1000-900);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, 1000-950);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, 1000-950);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, 1000-1000);
	}
}

void car0102_mode_2_back(int gval) {
	int k = 5;
	int b = 100;
    device_gpio_control(g_device_type,26,0);
    if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 0);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED,b + 40 * k);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED,b + 40 * k);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, b + 40* k);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, b + 45 * k);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, b + 50 * k);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, b + 55 * k);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, b + 60 * k);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, b + 60 * k);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, b + 65 * k);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, b + 70 * k);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, b + 75 * k);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, b + 100 * k);
	}
}

void car0102_mode_3_left(int gval) {
	int b=7;
	if(gval<45){
		car0102_calculate_L_R(90);
	}else if(gval<70){
		car0102_calculate_L_R(50+gval+b);
	}else if(gval>=70){
		car0102_calculate_L_R(135);
	}	
}

void car0102_mode_4_right(int gval) {
	int b=7;
	if(gval<45){
		car0102_calculate_L_R(90);
	}else if(gval<70){
		car0102_calculate_L_R(130-gval-b);
	}else if(gval>=70){
		car0102_calculate_L_R(45);
	}
	      
}

// 手机控制：MQTT线程调用，直接写硬件
void car0102_speed_change(int *buf) {
    int mode = buf[1];
    int val = buf[2];

    pthread_mutex_lock(&s_car0102_hw_mutex);
    s_control_source = CTRL_PHONE;
    s_current_speed = 0;
    s_speed_timeout = 0;
    s_steering_timeout = 0;

    switch(mode) {
        case 1: car0102_mode_1_flont(val); break;
        case 2: car0102_mode_2_back(val); break;
        case 3: car0102_mode_3_left(val); break;
        case 4: car0102_mode_4_right(val); break;
        default: break;
    }
    pthread_mutex_unlock(&s_car0102_hw_mutex);
}

// 方向盘线程：速度平滑处理（前进后退都慢慢加速，后退最大速度慢一点）
static void car0102_speed_smooth_process() {
    if (s_control_source != CTRL_STEERING) {
        return;
    }

    s_speed_timeout--;
    if (s_speed_timeout <= 0) {
        s_current_speed = 0;
        device_gpio_control(g_device_type, 26, 0);
        pwmWrite(PWM_PIN_SPEED, 0);
        if (s_steering_timeout <= 0) {
            s_control_source = CTRL_NONE;
        }
        return;
    }

    // 后退目标速度按比例缩小（默认0.6，即比前进慢40%）
    int limited_target = s_target_speed;
    if (s_speed_mode == 2) {
        limited_target = (limited_target * REV_TARGET_SCALE_NUM) / REV_TARGET_SCALE_DEN;
    }

    // 平滑加速/减速步长（减小使加速更平缓）
    #define ACCEL_STEP 5
    #define DECEL_STEP 6

    if (s_speed_mode == 0) {
        if (s_current_speed > 0) {
            s_current_speed -= DECEL_STEP;
            if (s_current_speed < 0) s_current_speed = 0;
        }
    } else if (s_speed_mode == 1) {
        if (s_current_speed < limited_target) {
            s_current_speed += ACCEL_STEP;
        } else if (s_current_speed > limited_target) {
            s_current_speed -= DECEL_STEP;
        }
    } else if (s_speed_mode == 2) {
        if (s_current_speed < limited_target) {
            s_current_speed += ACCEL_STEP;
        } else if (s_current_speed > limited_target) {
            s_current_speed -= DECEL_STEP;
        }
    }

    if (s_current_speed <= 0) {
        device_gpio_control(g_device_type, 26, 0);
        pwmWrite(PWM_PIN_SPEED, 0);
    } else if (s_speed_mode == 1) {
        device_gpio_control(g_device_type, 26, 1);
        pwmWrite(PWM_PIN_SPEED, 1000 - s_current_speed * 5);
    } else if (s_speed_mode == 2) {
        device_gpio_control(g_device_type, 26, 0);
        pwmWrite(PWM_PIN_SPEED, 100 + s_current_speed * REV_PWM_GAIN);
    }
}

// 方向盘线程：转向角度平滑处理
static void car0102_steering_angle_smooth_process() {
    if (s_control_source != CTRL_STEERING) {
        return;
    }

    s_steering_timeout--;
    if (s_steering_timeout <= 0) {
        if (s_speed_timeout <= 0) {
            s_control_source = CTRL_NONE;
        }
        s_steering_mode = 0;
        s_target_angle = 90;
        return;
    }

    // 只由转向消息续期。若长时间没有转向输入，则自动回中，避免仅前后控制时残留旧方向
    if (s_steering_cmd_timeout > 0) {
        s_steering_cmd_timeout--;
    }
    if (s_steering_cmd_timeout <= 0) {
        s_steering_mode = 0;
        s_target_angle = 90;
    }

    int wanted_angle = 90;
    int input_angle = s_target_angle;
    int smooth_step = STEER_STEP_TURN;

    if (input_angle < 0) input_angle = 0;
    if (input_angle > 180) input_angle = 180;

    if (s_steering_mode == 3) {
        // 左转: 90 -> 130
        wanted_angle = 90 + (input_angle * 37) / 180;
    } else if (s_steering_mode == 4) {
        // 右转: 90 -> 50
        wanted_angle = 90 - (input_angle * 37) / 180;
    } else {
        wanted_angle = 90;
        smooth_step = STEER_STEP_RETURN; // 回正阶段使用更大的步长
    }

    if (wanted_angle > 130) wanted_angle = 130;
    if (wanted_angle < 50) wanted_angle = 50;

    // 平滑逼近目标角度，避免持续MQTT造成抖动
    if (s_current_angle < wanted_angle) {
        s_current_angle += smooth_step;
        if (s_current_angle > wanted_angle) s_current_angle = wanted_angle;
    } else if (s_current_angle > wanted_angle) {
        s_current_angle -= smooth_step;
        if (s_current_angle < wanted_angle) s_current_angle = wanted_angle;
    }

    car0102_calculate_L_R(s_current_angle);
}

// 方向盘线程入口：合并速度+转向平滑，加锁保护
void car0102_steering_smooth_all(void) {
    pthread_mutex_lock(&s_car0102_hw_mutex);
    car0102_speed_smooth_process();
    car0102_steering_angle_smooth_process();
    pthread_mutex_unlock(&s_car0102_hw_mutex);
}

// MQTT接收处理函数（方向盘专用）
void car0102_control_steering_change_process(int *buf) {
    int mode = buf[1];
    int val = buf[2];

    pthread_mutex_lock(&s_car0102_hw_mutex);
    s_control_source = CTRL_STEERING;
    s_steering_timeout = 200;
    s_speed_timeout = 200;

    if (mode == 1) {
        s_speed_mode = 1;
        if (val < 50) {
            // 油门置0：立即停车（不走平滑）
            s_target_speed = 0;
            s_current_speed = 0;
            s_speed_mode = 0;
            device_gpio_control(g_device_type, 26, 0);
            pwmWrite(PWM_PIN_SPEED, 0);
        } else if (val <= 200) {
            s_target_speed = val - 40;
        } else {
            s_target_speed = 160;
        }
    } else if (mode == 2) {
        s_speed_mode = 2;
        if (val < 50) {
            // 刹车/后退置0：立即停车（不走平滑）
            s_target_speed = 0;
            s_current_speed = 0;
            s_speed_mode = 0;
            device_gpio_control(g_device_type, 26, 0);
            pwmWrite(PWM_PIN_SPEED, 0);
        } else if (val <= 200) {
            s_target_speed = (val - 40) * 3 / 4;
        } else {
            s_target_speed = 80;
        }
    } else if (mode == 3) {
        if (val == 0) {
            // 转向置0：立即回正（不走平滑）
            s_steering_mode = 0;
            s_target_angle = 90;
            s_current_angle = 90;
            s_steering_cmd_timeout = 0;
            car0102_calculate_L_R(90);
        } else {
            s_steering_cmd_timeout = 60;
            s_target_angle = val;
            s_steering_mode = 3;
            if (s_target_angle < 0) s_target_angle = 0;
            if (s_target_angle > 180) s_target_angle = 180;
        }
    } else if (mode == 4) {
        if (val == 0) {
            // 转向置0：立即回正（不走平滑）
            s_steering_mode = 0;
            s_target_angle = 90;
            s_current_angle = 90;
            s_steering_cmd_timeout = 0;
            car0102_calculate_L_R(90);
        } else {
            s_steering_cmd_timeout = 60;
            s_target_angle = val;
            s_steering_mode = 4;
            if (s_target_angle < 0) s_target_angle = 0;
            if (s_target_angle > 180) s_target_angle = 180;
        }
    }
    pthread_mutex_unlock(&s_car0102_hw_mutex);
}