#include "b6x.h"
#include "drvs.h"
#include "dbg.h"
#include "sftmr.h"
#include "sys_config.h"
#include "stdio.h"

#include "app_control_out.h"

#define DBG_CONTROL_EN  1

#if (DBG_CONTROL_EN)
#include "dbg.h"
#define DEBUG(format, ...) debug("[control]" format "\r\n", ##__VA_ARGS__)
#else
#define DEBUG(format, ...)
#define debugHex(dat, len)
#endif


uint16_t ACC_Dmin ;
uint16_t ACC_Dmax ;

// 使能电平
bool app_control_en[PA_MAX]={
[OUT_12V_Control]   =OE_HIGH,   // 12V电源控制输出 /H:12V输出
[OUT_Door_lock]     =OE_HIGH,   // 开锁 /H:开锁(同)
[IN_01]             =OE_LOW,    // 自定义输入
[IN_GPS]            =OE_LOW,    // GPS输入0 /L:低使能/边沿触发
[IO_MANAGER_MODE]   =OE_LOW,    // 管理员模式 /边沿触发
[IO_BACK]           =OE_LOW,    // 倒车 /H:前进/L:后退
[IO_BRAKE]          =OE_LOW,    // 制动 /L:低使能

[IO_TTL_TX]         =OE_LOW,    // 串口发送
[IO_TTL_RX]         =OE_LOW,    // 串口接收
};

sys_sta_t sys_sta;
// ={
//     .Pmode = 0,
//     .Smode = 0,
//     .O_12V = 0,
//     .O_lock = 0,
//     .I_01 = 0,
//     .I_brake = 0,
//     .I_back = 0,
//     .IO_TX = 0,
//     .IO_RX = 0,
//     .A_brake = 0,
//     .A_Speed_Cut = 0,
//     .Reserve = 0,
// };

// 油门控制输入初始化/PA08/CH3
// 电池电压检测/充电检测/PA04/CH7
static void acc_in_bat_in_sadc_init(void)
{
    // Analog Enable
    GPIO_DIR_CLR(GPIO08 | GPIO04 | GPIO02);

    // ADC
    iom_ctrl(PA08, IOM_ANALOG);//ACC//AIN7//油门
    iom_ctrl(PA02, IOM_ANALOG);//ACC_FEEDBACK//AIN2//油门反馈
    iom_ctrl(PA04, IOM_ANALOG);//BAT//AIN3//电池电量

    // sadc init
#if SADC_VREF==VREF_VDD
    sadc_init(SADC_ANA_VREF_VDD);//设置参考电压(3.3V)
#elif SADC_VREF==VREF_1V2
    sadc_init(SADC_ANA_VREF_1V2);//设置参考电压(1.2V)
#elif SADC_VREF==VREF_2V4
    sadc_init(SADC_ANA_VREF_2V4);//设置参考电压(2.4V)
#endif
    
    uint16_t ACC_val=0;
    uint8_t time =0;
    // uint8_t len, dat[20];
    while(1){
        bootDelayMs(300); 
        ACC_val =2*((SADC_VREF * sadc_read(SADC_CH_VDD33, 3))/1024);

        // len =sprintf((char *)dat,"\nVDD:%d\n",ACC_val);
        // uart_send(UART1_PORT,len,dat);
        // uart_send(UART1_PORT,len,dat);

        if(3000 <=ACC_val && 6 > time){ //&& 3500 >= ACC_val){
            break;
        }
        if(20 < time++){
            ACC_val=3300;
            break;
        }
    }
    ACC_ADC_init(ACC_val); //获取VDD33实际电压，纠正DAC输出电压误差
}

// 获取油门输入信号原始ADC值
uint16_t get_in_acc_raw(void){
    return (sadc_read(ACC_IN_CH, 0));
}

// 获取油门输入电压信号（unit:mV)
uint16_t get_in_acc_voltage(void){
    return ((((SADC_VREF * sadc_read(ACC_IN_CH, 3)) / 1024) / ACC_DOWN_Res) * (ACC_UP_Res + ACC_DOWN_Res));
}

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

// 获取电池电压值（unit:V)
float get_bat_voltage(void){
    return (((((SADC_VREF / 1000.0) * sadc_read(BAT_IN_CH, 3))/1024.0)/BAT_DOWN_Res)*(BAT_UP_Res+BAT_DOWN_Res));
}

