#include "SysDiagDetect.h"
#include "FunctionState.h"
#include "OsekCom/OsekCom.h"
#include "RTE.h"
#include "forcedetect.h"
#include "forceSnsr.h"
#include "Speaker.h"
uint8 Touch_Sensor_Fault=0,Touch_Sensor_Fault_key1=0,Touch_Sensor_Fault_key2=0,Touch_Sensor_Fault_key3=0,Touch_Sensor_Fault_pad=0;

static uint8 Bat_Rng = 0;
static void Battery_Volt_Diag_Task(void)
{
    if (Fuction_State != Function_State_A)
    {
        Bat_Rng = Bat_Out_Range;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTLCMNFLTSTS, &Bat_Rng);
    }
    else
    {
        Bat_Rng = Bat_In_Range;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTLCMNFLTSTS, &Bat_Rng);
    }
}

uint8 Force_Sens_Err = 0;
static void Force_Sensor_Diag_Task(void)
{
    uint16 Diag_Press_baseline;
    sint16 Diag_Convert_baseline;
    uint16 CurrentDiag_Press_signal;
    static uint16 LastDiag_Press_signal = 0;
    static uint16 Press_NoChange_Count = 0;

    // basline + - convert
    Diag_Press_baseline = Get_forcedetect_basline_value();
    if ((Diag_Press_baseline >> 11) == 1)
    {
        Diag_Convert_baseline = Diag_Press_baseline - 4096;
    }
    else
    {
        Diag_Convert_baseline = Diag_Press_baseline;
    }

    CurrentDiag_Press_signal = Get_forcedetect_force_value();

    // Force sensor Diag start
    if (forcesnsr_i2c_process_state_u8 == 1)
    {
        Force_Sens_Err = 1;
    }
    else
    {
        Force_Sens_Err = 0;
        if (CurrentDiag_Press_signal == LastDiag_Press_signal)
        {
            Press_NoChange_Count++;
            if (Press_NoChange_Count >= 800) // 4 seconds
            {
                Force_Sens_Err = 1;
                Press_NoChange_Count = 800;
            }
        }
        else
        {
            Press_NoChange_Count = 0;
        }

        if (Diag_Convert_baseline < -2040 || Diag_Convert_baseline > 1000)
        {
            Force_Sens_Err = 1;
        }
    }

    // Force_Sens_Err = 1; //just for test,to set sensor error
    LastDiag_Press_signal = CurrentDiag_Press_signal;
    InitMessage(SIG_DIAGCFAILRTOUCHPANSWTLSNSRFLTSTS, &Force_Sens_Err);
}

// Touch_Sensor_Sts00;

static void Touch_Sensor_Diag_Task(void)
{
    static uint16_t fault_countk1 = 0,fault_countk2 = 0,fault_countk3 = 0, fault_countpad = 0;
    //恢复
    if (Touch_Sensor_delta00 < 200 && Touch_Sensor_delta13 < 200)
    {
        Touch_Sensor_Fault_key1 = Touch_NoFault;
        fault_countk1 = 0;
    }
    else
    {
        fault_countk1++;
        if (fault_countk1 > 40)
        {
            fault_countk1 = 40;
            Touch_Sensor_Fault_key1 = Touch_Out_Range;
        }
        
    }
    //巡航
    if (Touch_Sensor_delta01 < 200 && Touch_Sensor_delta14 < 200)
    {
        Touch_Sensor_Fault_key2 = Touch_NoFault;
        fault_countk2 = 0;
    }
    else
    {
        fault_countk2++;
        if (fault_countk2 > 40)
        {
            fault_countk2 = 40;
            Touch_Sensor_Fault_key2 = Touch_Out_Range;
        }
        
    }
    //取消
    if (Touch_Sensor_delta02 < 200 && Touch_Sensor_delta15 < 200)
    {
        Touch_Sensor_Fault_key3 = Touch_NoFault;
        fault_countk3 = 0;
    }
    else
    {
        fault_countk3++;
        if (fault_countk3 > 40)
        {
            fault_countk3 = 40;
            Touch_Sensor_Fault_key3 = Touch_Out_Range;
        }
        
    }
    //PAD
    if (Touch_Sensor_delta03 < 300 && Touch_Sensor_delta04 < 300 && Touch_Sensor_delta05 < 300 && Touch_Sensor_delta06 < 300 && Touch_Sensor_delta07 < 300 
    && Touch_Sensor_delta08 < 300 && Touch_Sensor_delta09 < 300 && Touch_Sensor_delta10 < 300 && Touch_Sensor_delta11 < 300 && Touch_Sensor_delta12 < 300)
    {
        Touch_Sensor_Fault_pad = Touch_NoFault;
        fault_countpad = 0;
    }
    else
    {
        fault_countpad++;
        if (fault_countpad > 40)
        {
            fault_countpad = 40;
            Touch_Sensor_Fault_pad = Touch_Out_Range;
        }
        
    }
    

    if (Touch_Sensor_Fault_key1 == Touch_NoFault && 
        Touch_Sensor_Fault_key2 == Touch_NoFault && 
        Touch_Sensor_Fault_key3 == Touch_NoFault && 
        Touch_Sensor_Fault_pad == Touch_NoFault )
    {
        Touch_Sensor_Fault = Touch_NoFault;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTLTOUCHDFLTSTS, &Touch_Sensor_Fault);
    }
    else
    {
        Touch_Sensor_Fault = Touch_Out_Range;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTLTOUCHDFLTSTS, &Touch_Sensor_Fault);
    }
}

extern uint8_t Allow_Touch_flag;//todo
static uint8 Vibra_Fault = 0;
static void Vibration_Diag_Task(void)
{
    // AD_data=voltage * 4096/3.3V   normal:0-0.01(15)  short: 0.15(185)-0.36(448)  0.6(742)-0.7(870)  0.95(1177)-1.05(1305) 1.27(1575)-1.39(1727)  open:1.55(1921) - 1.72(2136)

    Vibra_Fault = Get_Speaker_Fault();//
    InitMessage(SIG_DIAGCFAILRTOUCHPANSWTLVIBRATIONFLTSTS, &Vibra_Fault);
}

void Sys_Diag_Detcet_Task(void)
{
    Battery_Volt_Diag_Task();
    if (Bat_Rng == Bat_In_Range)
    {
        Force_Sensor_Diag_Task();
        Touch_Sensor_Diag_Task();
        Vibration_Diag_Task();
    }
}
