#include <stdint.h>
#include "cm_adc.h"
#include "cm_gpio.h"
#include "cm_iomux.h"
#include "cm_pwm.h"

#include "local_tts.h"
#include "control_out.h"

#define ADC_DAC_DEBUG_ENABLE 1

#if ADC_DAC_DEBUG_ENABLE
#include "app_uart.h"
#define DEBUG(fmt, args...) app_printf("[CONTROL_OUT]" fmt, ##args)
#else
#define DEBUG(fmt, arg...)
#endif

#define PWM0_IOMUX CM_IOMUX_PIN_74, CM_IOMUX_FUNC_FUNCTION1
#define PWM1_IOMUX CM_IOMUX_PIN_75, CM_IOMUX_FUNC_FUNCTION1
#define PWM2_IOMUX CM_IOMUX_PIN_21, CM_IOMUX_FUNC_FUNCTION2
#define PWM3_IOMUX 0, 0

osThreadId_t control_out_TaskHandle = NULL;

SYS_CONF_t sys_conf ={
    .SW_version = 1,
    .HW_version = 1,
    .VER_type = 1,
    .Manager_sLim =100,
    .Tourist_sLim =50,
    .Speed_Cut_sLim =20,
    .Brake_DLimit =1200,
    .Speed_Cut_DLimit =2000,
    .Brake_DLimit_B =1200,
    .Speed_Cut_DLimit_B =2000,
};
sys_sta_t sys_sta;


typedef enum {
    ADC_power = 0,
    ADC_acc = 1,
}adc_chx_t;

int32_t get_voltage(adc_chx_t chx){
    int32_t voltage = 0;
    if(chx == ADC_acc){
        cm_gpio_set_level(CM_GPIO_NUM_20, 0);
    }else if(chx == ADC_power){
        cm_gpio_set_level(CM_GPIO_NUM_20, 1);
    }else{
        return 0;
    }
    osDelay(1);//5ms
	if(0 != cm_adc_read(CM_ADC_0,&voltage)){
		DEBUG("ADC read fail\r\n");
		voltage = 0;
	}
    if(chx == ADC_acc){
        voltage = voltage * ((ACC_RES_UP+ACC_RES_DOWN) / ACC_RES_DOWN); //转化为电压值
        cm_gpio_set_level(CM_GPIO_NUM_20, 1);
    }else if(chx == ADC_power){
        voltage = voltage * ((VIN_RES_UP+VIN_RES_DOWN) / VIN_RES_DOWN); //转化为电压值
        cm_gpio_set_level(CM_GPIO_NUM_20, 0);
    }else{
        return 0;
    }
    // cm_gpio_set_level(CM_GPIO_NUM_20, 1); //恢复默认状态
    return voltage;
}

uint32_t ACC_Dmin =(((ACC_OUT_Voltage_MIN*ACC_OUT_RES_DOWN)/(ACC_OUT_RES_UP+ACC_OUT_RES_DOWN))*(ACC_OUT_PERIOD_US/(1800)));
uint32_t ACC_Dmax =(((ACC_OUT_Voltage_MAX*ACC_OUT_RES_DOWN)/(ACC_OUT_RES_UP+ACC_OUT_RES_DOWN))*(ACC_OUT_PERIOD_US/(1800)));

// 获取输入油门百分比（unit:%)(踏板信号)
uint16_t get_in_acc_percent(void){
    uint16_t acc_voltage =get_voltage(ADC_acc);
    if(acc_voltage < 900){
        return 0;
    }else if(acc_voltage > 3800){
        return 100;
    }else{
        return ((acc_voltage-900)/29);
        // return (100*(acc_voltage - 900)/(3800-900));
    }
}

// 设置输出油门大小
void set_out_acc_percent(uint8_t percent){//1000000设置频率为10KHz
    if(0 != cm_pwm_open_ns(CM_PWM_DEV_0,ACC_OUT_PERIOD_US,(ACC_Dmin+((ACC_Dmax-ACC_Dmin)*(percent))/100))){ 
        DEBUG("pwm0 open error\n");
    }
}


#define _out_UPDETE_TIME 10
static uint32_t back_timecount = 0;
static uint8_t door_lock_last_status = 0;
static uint8_t S_MODE_last_status = 1;


