#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_key=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_DIAGCFAILRTOUCHPANSWTRCMNFLTSTS, &Bat_Rng);
    }
    else
    {
        Bat_Rng = Bat_In_Range;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTRCMNFLTSTS, &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_DIAGCFAILRTOUCHPANSWTRSNSRFLTSTS, &Force_Sens_Err);
}

// Touch_Sensor_Sts00;

static void Touch_Sensor_Diag_Task(void)//5ms
{
    static uint16_t fault_count1 = 0,fault_count2 = 0;
    if (Touch_Sensor_delta00 < 200 && Touch_Sensor_delta01 < 200 && Touch_Sensor_delta02 < 200 && Touch_Sensor_delta03 < 200 && Touch_Sensor_delta04 < 200)
    {
        Touch_Sensor_Fault_key = Touch_NoFault;
        fault_count1 = 0;
    }
    else
    {
        fault_count1++;
        if (fault_count1 > 40)
        {
            fault_count1 = 40;
        }
        Touch_Sensor_Fault_key = Touch_Out_Range;
    }
    if (Touch_Sensor_delta05 < 300 && Touch_Sensor_delta06 < 300 && Touch_Sensor_delta07 < 300 && Touch_Sensor_delta08 < 300 && Touch_Sensor_delta09 < 300 &&
        Touch_Sensor_delta10 < 250 && Touch_Sensor_delta11 < 250 && Touch_Sensor_delta12 < 250 && Touch_Sensor_delta13 < 250 && Touch_Sensor_delta14 < 250)
    {
        Touch_Sensor_Fault_pad = Touch_NoFault;
        fault_count2 = 0;
    }
    else
    {
        fault_count2++;
        if (fault_count2 > 40)
        {
            fault_count2 = 40;
        }
        Touch_Sensor_Fault_pad = Touch_Out_Range;
    }
    
    if ( Touch_Sensor_Fault_pad == Touch_NoFault && Touch_Sensor_Fault_key == Touch_NoFault)
    {
        Touch_Sensor_Fault = Touch_NoFault;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTRTOUCHDFLTSTS, &Touch_Sensor_Fault);
    }
    else
    {
        Touch_Sensor_Fault = Touch_Out_Range;
        InitMessage(SIG_DIAGCFAILRTOUCHPANSWTRTOUCHDFLTSTS, &Touch_Sensor_Fault);
    }

}
extern uint8_t Allow_Touch_flag;
static uint8 Vibra_Fault = 0;
static void Vibration_Diag_Task(void)
{
    Vibra_Fault = Get_Speaker_Fault();//Allow_Touch_flag;
    InitMessage(SIG_DIAGCFAILRTOUCHPANSWTRVIBRATIONFLTSTS, &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();
    }
}