// 油门调节输出初始化/PWM4/PA05
static void acc_out_pwm_init(void){

    iom_ctrl(PA05, IOM_SEL_TIMER);

    // CTMR
    pwm_init(PWM_CTMR, 15, 1023);//fo =16MHz /((15+1)*(1023+1))
    
    pwm_chnl_cfg_t chnl_conf;
    chnl_conf.duty = 0;
    chnl_conf.ccer = PWM_CCER_SIPH;
    chnl_conf.ccmr = PWM_CCMR_MODE1;
    pwm_chnl_set(PWM_CTMR_CH4, &chnl_conf);

    pwm_start(PWM_CTMR);
}

// 设置输出油门大小
void set_out_acc_percent(uint8_t percent){
    pwm_duty_upd(PWM_CTMR_CH4 , ACC_PERCENT(percent));
}

//中控控制IO初始化
void app_control_init(void){
    // out   
    gpio_dir_output(OUT_12V_Control,OE_LOW);// 12V电源控制输出 /H:12V输出
    gpio_dir_output(OUT_Door_lock,OE_LOW);  // 电门锁 /H:开锁
    gpio_dir_output(IO_MANAGER_MODE,OE_LOW);// 管理员模式 /边沿触发
    gpio_dir_output(IO_BACK,OE_HIGH);       // 倒车 /H:前进/L:后退
    gpio_dir_output(IO_BRAKE,OE_LOW);       // 制动 /L:低使能

    gpio_dir_output(IO_TTL_TX,OE_HIGH);     // 串口发送
    gpio_dir_output(IO_TTL_RX,OE_LOW);      // 串口接收
    //in
    // gpio_dir_input(OUT_12V_Control,IE_DOWN);// 12V电源控制输出 /H:12V输出
    gpio_dir_input(OUT_Door_lock,IE_DOWN);  // 电门锁 /H:开锁
    gpio_dir_input(IO_MANAGER_MODE,IE_DOWN);// 管理员模式 /边沿触发
    gpio_dir_input(IO_BACK,IE_DOWN);       // 倒车 /H:前进/L:后退
    gpio_dir_input(IO_BRAKE,IE_DOWN);       // 制动 /L:低使能

    gpio_dir_input(IO_TTL_TX,IE_DOWN);     // 串口发送
    gpio_dir_input(IO_TTL_RX,IE_DOWN);      // 串口接收
    
    //默认都失能
    Set_Status(OUT_12V_Control,0);
    Set_Status(OUT_Door_lock,0);
    Set_Status(IO_MANAGER_MODE,0);
    Set_Status(IO_BACK,0); //失能倒车
    Set_Status(IO_BRAKE,0);

    Set_Status(IO_TTL_TX,0);
    Set_Status(IO_TTL_RX,0);

    // in
    gpio_dir_input(IN_01,IE_UP);          // 自定义输入
    gpio_dir_input(IN_GPS,IE_UP);         // GPS输入0 /边沿触发

    // // EXTI config
    // exti_init(EXTI_DBC(15, 4));
    // exti_set(EXTI_FTS, EXTI_SRC(IN_GPS) | EXTI_SRC(IO_BACK) | EXTI_SRC(IO_BRAKE) | EXTI_SRC(IO_MANAGER_MODE)); // falling
    // exti_set(EXTI_DBE, EXTI_SRC(IN_GPS) | EXTI_SRC(IO_BACK) | EXTI_SRC(IO_BRAKE) | EXTI_SRC(IO_MANAGER_MODE));
    // exti_set(EXTI_IER, EXTI_SRC(IN_GPS) | EXTI_SRC(IO_BACK) | EXTI_SRC(IO_BRAKE) | EXTI_SRC(IO_MANAGER_MODE));
    
    // IRQ enable
    // NVIC_EnableIRQ(EXTI_IRQn);

    // sys_sta.Pmode = 0;
    sys_sta.Manager =0;
    sys_sta.Tourist =0;
    // sys_sta.Smode = 0;
    sys_sta.IOgps = Get_Status(IN_GPS);
    sys_sta.IOmanager = Get_Status(IO_MANAGER_MODE);

    sys_sta.O_12V = 0;
    sys_sta.O_lock = 0;
    sys_sta.I_01 = Get_Status(IN_01);
    sys_sta.I_brake = Get_Status(IO_BRAKE);
    sys_sta.I_back = Get_Status(IO_BACK);
    sys_sta.IO_TX = Get_Status(IO_TTL_TX);
    sys_sta.IO_RX = Get_Status(IO_TTL_RX);
    sys_sta.A_brake = 0;
    sys_sta.A_Speed_Cut = 0;
    sys_sta.Reserve = 0;
    DEBUG("sys_sta_t:lenght=%d",sizeof(sys_sta));
    DEBUG("sys_sta_t=%#04X",*(uint16_t*)&sys_sta);

    acc_in_bat_in_sadc_init();
    acc_out_pwm_init();

    __enable_irq();
}