void control_out_task(void *argument){
    cm_gpio_level_e level = 0;
    while(1){
        // DEBUG("ADC_acc:%dmV\r\n",get_voltage(ADC_acc));
        // DEBUG("ADC_power:%dmV\r\n\n",get_voltage(ADC_power));
        // 系统模式状态
        if(0 ==cm_gpio_get_level(IO_MANAGER_MODE, &level)){ // 更新系统模式
            sys_sta.MAG_MODE = level; // 1:游客模式,0:管理模式
            // DEBUG("IO_SYS_MODE:%d\r\n",level);
        }
        // 倒车状态
        if(0 ==cm_gpio_get_level(IO_RX_BACK, &level)){ // 更新倒车模式
            sys_sta.IO_RX_back = level; // 0:倒车模式,1:正常模式
            // DEBUG("IO_RX_back:%d\r\n",level);
        }
        // 刹车状态
        if(1 == sys_sta.A_brake){ // 自动刹车触发
            cm_gpio_set_direction(IO_TX_BRAKE, CM_GPIO_DIRECTION_OUTPUT);
            cm_gpio_set_level(IO_TX_BRAKE, 0); // 打开刹车
            // DEBUG("auto brake\r\n");
        }else{
            cm_gpio_set_direction(IO_TX_BRAKE, CM_GPIO_DIRECTION_INPUT);
            cm_gpio_set_pull(IO_TX_BRAKE, CM_GPIO_PULL_UP); // 关闭刹车
            cm_gpio_set_level(IO_TX_BRAKE, 1);
            // DEBUG("close brake\r\n");
        }
        if(0 ==cm_gpio_get_level(IO_TX_BRAKE, &level)){ // 更新刹车状态
            // if(level != sys_sta.IO_TX_brake && level == 0){
            //     local_tts_text_play("刹车",0,0);
            // }
            sys_sta.IO_TX_brake = level; // 0:刹车模式,1:正常模式
            // DEBUG("IO_TX_brake:%d\r\n",level);
            // if(level == 0){
            //     sys_sta.O_door_lock = 1; // 电门锁打开
            //     sys_sta.PLT_MODE = 1; // 
            // }else{
            //     sys_sta.O_door_lock = 0; // 电门锁打开
            //     sys_sta.PLT_MODE = 1; // 
            // }
        }

        // 动作语音提示
        if((door_lock_last_status != sys_sta.O_door_lock) && (1 == sys_sta.MAG_MODE ) && (1 == sys_sta.PLT_MODE)){ // 游客模式下，电门锁状态变化时触发
            door_lock_last_status = sys_sta.O_door_lock;
            if(1 == sys_sta.O_door_lock){ // 电门锁打开
                // DEBUG("\n\ndoor lock open\r\n\n");
                // local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
                local_tts_text_play("欢迎使用游园猫",0,0);
            }else{ // 电门锁关闭
                // DEBUG("\n\ndoor lock close\r\n\n");
                // local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
                local_tts_text_play("行程结束，祝您生活愉快",0,0);
            }
        }
        if((S_MODE_last_status != sys_sta.MAG_MODE) || ((0 == sys_sta.PLT_MODE)&&(1 == sys_sta.O_door_lock))){ // 管理员模式状态变化时触发
            S_MODE_last_status = sys_sta.MAG_MODE;
            if((0 == sys_sta.MAG_MODE)||(0 == sys_sta.PLT_MODE)){ // 管理模式
                DEBUG("\n\nMAG_MODE\r\n\n");
                // local_tts_set(5, 7, CM_LOCAL_TTS_DIGIT_AUTO);
                local_tts_text_play("管理员模式",0,1);
            }else{ // 退出管理员模式
                DEBUG("\n\nPLT_MODE\r\n\n");
                // local_tts_set(10, 7, CM_LOCAL_TTS_DIGIT_AUTO);
                local_tts_text_play("退出管理员模式",0,1);
            }
        }

        if((sys_sta.IO_RX_back == 0)&&((1 == sys_sta.O_door_lock) || (0 == sys_sta.MAG_MODE ))){ // 倒车模式
            back_timecount++;
            // DEBUG("back_timecount:%d\r\n\n",back_timecount);
            if(back_timecount > _out_UPDETE_TIME*10){ // 3秒播报倒车提示语一次
                back_timecount = 0;
                DEBUG("\n\n>>>>>>>>>>>>back time play.......... \r\n\n");
                // local_tts_set(5, 15, CM_LOCAL_TTS_DIGIT_AUTO);
                local_tts_text_play("请注意倒车！",0,1); // 忙时自动取消倒车提示语
            }
        }

        //油门控制
        if((1 == sys_sta.O_door_lock) || (0 == sys_sta.MAG_MODE )){
            cm_gpio_set_level(OUT_Door_lock, 1); // 打开电门锁
            uint8_t acc_percent =0;
            if((1 == sys_sta.MAG_MODE)&&(1 == sys_sta.PLT_MODE )){ // 游客模式
                if(sys_sta.A_Speed_Cut){ // 自动减速
                    acc_percent = (get_in_acc_percent() * sys_conf.Speed_Cut_sLim) / 100.0;
                }else{
                    acc_percent = (get_in_acc_percent() * sys_conf.Tourist_sLim) / 100.0;
                }
            }else{ // 管理员模式
                acc_percent = (get_in_acc_percent() * sys_conf.Manager_sLim) / 100.0;
            }
            set_out_acc_percent(acc_percent);
            // DEBUG("acc_percent:%d\r\n",acc_percent);
        }else{
            cm_gpio_set_level(OUT_Door_lock, 0); // 关闭电门锁
            set_out_acc_percent(0);
        }

        osDelay(_out_UPDETE_TIME/5);
    }
}

