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

uint16_t ACC_Dmin ;
uint16_t ACC_Dmax ;

//管理员模式速度极限
uint8_t M_mode_sLim =M_DEFAULT_sLim;
//游客模式速度极限
uint8_t U_mode_sLim =U_DEFAULT_sLim;

// 使能电平
bool app_control_en[PA_MAX]={
    [IN_GPS]            =OE_LOW,    // GPS输入0 /边沿触发/L:低使能(反)
    [IN_Back]           =OE_LOW,    // 倒车 /边沿触发
    [IN_Brake]          =OE_LOW,    // 制动 /边沿触发
    [IN_Manager_Mode]   =OE_LOW,    // 管理员模式 /边沿触发

    [OUT_Buzzer]        =OE_HIGH,   // 蜂鸣器 /H:高使能
    [OUT_Low_speed]     =OE_HIGH,   // 使能低速档位 /L:低使能(反)
    [OUT_Back_car]      =OE_HIGH,   // 使能倒车 /L:低使能倒车(反)
    [OUT_Low_brake]     =OE_HIGH,   // 使能低制动 /L:低使能(反)
    [OUT_High_brake]    =OE_LOW,    // 使能高制动 /H:高使能(反)    
    [OUT_Door_lock]     =OE_HIGH,   // 开锁 /H:开锁(同)
};


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

    // ADC
    iom_ctrl(PA04, IOM_ANALOG);//BAT//AIN3//电池电量
    iom_ctrl(PA08, IOM_ANALOG);//ACC//AIN7//油门

    // 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(SADC_CH_AIN7, 0));
}

// 获取油门输入电压信号（unit:mV)
uint16_t get_in_acc_voltage(void){
    return ((((SADC_VREF * sadc_read(SADC_CH_AIN7, 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(SADC_CH_AIN3, 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_Door_lock,OE_LOW);// 默认锁车
    gpio_dir_output(OUT_Low_speed,OE_LOW);// 低速档位，默认失能
    gpio_dir_output(OUT_Back_car,OE_LOW);// 默认前进模式
    gpio_dir_output(OUT_Low_brake,OE_LOW);// 默认关低制动
    gpio_dir_output(OUT_High_brake,OE_HIGH);// 默认关高制动
    gpio_dir_output(OUT_Buzzer,OE_LOW);// 默认关蜂鸣器
    //in
    gpio_dir_input(OUT_Door_lock,IE_DOWN);// 默认锁车
    gpio_dir_input(OUT_Low_speed,IE_DOWN);// 低速档位，默认失能
    gpio_dir_input(OUT_Back_car,IE_DOWN);// 默认前进模式
    gpio_dir_input(OUT_Low_brake,IE_DOWN);// 默认关低制动
    gpio_dir_input(OUT_High_brake,IE_UP);// 默认关高制动
    gpio_dir_input(OUT_Buzzer,IE_DOWN);// 默认关蜂鸣器
    
    //默认都失能
    Set_Status(OUT_Door_lock,0);
    Set_Status(OUT_Low_speed,0);
    Set_Status(OUT_Back_car,0); //失能倒车
    Set_Status(OUT_Low_brake,0);
    Set_Status(OUT_High_brake,0);
    Set_Status(OUT_Buzzer,0);

    // in
    gpio_dir_input(IN_GPS, IE_UP);  // Input enable, pull-up
    gpio_dir_input(IN_Back, IE_UP);    
    gpio_dir_input(IN_Brake, IE_UP);
    gpio_dir_input(IN_Manager_Mode, IE_UP);

    // // EXTI config
    // exti_init(EXTI_DBC(15, 4));
    // exti_set(EXTI_FTS, EXTI_SRC(IN_GPS) | EXTI_SRC(IN_Back) | EXTI_SRC(IN_Brake) | EXTI_SRC(IN_Manager_Mode)); // falling
    // exti_set(EXTI_DBE, EXTI_SRC(IN_GPS) | EXTI_SRC(IN_Back) | EXTI_SRC(IN_Brake) | EXTI_SRC(IN_Manager_Mode));
    // exti_set(EXTI_IER, EXTI_SRC(IN_GPS) | EXTI_SRC(IN_Back) | EXTI_SRC(IN_Brake) | EXTI_SRC(IN_Manager_Mode));
    
    // IRQ enable
    // NVIC_EnableIRQ(EXTI_IRQn);

    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(IN_Back))
    {
        EXTI->ICR.Word = EXTI_SRC(IN_Back);
        
    }
    
    if (irq_sta & EXTI_SRC(IN_Brake))
    {
        EXTI->ICR.Word = EXTI_SRC(IN_Brake);

    }

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

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

// 控制进程，定时调用
void Control_procedure(void){
   
    //电门锁
    Set_Status(OUT_Door_lock,(PAD_User_Mode || PAD_Manager_Mode || Get_Status(IN_GPS) || Get_Status(IN_Manager_Mode)));

    // 倒车
    Set_Status(OUT_Back_car,Get_Status(IN_Back));
    // 刹车
    Set_Status(OUT_Low_brake,(Get_Status(IN_Brake) || SYS_AUTO_brake));
    Set_Status(OUT_High_brake,(Get_Status(IN_Brake) || SYS_AUTO_brake));

    //油门控制
    if(Get_Status(OUT_Door_lock)){
        set_out_acc_percent((SYS_AUTO_Speed_Cut?0.5:1)*get_in_acc_percent()*((PAD_Manager_Mode || Get_Status(IN_Manager_Mode)) ? sys_conf_info.M_mode_sLim : sys_conf_info.U_mode_sLim) / 100.0);
    }else{
        set_out_acc_percent(0);
    }
}