// GPIO外部中端处理
void EXTI_IRQHandler(void)
{
    uint32_t irq_sta = EXTI->RIF.Word;

    if (irq_sta & EXTI_SRC(IN_GPS))//
    {
        EXTI->ICR.Word = EXTI_SRC(IN_GPS);
        
    }
    
    if (irq_sta & EXTI_SRC(IO_BACK))
    {
        EXTI->ICR.Word = EXTI_SRC(IO_BACK);
        
    }
    
    if (irq_sta & EXTI_SRC(IO_BRAKE))
    {
        EXTI->ICR.Word = EXTI_SRC(IO_BRAKE);

    }

    if (irq_sta & EXTI_SRC(IO_MANAGER_MODE))
    {
        EXTI->ICR.Word = EXTI_SRC(IO_MANAGER_MODE);

    }
}
 
// // PAD控制进入管理员模式
// bool PAD_Manager_Mode =0;
// // PAD控制进入游客模式
// bool PAD_User_Mode =0;
// // PAD控制LED开关
// bool PAD_LED_ONOFF =0;
// // 系统自动刹车
// bool SYS_AUTO_brake =0;
// // 系统自动减速
// bool SYS_AUTO_Speed_Cut =0;

// uint8_t Get_SYS_Status(void){
//     return (PAD_Manager_Mode << 3) | (PAD_User_Mode << 2) | (PAD_LED_ONOFF << 1) | (SYS_AUTO_brake << 0) | (SYS_AUTO_Speed_Cut << 4);
// }

// 控制进程，定时调用
void Control_procedure(void){

    
    //电门锁
    // sys_sta.Smode = 0;
    sys_sta.IOgps = Get_Status(IN_GPS); //更新GPS状态
    sys_sta.IOmanager = Get_Status(IO_MANAGER_MODE); //更新管理员模式状态
    sys_sta.O_lock = (0 != (sys_sta.Pmode | sys_sta.Smode));
    Set_Status(OUT_Door_lock, sys_sta.O_lock);
    // Set_Status(OUT_Door_lock,(PAD_User_Mode || PAD_Manager_Mode || Get_Status(IN_GPS) || SYS_Manager_STA));
    
    //12V电源
    Set_Status(OUT_12V_Control ,sys_sta.O_12V);// 更新12V电源状态
    // Set_Status(OUT_12V_Control,(PAD_LED_ONOFF));

    // 刹车
    Set_Status(IO_BRAKE ,sys_sta.A_brake);// 输出为低电平时，可刹车
    sys_sta.I_brake = Get_Status(IO_BRAKE); //更新刹车状态// 输出为高电平时，可读取外部刹车状态
    // Set_Status(IO_BRAKE,SYS_AUTO_brake);

    sys_sta.I_01 = Get_Status(IN_01);
    sys_sta.I_back = Get_Status(IO_BACK);
    sys_sta.IO_TX = Get_Status(IO_TTL_TX);
    sys_sta.IO_RX = Get_Status(IO_TTL_RX);

    //油门控制
    if(0 != (sys_sta.Pmode | sys_sta.Smode)){
        uint8_t acc_percent =0;
        if(0x01 == (sys_sta.Pmode | sys_sta.Smode)){//管理员模式
            acc_percent = (get_in_acc_percent() * sys_conf.Manager_sLim) / 100.0;
        }else{//游客模式
            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;
            }
        }
        set_out_acc_percent(acc_percent);
    }else{
        set_out_acc_percent(0);
    }

    // //油门控制
    // if((PAD_User_Mode || PAD_Manager_Mode || Get_Status(IN_GPS) || SYS_Manager_STA)){
    //     uint8_t acc_percent =0;

    //     // acc_percent = get_in_acc_percent() * 
    //     // (((PAD_Manager_Mode || SYS_Manager_STA) ? sys_conf.Manager_sLim : (SYS_AUTO_Speed_Cut ? sys_conf.Speed_Cut_sLim : sys_conf.Tourist_sLim)) / 100.0);
        
    //     if(PAD_Manager_Mode || SYS_Manager_STA){//管理员模式
    //         acc_percent = (get_in_acc_percent() * sys_conf.Manager_sLim) / 100.0;
    //     }else{//游客模式
    //         if(SYS_AUTO_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;
    //         }
    //     }
    //     set_out_acc_percent(acc_percent);
    // }else{
    //     set_out_acc_percent(0);
    // }
}