void control_out_init(void){
    cm_gpio_cfg_t cfg = {0};

    // 手动设置初始值
    sys_sta.O_door_lock = 0; // 电门锁状态:关闭
    sys_sta.PLT_MODE = 1; // 模式:游客模式
    sys_sta.P_Radar_EN = 1; // 雷达使能状态:开启
    // 自动设置
    sys_sta.MAG_MODE = 1; // 系统模式:游客模式
    sys_sta.IO_RX_back = 1; // 倒车模式:正常模式
    sys_sta.IO_TX_brake = 1; // 刹车模式:正常模式
    sys_sta.A_brake = 0; // 自动刹车状态:关闭
    sys_sta.A_Speed_Cut = 0; // 自动减速状态:关闭

    // SW_ADC
    cfg.direction = CM_GPIO_DIRECTION_OUTPUT;
    cfg.pull = CM_GPIO_PULL_UP;
    cm_iomux_set_pin_func(CM_IOMUX_PIN_86, CM_IOMUX_FUNC_FUNCTION2);//初始化之前一定要先设置引脚复用
    cm_gpio_init(CM_GPIO_NUM_20, &cfg);
    cm_gpio_set_level(CM_GPIO_NUM_20, 1);

    // 电门锁IO初始化 (输出)
    cfg.direction = CM_GPIO_DIRECTION_OUTPUT;
    cfg.pull = CM_GPIO_PULL_UP;
    cm_iomux_set_pin_func(CM_IOMUX_PIN_87, CM_IOMUX_FUNC_FUNCTION2);
    cm_gpio_init(OUT_Door_lock, &cfg);
    cm_gpio_set_level(OUT_Door_lock, 0);//默认关闭

    // 管理员模式IO初始化 (输入)
    cfg.direction = CM_GPIO_DIRECTION_INPUT;
    cfg.pull = CM_GPIO_PULL_UP;
    cm_iomux_set_pin_func(CM_IOMUX_PIN_76, CM_IOMUX_FUNC_FUNCTION2);
    cm_gpio_init(IO_MANAGER_MODE, &cfg);
    // cm_gpio_set_level(IO_MANAGER_MODE, 0);

    // 倒车IO初始化 (输入)
    cfg.direction = CM_GPIO_DIRECTION_INPUT;
    cfg.pull = CM_GPIO_PULL_UP;
    cm_iomux_set_pin_func(CM_IOMUX_PIN_50, CM_IOMUX_FUNC_FUNCTION2);
    cm_gpio_init(IO_RX_BACK, &cfg);
    // cm_gpio_set_level(IO_RX_BACK, 0);

    // 刹车IO初始化 (输入输出)
    cfg.direction = CM_GPIO_DIRECTION_INPUT;
    cfg.pull = CM_GPIO_PULL_UP;
    cm_iomux_set_pin_func(CM_IOMUX_PIN_51, CM_IOMUX_FUNC_FUNCTION2);
    cm_gpio_init(IO_TX_BRAKE, &cfg);
    // cm_gpio_set_level(IO_TX_BRAKE, 0);
    
    // pwm init
    cm_iomux_set_pin_func(PWM0_IOMUX);
    if(0 != cm_pwm_set_clk(CM_PWM_DEV_0, CM_PWM_CLK_12800K)){//时钟源选择需要放在open之前
        DEBUG("pwm0 set clk fail\r\n");
        return;
    }
    set_out_acc_percent(0); // 设置输出油门大小为0%

    osThreadAttr_t control_out_task_attr = {0};
    control_out_task_attr.name  = "control_out_task";
    control_out_task_attr.stack_size = 4096*4;
    control_out_task_attr.priority = osPriorityNormal;

    control_out_TaskHandle = osThreadNew((osThreadFunc_t)control_out_task,0,&control_out_task_attr);
}