#include "uds_config.h"
#include "uds_nvm.h"
#include "BSP_CAN.h"
#include "crc.h"
#if BOOTLOADER_CODE_FLAG == 0
#include "can.h"
#include "Car_CANIDInfo.h"

const uint8_t SWFingerPrint[12] __attribute__((at(APP_FlagAddress))) = 
    {'S','5','0','8','_','D','C','U','2','4','0','1'};
#else

#endif


//INT32U g_ulRand=0;
DIDs gDIDCache;


//uint8_t g_testerprogramdateflag = false;
//uint8_t g_testerserialnubnerflag = false;

//extern INT8U strMcuAppst[4];
//extern INT8U strMcuAppDate[4];
extern INT32U ParseCanRxCnt;
extern uint32_t FBL_AppSize;


//_HSCANERROR HsCanError;
uint32_t g_ulCanStatus;

VehicleInfo2UdsType vehicleInfo2uds;
UdsInfo2VehicleType udsInfo2Vehicle = {
    .NcmRxEnable = true,
    .NcmTxEnable = true,
    .NmcmRxEnable = true,
    .NmcmTxEnable = true,
    .UdsOffLine = true,
};


uint16_t diagStart_InitTimer = DIAG_START_INIT_TIME;
uint16_t diagStart_Timer = DIAG_START_TIME;
DTC_STRUCT dtcComm;
DTC_StatusType dtcStatusBak;
DTC_Fault_Flags faultInfo;       //故障信息
DTC_Options dtcOptions;
const uint32_t DTC_LIST_TABLE[DTC_ID_NUM] = {
    DTC_FOREACH(GENERATE_DTC_VALUE_ARRAY)
};
uint8_t dtcsnapshotNumberTotal;
uint16_t dtcSnapshotDataBuf[DTC_ID_NUM];
static uint16_t BMS_CommLossTimer = 0;
static uint16_t VCU_CommLossTimer = 0;

void UdsDidUpdateVehicle(void)
{
    udsInfo2Vehicle.ChargeTempProtVal = gDIDCache.DID_DCUVariousTempProtectVal[0];
    udsInfo2Vehicle.BoardTempProtVal = gDIDCache.DID_DCUVariousTempProtectVal[1];
    udsInfo2Vehicle.DCVoltProtVal = WORD(gDIDCache.DID_DCVoltProtectVal[0], gDIDCache.DID_DCVoltProtectVal[1]);
    udsInfo2Vehicle.InsulDetVoltVal = WORD(gDIDCache.DID_InsulationDectVolt[0], gDIDCache.DID_InsulationDectVolt[1]);
}

void UdsDtcUpdateVehicle(void)
{
    uint8_t index;
    uint8_t num, list[DTC_ID_NUM];
    static DTC_Fault_Flags fflag = {.w = 0};

    if (fflag.w != faultInfo.w) {
        fflag = faultInfo;
        num = 0;
        for (index = 0; index < DTC_ID_NUM; index++) {
            if (dtcComm.DTCStatus.recordData[index].status == ((1 << DTC_STATUS_Test_Failed) | (1 << DTC_STATUS_Confirmed_DTC))) {
                list[num++] = index + 1;
            }
        }
        udsInfo2Vehicle.DTCNum = num;
        memset(udsInfo2Vehicle.DTCList, 0, DTC_ID_NUM);
        memcpy(udsInfo2Vehicle.DTCList, list, num);
    }
}

void DID_SignalInit(void)
{
    uint8_t i;
    uint8_t index;
    uint16_t size;
    uint8_t *cachePtr, *didPtr;
    uint8_t *nvmDidPtr;
    DIDS_NvmType *dids;
    Param_NvmType *param;

    EE_Read(NVM_IMAGE_DID_ADDR, (uint8_t **)&dids);
    cachePtr = (uint8_t *)&gDIDCache;

    for (i = 0; i < DID_StoreInFlash_NUM; i++) {
        index = GetDIDIndexByID(DIDStoreInFlashId[i]);

        if (index != 0xff) {
            didPtr = cachePtr + DIDOffset[index];
            size = DIDStoreInFlashSize[i];
            nvmDidPtr = (uint8_t *)dids + DIDStoreInFlashOffset[i];

            if (CRC16_DataCheck(nvmDidPtr, size) == 0) {
                memcpy(didPtr, nvmDidPtr, size);
            }
        }
    }
    UdsDidUpdateVehicle();

    EE_Read(NVM_IMAGE_PARAM_ADDR, (uint8_t **)&param);
    if (CRC16_DataCheck((uint8_t *)param, sizeof(Param_NvmType)-2) == 0) {
        UdsParam.SA_FailureCounter = param->Param.SA_FailureCounter;
        UdsService.KeyErrCnt = UdsParam.SA_FailureCounter;
        if (UDS_27SERVICE_ERR_NUM_MAX <= UdsService.KeyErrCnt) {
            UdsService.KeyErrLockTimer = UDS_27SERVICE_KEY_ERRLOCK_TIME;
	        UdsService.KeyLockFlg = true;
        }
    } else {
        UdsService.KeyErrLockTimer = 0;
        UdsService.KeyLockFlg = false;
        UdsService.KeyErrCnt = 0;
    }
}

void DTC_SignalInit(void)
{
#if BOOTLOADER_CODE_FLAG == 0
    uint8_t index;
    uint8_t size;
    DTCS_NvmType *dtc, *dtcBak;

    EE_Read(NVM_IMAGE_DTC_ADDR, (uint8_t **)&dtc);
    size = sizeof(DTC_RecordDataBufType);

    for (index = 0; index < DTC_ID_NUM; index++) {
        if (CRC16_DataCheck((uint8_t *)&dtc->DTC_Status.recordData[index], size - 2) == 0) {
            memcpy((uint8_t *)&dtcComm.DTCStatus.recordData[index], (uint8_t *)&dtc->DTC_Status.recordData[index], size);
            memcpy((uint8_t *)&dtcStatusBak.recordData[index], (uint8_t *)&dtc->DTC_Status.recordData[index], size);
        }

        if (dtcComm.DTCStatus.recordData[index].status == ((1 << DTC_STATUS_Test_Failed) | (1 << DTC_STATUS_Confirmed_DTC))) {
            faultInfo.w |= 1 << index;
        }
    }
    // UdsDtcUpdateVehicle();
    // diagStart_InitTimer = DIAG_START_INIT_TIME;
    // diagStart_Timer = DIAG_START_TIME;
    dtcOptions.w = 0xffffffff;
    SnapshotRecordNumberGet(dtcComm.DTCStatus.recordData, dtcSnapshotDataBuf, &dtcsnapshotNumberTotal);
#endif
}

void UDS_ServiceInit(void)
{
    // //ECU 上电或复位后默认处于闭锁状态，同时错误计数器重置为零。
    // UdsService.KeyErrLockTimer = 0;
    // UdsService.KeyLockFlg = false;
    // UdsService.KeyErrCnt = 0;

    UdsService.SessionDiagModel = _UDS_DEFAULT_SESSION;
    UdsService.SeedKeyIsOK = 0;
    UdsService.AlreadSendSeed = false;
    UdsService.GenerateSeedReq_Non = false;
    UdsService.SessionTimer = 0;
    UdsService.NcmDisRxAndTx = 0;
    UdsService.NmcmDisRxAndTx = 0;
    UdsService.DisDTCRecord = 0;
}

void DTC_SignalRecover(void)
{

}


void GenRand(void)
{

}


#if BOOTLOADER_CODE_FLAG == 0

void SnapshotRecordLoad(DTC_RecordDataBufType *record)
{
	record->snapshot = vehicleInfo2uds.Snapshot;
}

uint8_t SnapshotRecordFill(uint8_t *data, DTC_SnapshotType *snapshot)
{
    uint8_t j = 0;
    uint16_t dataId;

    dataId = 0xCF00;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->BatVolt;
    dataId = 0xCF01;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->SOCH;
    data[j++] = snapshot->SOCL;
    dataId = 0xCF02;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->DCInVolH;
    data[j++] = snapshot->DCInVolL;
    dataId = 0xCF03;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->GunSts;
    dataId = 0xCF04;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->DCRealySts;
    dataId = 0xCF05;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->ACRealySts;
    dataId = 0xCF06;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->CC2ResistorH;
    data[j++] = snapshot->CC2ResistorL;
    dataId = 0xCF07;
    data[j++] = (uint8_t)(dataId >> 8);
    data[j++] = (uint8_t)(dataId & 0x00ff);
    data[j++] = snapshot->CommSts;

    return j;
}

void SnapshotRecordNumberGet(DTC_RecordDataBufType *record_data, uint16_t *dataBuf, uint8_t *num)
{
    uint8_t i = 0;
    uint8_t k = 0, j = 0;
    uint16_t temp;

    *num = 0;
    for (i=0; i<DTC_ID_NUM; i++) {
        if (record_data[i].snapNumber) {
            dataBuf[(*num)++] = (uint16_t)i<<8 | record_data[i].snapNumber;
        }
    }
    if (*num) {
        //升序
        for (j=0; j<(*num)-1; j++) {  
            for (k=0; k<(*num)-j-1; k++) {  
                if ((uint8_t)dataBuf[k]>(uint8_t)dataBuf[k+1]) {  
                    temp = dataBuf[k+1];  
                    dataBuf[k+1] = dataBuf[k];	
                    dataBuf[k] = temp;  
                }  
            }  
        }
    }
}

uint8_t DiagDtcReportSnapshotFill(uint8_t sub_func, DTC_RecordDataBufType *record_data, uint8_t *data_buf, uint8_t *length)
{
    uint8_t ii = 0;
    uint8_t ret = 1;
    uint8_t num = 0, index;
    uint32_t dtc_record;
    uint16_t *snap_buf = dtcSnapshotDataBuf;

    data_buf[(*length)++] = DIAG_SERVE_READ_DTC|0x40;
    data_buf[(*length)++] = sub_func;

    SnapshotRecordNumberGet(record_data, snap_buf, &num);

    for (ii=0; ii<num; ii++) {
        index = (uint8_t)(snap_buf[ii]>>8);
        dtc_record = DTC_LIST_TABLE[index];
        data_buf[(*length)++] = (uint8_t)(dtc_record>>16);
        data_buf[(*length)++] = (uint8_t)(dtc_record>>8);
        data_buf[(*length)++] = (uint8_t)(dtc_record>>0);
        data_buf[(*length)++] = record_data[index].snapNumber;
    }
    return 0;
}

uint8_t DiagDtcSnapshotRecordFill(uint8_t sub_func, DTC_MaskRecordTpye mask_record,
            uint8_t snap_record_num, DTC_RecordDataBufType *record_data, uint8_t *data_buf, uint8_t *length)
{
    uint8_t ii = 0;
    uint8_t ret = 1;

    data_buf[(*length)++] = DIAG_SERVE_READ_DTC|0x40;
    data_buf[(*length)++] = sub_func;

    for (ii=0; ii<DTC_ID_NUM; ii++) {
        if ((mask_record.dtc & 0x00ffffff) == DTC_LIST_TABLE[ii]) {
            data_buf[(*length)++] = mask_record.DTCHightByte;
            data_buf[(*length)++] = mask_record.DTCMiddleByte;
            data_buf[(*length)++] = mask_record.DTCLowByte;
            data_buf[(*length)++] = record_data[ii].status;

            if ((snap_record_num != 0x01) && (snap_record_num != 0xff)) {
                break;
            }

            ret = 0;
            if (record_data[ii].extended.FaultCounter == 0) {
                    break;
            }
            data_buf[(*length)++] = 0x01;
            data_buf[(*length)++] = 0x08;	
            *length += SnapshotRecordFill(&data_buf[*length], &record_data[ii].snapshot);
            break;
        }
    }
    return ret;
}

uint8_t DiagDtcExtendedDataRecordFill(uint8_t subFunc, DTC_MaskRecordTpye mask_record,
            uint8_t extended_record_num, DTC_RecordDataBufType *record_data, uint8_t *data_buf, uint8_t *length)
{
    uint8_t ret = 1;
#if 0
    uint8_t ii = 0;

    if (extended_record_num == 0x01 \
        || extended_record_num == 0xff) {
        data_buf[(*length)++] = DIAG_SERVE_READ_DTC|0x40;
        data_buf[(*length)++] = subFunc;

        for (ii=0; ii<DTC_ID_NUM; ii++) {
            if ((mask_record.dtc & 0x00ffffff) == DTC_LIST_TABLE[ii]) {
                ret = 0;
                data_buf[(*length)++] = mask_record.DTCHightByte;
                data_buf[(*length)++] = mask_record.DTCMiddleByte;
                data_buf[(*length)++] = mask_record.DTCLowByte;
                data_buf[(*length)++] = record_data[ii].status;

                switch (extended_record_num) {
                case 0x01:
                case 0xff:
                    data_buf[(*length)++] = 0x01;
                    data_buf[(*length)++] = record_data[ii].extended.FaultCounter;
                    data_buf[(*length)++] = record_data[ii].extended.FaultCounterOfAge;
                    data_buf[(*length)++] = record_data[ii].extended.FaultAgingCounter;
                    break;
                }
                break;
            }
        }
    }
#endif
    return ret;
}


void IO_InputOutputRelease(void)
{

}

void DiagUpdateDtcValue(uint8_t index, uint8_t status)
{
    DTC_RecordDataBufType *dtc_record_data;
    uint8_t *snapshot_number_total;

    snapshot_number_total = &dtcsnapshotNumberTotal;
    dtc_record_data = &dtcComm.DTCStatus.recordData[index];

    if (index >= DTC_ID_NUM) {
        return;
    }
    if (dtc_record_data->status != status) {
		if (status == ((1<<DTC_STATUS_Test_Failed) | (1<<DTC_STATUS_Confirmed_DTC))) {
			SnapshotRecordLoad(dtc_record_data);
            if (!dtc_record_data->snapNumber) {
                (*snapshot_number_total)++;
                dtc_record_data->snapNumber = *snapshot_number_total;
            }
            
			if (dtc_record_data->extended.FaultCounter != 0xff) {
				dtc_record_data->extended.FaultCounter ++;
			}
		}
		// dtc_record_data->extended.FaultAgingCounter = 0;
	}

    dtc_record_data->status = status;
}


//电压正且非busoff、电源ON下
void DiagFaultsMonitor(void)
{

    // if (CheckIsBatteryHigh())return;

    // if (CheckIsBatteryLow())return;

    // if (CheckIsIgnON()) {

    	if (vehicleInfo2uds.InOverVoltFault) {
    		faultInfo.b.fINPUT_OVERVOLTAGE = 1;
    	} else {
    		faultInfo.b.fINPUT_OVERVOLTAGE = 0;
    	}

    	if (vehicleInfo2uds.InUnderVoltFault) {
    		faultInfo.b.fINPUT_UNDERVOLTAGE = 1;
    	} else {
    		faultInfo.b.fINPUT_UNDERVOLTAGE = 0;
    	}

    	if (vehicleInfo2uds.ChargeTempDerateFault) {
    		faultInfo.b.fCHARGE_TEMP_DERATE = 1;
    	} else {
    		faultInfo.b.fCHARGE_TEMP_DERATE = 0;
    	}

    	if (vehicleInfo2uds.InterTempDerateFault) {
    		faultInfo.b.fINTER_TEMP_DERATE = 1;
    	} else {
    		faultInfo.b.fINTER_TEMP_DERATE = 0;
    	}

    	if (vehicleInfo2uds.InterOverTempFault) {
    		faultInfo.b.fINTER_OVERTEMP = 1;
    	} else {
    		faultInfo.b.fINTER_OVERTEMP = 0;
    	}

    	if (vehicleInfo2uds.ChargeOverTempFault) {
    		faultInfo.b.fCHARGE_OVERTEMP = 1;
    	} else {
    		faultInfo.b.fCHARGE_OVERTEMP = 0;
    	}

    	if (vehicleInfo2uds.DcPosRelayAdhFault) {
    		faultInfo.b.fDC_POS_RELAY_ADH = 1;
    	} else {
    		faultInfo.b.fDC_POS_RELAY_ADH = 0;
    	}

        if (vehicleInfo2uds.DcNegRelayAdhFault) {
    		faultInfo.b.fDC_NEG_RELAY_ADH = 1;
    	} else {
    		faultInfo.b.fDC_NEG_RELAY_ADH = 0;
    	}
    	
        if (vehicleInfo2uds.AcPosRelayAdhFault) {
    		faultInfo.b.fAC_POS_RELAY_ADH = 1;
    	} else {
    		faultInfo.b.fAC_POS_RELAY_ADH = 0;
    	}

        if (vehicleInfo2uds.AcNegRelayAdhFault) {
    		faultInfo.b.fAC_NEG_RELAY_ADH = 1;
    	} else {
    		faultInfo.b.fAC_NEG_RELAY_ADH = 0;
    	}

        if (vehicleInfo2uds.HvInterlockFault) {
    		faultInfo.b.fHV_INTERLOCK = 1;
    	} else {
    		faultInfo.b.fHV_INTERLOCK = 0;
    	}

        if (vehicleInfo2uds.CcCc2RelayCtrlInv) {
    		faultInfo.b.fCC_CC2_RELAY_CTRL_INV = 1;
    	} else {
    		faultInfo.b.fCC_CC2_RELAY_CTRL_INV = 0;
    	}

        if (vehicleInfo2uds.CpCc1RelayCtrlInv) {
    		faultInfo.b.fCP_CC1_RELAY_CTRL_INV = 1;
    	} else {
    		faultInfo.b.fCP_CC1_RELAY_CTRL_INV = 0;
    	}

        if (CheckIsCanBusoff()) {
            if (vehicleInfo2uds.BusoffCnt >= 8)
            {
                faultInfo.b.fECU_ENTER_BUSOFF = 1;
            }
        } else {
            faultInfo.b.fECU_ENTER_BUSOFF = 0;
        }

        if (CheckIsCanBusoff())return;

        // if (diagStart_Timer) {
		// 	diagStart_Timer --;
		// 	if (diagStart_Timer == 0) {
		// 		BMS_CommLossTimer = 0;
		// 	}
		// 	return;
		// } else {
            if (++BMS_CommLossTimer > 200) {
                faultInfo.b.fBMS_COMM_LOSE = 1;
                BMS_CommLossTimer = 0;
            }
            if (++VCU_CommLossTimer > 300) {
                faultInfo.b.fVCU_COMM_LOSE = 1;
                VCU_CommLossTimer = 0;
            }
        // }
    // } else {
    //     diagStart_Timer = DIAG_START_TIME;
    // }

}

void Uds_SetMsgMonitorTimer(uint16_t msgid)
{
#if BOOTLOADER_CODE_FLAG == 0
    if (!udsInfo2Vehicle.NcmRxEnable) return;
	switch (msgid) {
		case Bms_104:
			faultInfo.b.fBMS_COMM_LOSE = 0;
			BMS_CommLossTimer = 0;
			break;
        case Vcu_state_111:
			faultInfo.b.fVCU_COMM_LOSE = 0;
			VCU_CommLossTimer = 0;
		default:
			break;
	}
#endif
}

void GetBatteryStatus(void)
{
    if (CheckIsBatteryHigh()) {
		faultInfo.b.fECU_VOLTAGE_HIGH = 1;
	} else {
		faultInfo.b.fECU_VOLTAGE_HIGH = 0;
	}

	if (CheckIsBatteryLow()) {
		faultInfo.b.fECU_VOLTAGE_LOW = 1;
	} else {
		faultInfo.b.fECU_VOLTAGE_LOW = 0;
	}
}

#if 0
void DtcAging_Process(void)
{
    uint8_t index;
    DTC_RecordDataBufType *dtc_record_data;
    static uint8_t ignStsBak = 1;

    if (ignStsBak != vehicleInfo2uds.IgnSts) {
        ignStsBak = vehicleInfo2uds.IgnSts;

        if (vehicleInfo2uds.IgnSts) {
            for (index = 0; index < DTC_ID_NUM; index++) {
                dtc_record_data = &dtcComm.DTCStatus.recordData[index];
                if (dtc_record_data->status == (1 << DTC_STATUS_Confirmed_DTC)) {
                    dtc_record_data->extended.FaultAgingCounter++;
                    if (dtc_record_data->extended.FaultAgingCounter >= 40) {
                        dtc_record_data->extended.FaultAgingCounter = 0;
                        dtc_record_data->status = 0;
                        if (dtc_record_data->extended.FaultCounterOfAge != 0xff) {
                            dtc_record_data->extended.FaultCounterOfAge++;
                        }
                    }
                }
            }
        }
    }
}
#endif

void ClearHardwareFault(void)
{
	UdsApi_HardwareFault_ForceClear();
}

void GetFaultInfo(void)
{
    uint8_t shift, status;
    uint8_t size;
    uint16_t crc16;
    DTC_Fault_Flags littleFault;
    DTC_RecordDataBufType *pDtcRecordData;
    static uint8_t wrIndex = 0;
    static uint8_t timer100ms;

    if (diagStart_InitTimer > 0) {  //电压正常后才计数
        diagStart_InitTimer --;

        return;
    }
    GetBatteryStatus();

    DiagFaultsMonitor();

    if (UdsService.DisDTCRecord) {
        return;
    }

    // DtcAging_Process();

    if (DTC_ID_NUM <= 32) {
        //littleFault.w= BigLittleSwap32(faultInfo.w);
        littleFault.w = faultInfo.w;

        for (shift = 0; shift < DTC_ID_NUM; shift++) {

            if ((littleFault.w >> shift) & 0x00000001) {
                status = (1 << DTC_STATUS_Test_Failed) | (1 << DTC_STATUS_Confirmed_DTC); //当前故障
            } else {
                status = dtcComm.DTCStatus.recordData[shift].status & (~(1 << DTC_STATUS_Test_Failed));
            }

            DiagUpdateDtcValue(shift, status);
        }
    } else {
        //超出32个的，需要特殊处理
    }

    size = sizeof(DTC_RecordDataBufType);
    pDtcRecordData = &dtcComm.DTCStatus.recordData[wrIndex];
    if (pDtcRecordData->status != dtcStatusBak.recordData[wrIndex].status \
        /*|| pDtcRecordData->extended.FaultAgingCounter != dtcStatusBak.recordData[wrIndex].extended.FaultAgingCounter*/) {
        crc16 = CRC_Cal16_WithCfg((uint8_t *)pDtcRecordData, size - 2);
        pDtcRecordData->crc16[0] = (uint8_t)crc16;
        pDtcRecordData->crc16[1] = (uint8_t)(crc16 >> 8);

        dtcStatusBak.recordData[wrIndex].status = pDtcRecordData->status;
        // dtcStatusBak.recordData[wrIndex].extended.FaultAgingCounter = pDtcRecordData->extended.FaultAgingCounter;
        EE_Write(NVM_IMAGE_DTC_ADDR + (size * wrIndex), (uint8_t *)pDtcRecordData, size, NULL);
    }
    wrIndex++;
    if (wrIndex >= DTC_ID_NUM) {
        wrIndex = 0;
    }

    if (++timer100ms >= 10) {
        timer100ms = 0;
        UdsDtcUpdateVehicle();
    }
}


void ClearDtc(void)
{
    uint8_t shift;
    uint16_t crc16;
    DTC_RecordDataBufType *pDtcRecordData;

    faultInfo.w = 0;
    dtcsnapshotNumberTotal = 0;
    for (shift = 0; shift < DTC_ID_NUM; shift++) {
        pDtcRecordData = &dtcComm.DTCStatus.recordData[shift];

        memset((uint8_t *)pDtcRecordData, 0x00, sizeof(DTC_RecordDataBufType) - 2);
        crc16 = CRC_Cal16_WithCfg((uint8_t *)pDtcRecordData, sizeof(DTC_RecordDataBufType) - 2);
        pDtcRecordData->crc16[0] = (uint8_t)crc16;
        pDtcRecordData->crc16[1] = (uint8_t)(crc16 >> 8);
    }

    memcpy((uint8_t *)&dtcStatusBak, (uint8_t *)&dtcComm.DTCStatus, sizeof(DTC_StatusType));
    EE_Write(NVM_IMAGE_DTC_ADDR, (uint8_t *)&dtcComm.DTCStatus, sizeof(DTC_StatusType), NULL);
    ClearHardwareFault();
}


bool GetDtcStatusMask(uint8_t *pOut, uint8_t *len, uint8_t subFunc, uint8_t mask)
{
    bool retValue = FALSE;
    uint8_t ii = 0;
    uint8_t j = 0;

    dtcComm.DTCcount.map = 0;
    pOut[j++] = DIAG_SERVE_READ_DTC | 0x40;
    pOut[j++] = subFunc;
    pOut[j++] = DTC_STATUS_Support_Bit_Status;

    for (ii = 0; ii < DTC_ID_NUM; ii++) {
        /*if (((dtcComm.DTCStatus[ii]&DTC_STATUS_Support_Bit_Status) == (mask&DTC_STATUS_Support_Bit_Status))\
        ||(subFunc == DTC_ReportSupportedDTCs))*/
        if ((dtcComm.DTCStatus.recordData[ii].status & mask) \
            || (dtcComm.DTCStatus.recordData[ii].status == mask)\
            || (subFunc == DTC_ReportSupportedDTCs)) {
            if ((dtcOptions.w >> ii) & 0x00000001) {
                retValue = TRUE;
                dtcComm.DTCcount.map++;
                pOut[j++] = (uint8_t)(DTC_LIST_TABLE[ii] >> 16);
                pOut[j++] = (uint8_t)(DTC_LIST_TABLE[ii] >> 8);
                pOut[j++] = (uint8_t)(DTC_LIST_TABLE[ii] >> 0);
                pOut[j++] = dtcComm.DTCStatus.recordData[ii].status;
            }
        }
    }

    *len = j;
    return retValue;
}
#endif

void DiagDIDStoreFlash(uint16_t id)
{
    uint8_t didIndex, nvmDidIndex;
    //uint16_t offset;
    uint16_t size;
    uint8_t *cachePtr, *didPtr, *nvmDidPtr = NULL;
    uint16_t crc16;
    bool storage_flag = false;

    nvmDidIndex = GetDIDStoreInFlashIndexByID(id);

    if (nvmDidIndex != 0xFF) {
        didIndex = GetDIDIndexByID(id);

        if (didIndex != 0xFF) {
            size = DIDSize[didIndex];
            cachePtr = (uint8_t *)&gDIDCache;
            didPtr = cachePtr + DIDOffset[didIndex];
            crc16 = CRC_Cal16_WithCfg(didPtr, size);
            didPtr[size] = (uint8_t)crc16;
            size++;
            didPtr[size] = (uint8_t)(crc16 >> 8);
            size++;
            nvmDidPtr = (uint8_t *)NVM_IMAGE_DID_ADDR + DIDStoreInFlashOffset[nvmDidIndex];

            if (nvmDidPtr != NULL) {
                EE_Write(nvmDidPtr, didPtr, size, NULL);
            }
            UdsDidUpdateVehicle();
        }
    }
}

void CheckDtcHsCanError(void)
{
    static uint16_t Timer100msCnt = 0;
#if BOOTLOADER_CODE_FLAG == 0
    static uint16_t Timer10msCnt = 0;
#endif
    uint16_t crc16;


#if BOOTLOADER_CODE_FLAG == 0

    if (++Timer10msCnt >= 10) {
        Timer10msCnt = 0;
        GetFaultInfo();
        // IgnChangeProcess(); 没有老化
    }
    gDIDCache.DID_CurrentDCVolt[0] = MSB(vehicleInfo2uds.CurrDcVolt);
    gDIDCache.DID_CurrentDCVolt[1] = LSB(vehicleInfo2uds.CurrDcVolt);
    gDIDCache.DID_DCUVariousCurrentTempVal[0] = vehicleInfo2uds.ChargeTemp;
    gDIDCache.DID_DCUVariousCurrentTempVal[1] = vehicleInfo2uds.BoardTemp;

#endif
    if (++Timer100msCnt >= 100) {
        Timer100msCnt = 0;
        
        if (UdsParam.SA_FailureCounter != UdsService.KeyErrCnt) {
            UdsParam.SA_FailureCounter = UdsService.KeyErrCnt;
            crc16 = CRC_Cal16_WithCfg((uint8_t *)&UdsParam, sizeof(UdsParam) - 2);
            UdsParam.crc16[0] = (uint8_t)crc16;
            UdsParam.crc16[1] = (uint8_t)(crc16 >> 8);
            EE_Write(NVM_IMAGE_PARAM_ADDR, (uint8_t*)&UdsParam, sizeof(UdsParam), NULL);
        }
    }

    if ((UdsService.UdsTxFlg == true) && (UdsService.UdsTxCompFlg == true)) {
        UdsService.UdsTxFlg = false;
        UdsService.UdsTxCompFlg = false;
        HAL_UDS_Tx_Callback();
        //UdsQueueCanOut();
        UdsService.TxCompFlg = true;
        //Debug("HAL_UDS_Tx_Callback \r\n"); //
    }
}

#if BOOTLOADER_CODE_FLAG == 0
INT8U Get0x19SerDiagData(INT8U *pOut, INT8U Type, INT8U *pdata)
{
    INT8U RstLen = 0;
//    INT16U i=0;
//    INT32U dtc_code = 0;
//    INT8U num = 0;
    uint8_t dtcReportSubFunction;
    uint8_t dtcRecordNumber;
    uint8_t dtcStatusMask;
    DTC_MaskRecordTpye dtcMaskRecord;

    dtcReportSubFunction = Type;
    dtcStatusMask = pdata[1];

    switch (dtcReportSubFunction) {
        case DTC_ReportNumberOfDTCByStatusMask:
        case DTC_ReportDTCByStatusMask:
        case DTC_ReportSupportedDTCs:
            if (GetDtcStatusMask(pOut, &RstLen, dtcReportSubFunction, dtcStatusMask) == FALSE) {
                RstLen = 0;
                pOut[RstLen++] = DIAG_SERVE_READ_DTC | 0x40; //0x59;
                pOut[RstLen++] = dtcReportSubFunction;
                pOut[RstLen++] = DTC_STATUS_Support_Bit_Status;
            }

            if (dtcReportSubFunction == DTC_ReportNumberOfDTCByStatusMask) {
				RstLen = 0;
				pOut[RstLen++] = DIAG_SERVE_READ_DTC|0x40;//0x59;
                pOut[RstLen++] = dtcReportSubFunction;
                pOut[RstLen++] = DTC_STATUS_Support_Bit_Status;
                pOut[RstLen++] = ISO15031_6_DTCformat;
                pOut[RstLen++] = dtcComm.DTCcount.b.DTCcountHighByte;
                pOut[RstLen++] = dtcComm.DTCcount.b.DTCcountLowByte;
			}

            break;
        case DTC_ReportDTCSnapshotIdentification:
            DiagDtcReportSnapshotFill(dtcReportSubFunction, dtcComm.DTCStatus.recordData, pOut, &RstLen);
            break;
        case DTC_ReportDTCSnapshotRecordByDTCNumber:
            dtcMaskRecord.DTCHightByte = pdata[1];
            dtcMaskRecord.DTCMiddleByte = pdata[2];
            dtcMaskRecord.DTCLowByte = pdata[3];
            dtcRecordNumber = pdata[4];
            if (DiagDtcSnapshotRecordFill(dtcReportSubFunction, dtcMaskRecord, dtcRecordNumber,
                    dtcComm.DTCStatus.recordData, pOut, &RstLen) != 0) {
                    RstLen = 0;
            }
            break;
        case DTC_ReportDTCExtendedDataRecordByDTCNumber:
            dtcMaskRecord.DTCHightByte = pdata[1];
            dtcMaskRecord.DTCMiddleByte = pdata[2];
            dtcMaskRecord.DTCLowByte = pdata[3];
            dtcRecordNumber = pdata[4];
            if (DiagDtcExtendedDataRecordFill(dtcReportSubFunction, dtcMaskRecord, dtcRecordNumber,
                    dtcComm.DTCStatus.recordData, pOut, &RstLen) != 0) {
                    RstLen = 0;
            }
            break;
        default:
            break;
    }

    return RstLen;
}
#endif



INT16U Get0x22SerDiagData(INT8U *pOut)
{
    INT16U RstLen = 0;
    INT16U ReqId = 0;
//    INT16U length = 0;
    uint8_t i;
    uint8_t *cachePtr;
    uint8_t size;

    ReqId = ((UdsService.ReqCmdBuf[0] << 8) + UdsService.ReqCmdBuf[1]);

    pOut[RstLen++] = 0x62;
    pOut[RstLen++] = ((ReqId >> 8) & 0xFF);
    pOut[RstLen++] = (ReqId & 0xFF);

    cachePtr = (uint8_t *)&gDIDCache;

    for (i = 0; i < DID_NUM; i++) {
        if (ReqId == DIDId[i]) {
            break;
        }
    }

    if (i == DID_NUM) {
        RstLen = 2;
    } else {
#if BOOTLOADER_CODE_FLAG

        if (!CheckIsSupportBootloaderRead_Sid22(ReqId)) {
            RstLen = 2;
        }

#endif
    }

    if (RstLen != 2) {
        size = DIDSize[i];

        memcpy(&pOut[RstLen], (cachePtr + DIDOffset[i]), size);
        RstLen += size;
    }

    return RstLen;
}

#if BOOTLOADER_CODE_FLAG

INT8U Set0x36DiagData(INT8U *pIn, INT16U PLen, INT8U *pOut, INT16U *RLen)
{
    uint8_t /*i=0,*/ucErrCode = 0;
    uint8_t sn = 0;


    sn = pIn[0];
    UdsUpData.totalRecvLength += (UdsService.RxDlc - 2);

    if (CheckIsBatteryHigh()) {
        ucErrCode = 0x92;
    } else if (CheckIsBatteryLow()) {
        ucErrCode = 0x93;
    } else if (UdsService.RxDlc > (FMC_PAGE_SIZE + 2)) {
        ucErrCode = 0x13;
    } else if ((UdsUpData.DownloadSeq != DOWNLOAD_SEQUENCE_34) && (UdsUpData.DownloadSeq != DOWNLOAD_SEQUENCE_36)) {
        ucErrCode = 0x24;
    } else if (sn != UdsUpData.Sn) {
        ucErrCode = 0x73;
    } else if (UdsUpData.totalRecvLength > FBL_AppSize) {
        ucErrCode = 0x71;
    } else {
        UdsUpData.RxProgramFlag = true;
        UdsUpData.DownloadSeq = DOWNLOAD_SEQUENCE_36;
        memcpy(UdsUpData.Buf, &TP_Buffer[2], (UdsService.RxDlc - 2));
        ucErrCode = 0x78;
    }

    return ucErrCode;

}
#endif

INT8U Set0x31DiagData(INT8U *pIn, INT16U PLen, INT8U *pOut, INT16U *RLen)
{

    uint8_t service_buf[50];
    uint8_t /*i=0,*/ucErrCode = 0, ucRsp = 0;
    uint16_t ReqId = 0;
    uint16_t dlen = 0;
    uint8_t result = 0;

    if (UdsService.RxDlc < 4) {
		return 0x13;
	}
    
    ucRsp = pIn[0];
    ReqId = pIn[1] << 8 | pIn[2];

    service_buf[dlen++] = 0x71;
    service_buf[dlen++] = pIn[0];
    service_buf[dlen++] = pIn[1];
    service_buf[dlen++] = pIn[2];

#if BOOTLOADER_CODE_FLAG == 0

	static bool routineStartFlg = false;
	if(0x01 == ucRsp) {
        ReqId = pIn[1]<<8 | pIn[2];
		if((true == CheckUdsDlcIsOk(UdsService.RxDlc,4) && (ReqId == 0x0203))\
       	||(true == CheckUdsDlcIsOk(UdsService.RxDlc,4) && (ReqId == 0x0204))
       	||(true == CheckUdsDlcIsOk(UdsService.RxDlc,5) && (ReqId == 0x0207))) {
       		if (UdsService.SeedKeyIsOK) {
       			if (true == CheckVechileStatus()) {
		       		switch (ReqId) {
		       			case 0x0204:
		       				result = 0x00;	//correctResult
                            service_buf[dlen++] = result;
		       				break;
		       			case 0x0207:
                            if (pIn[3] <= 1) {
                                result = 0x00;	//correctResult
                                udsInfo2Vehicle.DCDCEnable = pIn[3];    
                            } else {
                                result = 0x01;	//incorrectResult
                            }
                            service_buf[dlen++] = result;
		       				
		       				break;
		       			case 0x0203:
		       				// if (UdsService.BattErrFlg == false) {
		       					UdsService.ChkProgPreconditionFlg = true;
                                UdsService.ProgrammingPassTimer = PROGRAMMING_PASS_TIMEOUT_TIME;
		       				// }
		       				result = 0x00;	//correctResult
		       				service_buf[dlen++] = result;
		       				break;
		       			default:
		       				break;
		       		}
	       		} else {
	       			ucErrCode = 0x22;
	       		}
	       		if (!ucErrCode) {
	       			routineStartFlg = true;
	       		}
       		} else {
				ucErrCode = 0x33;
       		}
       	} else if ((ReqId == 0x0203) || (ReqId == 0x0204) || (ReqId == 0x0207)) {
       		ucErrCode = 0x13;
       	} else {
       		ucErrCode = 0x31;
       	}
   	} else if(0x03 == ucRsp) {
        ReqId = pIn[1]<<8 | pIn[2];
		if ((true == CheckUdsDlcIsOk(UdsService.RxDlc,4) && (ReqId == 0x0204))) {
       		if (UdsService.SeedKeyIsOK) {
	       		if (true == CheckVechileStatus()) {
	       			if (true == routineStartFlg) {
	       				routineStartFlg = false;
			       		switch (ReqId) {
			       			case 0x0204:
                                result = vehicleInfo2uds.OtaModeSts;
                                service_buf[dlen++] = result;
			       				break;
			       			default:
			       				break;
			       		}
		       		} else {
		       			ucErrCode = 0x24;
		       		}
	       		} else {
	       			ucErrCode = 0x22;
	       		}
       		} else {
				ucErrCode = 0x33;
       		}
       	} else if ((ReqId == 0x0204)) {
       		ucErrCode = 0x13;
       	} else {
       		ucErrCode = 0x31;
       	}
    } else if (0x02 == ucRsp) {
        if ((true == CheckUdsDlcIsOk(UdsService.RxDlc, 4) && (ReqId == 0x0204))) {
            if (UdsService.SeedKeyIsOK) {
                switch (ReqId) {
                    case 0x0204:
                        result = 0x00;	//correctResult
                        service_buf[dlen++] = result;
                        break;
                    default:
                        break;
                }
            } else {
                ucErrCode = 0x33;
            }
        } else if ((ReqId == 0x0204)) {
            ucErrCode = 0x13;
        } else {
            ucErrCode = 0x31;
        }
    } else {
        if (true == UdsService.PhyFlg) {
            UdsService.PhyFlg = false;
            ucErrCode = 0x12;
        }
    }

#endif
    *RLen = dlen;

    if (ucErrCode == 0) {
        memcpy(pOut, service_buf, dlen);
    }

    return  ucErrCode;

}

INT8U Set0x2EDidDiagData(INT8U *pIn, INT16U PLen)
{
    INT8U Rst = 0;
    INT16U ReqId = 0;

    ReqId = ((pIn[0] << 8) + pIn[1]);

#if BOOTLOADER_CODE_FLAG == 0
    if (ReqId == DIDId[VehicleIdentificationNumber]) {
        if(PLen == DIDSize[VehicleIdentificationNumber]) {
            if (UdsService.SeedKeyIsOK) {
                if (CheckVechileStatus()) {
                    memcpy(gDIDCache.DID_VehicleIdentificationNumber,&pIn[2],PLen);
                } else {
                    Rst = 0x22;
                }
            } else {
                Rst = 0x33;
            }
        } else {
            Rst = 0x13;
        }
    } else if (ReqId == DIDId[EOLData]) {
        if(PLen == DIDSize[EOLData]) {
            if (UdsService.SeedKeyIsOK) {
                if (CheckVechileStatus()) {
                    memcpy(gDIDCache.DID_EOLData,&pIn[2],PLen);
                } else {
                    Rst = 0x22;
                }
            } else {
                Rst = 0x33;
            }
        } else {
            Rst = 0x13;
        }
    } else if (ReqId == DIDId[DCUVariousTempProtectVal]) {
        if(PLen == DIDSize[DCUVariousTempProtectVal]) {
            if (UdsService.SeedKeyIsOK) {
                if (CheckVechileStatus()) {
                    memcpy(gDIDCache.DID_DCUVariousTempProtectVal,&pIn[2],PLen);
                } else {
                    Rst = 0x22;
                }
            } else {
                Rst = 0x33;
            }
        } else {
            Rst = 0x13;
        }
    } else if (ReqId == DIDId[DCVoltProtectVal]) {
        if(PLen == DIDSize[DCVoltProtectVal]) {
            if (UdsService.SeedKeyIsOK) {
                if (CheckVechileStatus()) {
                    memcpy(gDIDCache.DID_DCVoltProtectVal,&pIn[2],PLen);
                } else {
                    Rst = 0x22;
                }
            } else {
                Rst = 0x33;
            }
        } else {
            Rst = 0x13;
        }
    } else if (ReqId == DIDId[InsulationDectVolt]) {
        if(PLen == DIDSize[InsulationDectVolt]) {
            if (UdsService.SeedKeyIsOK) {
                if (CheckVechileStatus()) {
                    memcpy(gDIDCache.DID_InsulationDectVolt,&pIn[2],PLen);
                } else {
                    Rst = 0x22;
                }
            } else {
                Rst = 0x33;
            }
        } else {
            Rst = 0x13;
        }
    } else {
        Rst = 0x31;
    }

#else
    if (ReqId == DIDId[FingerPrint]) {
        if(PLen == DIDSize[FingerPrint]) {
            if (UdsService.SeedKeyIsOK) {
                memcpy(gDIDCache.DID_FingerPrint,&pIn[2],PLen);
            } else {
                Rst = 0x33;
            }
        } else {
            Rst = 0x13;
        }
    } else {
        Rst = 0x31;
    }

#endif

    if (Rst == 0) {
        DiagDIDStoreFlash(ReqId);
    }

    return Rst;
}

#if BOOTLOADER_CODE_FLAG == 0
INT8U Set0x2FDiagData(INT8U *pIn, INT16U PLen, INT8U *pOut, INT16U *RLen)
{
    INT8U ucErrCode = 0;
    
    return ucErrCode;
}
#endif

void UdsQueueCanOut(void)
{
    //if(Frame_Tx_Complete != false) return;
    //if(TP_Tx_Message_DLC) return;
    while ((UdsQueueData.In != UdsQueueData.Out)) {
        if (UdsQueueData.Out >= CAN0_MAX_RX) {
            UdsQueueData.Out = 0;
        }

        gUdsBusOffCanOutBak = UdsQueueData.Out;
        ParseUdsCan(UdsQueueData.CanMsg[UdsQueueData.Out]);
        UdsQueueData.Out++;
        ParseCanRxCnt++;
        // break;
    }

    //return;
}


uint8_t GenerateSID27SeedValue(uint8_t *seed)
{
	uint32_t random;

	random *= UdsApi_GetRandom();
    if (random == 0xffffffff) {
        random *= 0xaa;
    } else if (random == 0) {
        random += UdsApi_GetRandom();
    }
	seed[0] = (uint8_t)(random >> 0);
	seed[1] = (uint8_t)(random >> 16);
	seed[2] = (uint8_t)(random >> 8);
	seed[3] = (uint8_t)(random >> 0);
    return 0;
}

#if BOOTLOADER_CODE_FLAG
    extern void RAMFLASHInit(void);
#endif

uint8_t GenerateSID27KeyValue(uint8_t levelType,uint8_t *seed,uint8_t *key)
{
    uint8_t i = 0;

    uint8_t cal[4] = {0};
    uint8_t xor_data[4] = {0x90,0x23,0xB0,0x21};

    if(levelType == 1) { //Level 1
        for (i=0;i<4;i++)
        {
            cal[i] = seed[i] ^ xor_data[i];
        }
        key[0] = ((cal[0]&0x0f)<<4) | (cal[1] & 0xf0);
        key[1] = ((cal[1]&0x0f)<<4) | ((cal[2] & 0xf0)>>4);
        key[2] = ((cal[2]&0xf0)) | ((cal[3] & 0xf0)>>4);
        key[3] = ((cal[3]&0x0f)<<4) | (cal[0] & 0x0f);
    } else { //FBL_Level
        for (i=0;i<4;i++)
        {
            cal[i] = seed[i] ^ xor_data[i];
        }
        key[0] = ((cal[0] & 0x0f) << 4) | (cal[1] & 0x0f);
        key[1] = ((cal[1] & 0xf0) >> 4) | ((cal[2] & 0x0f) << 4);
        key[2] = ((cal[2] & 0xf0) >> 4) | ((cal[3] & 0xf0));
        key[3] = ((cal[3] & 0x0f)) | ((cal[0] & 0xf0) >> 4);
    }

    return 0;
}

//extern EReturnMsg bsp_flash_erase_sector(INT32U nFlashAddr);
//extern EReturnMsg bsp_flash_write_buff(INT32U nFlashAddr, INT8U *pBuff, INT32U nLen);
//extern EReturnMsg bsp_flash_read_buff(INT32U nFlashAddr, INT8U *pBuff, INT32U nLen);

uint8_t EE_Write(uint8_t *pImage, const uint8_t *pData, uint32_t Len, void (*pCbFn)(uint32_t))
{
    uds_service_nvm_write(pImage, pData, Len, pCbFn);
    return TRUE;
}


uint8_t EE_Read(uint8_t *pImage, uint8_t **pData)
{
    if (NVM_IMAGE_DTC_ADDR == pImage) {
        *pData = (uint8_t *)NVM_IMAGE_DTC_ADDR;
    } else if (NVM_IMAGE_DID_ADDR == pImage) {
        *pData = (uint8_t *)NVM_IMAGE_DID_ADDR;
    } else if (NVM_IMAGE_PARAM_ADDR == pImage) {
        *pData = (uint8_t *)NVM_IMAGE_PARAM_ADDR;
    } else {
        return FALSE;
    }

    return TRUE;
}

void SetUdsUpdataFlag(void)
{
#if (BOOTLOADER_CODE_FLAG == 0)
    SetExternProgrammingRequest();
#endif

}


bool CheckVechileStatus(void)
{
    bool cbRst = false;
    // uint16_t speed = WORD(vehicleInfo2uds.Snapshot.VehSpeedH, vehicleInfo2uds.Snapshot.VehSpeedL);

    // if (speed <= 80) {
    //     cbRst = true;
    // }
    cbRst = true;

    return cbRst;
}

bool CheckIsBatteryLow(void)
{
    bool bRst = false;

    if (vehicleInfo2uds.BattLow) {
        bRst = true;
    }

    return bRst;
}

bool CheckIsBatteryHigh(void)
{
    bool bRst = false;

    if (vehicleInfo2uds.BattHigh) {
        bRst = true;
    }

    return bRst;
}

bool CheckIsIgnON(void)
{
    bool bRst = false;

    if (vehicleInfo2uds.IgnSts) {
        bRst = true;
    }

    return bRst;
}

//bool CheckIsIgnOff2ON(void)
//{
//  bool bRst=false;
//
//    if (vehicleInfo2uds.IgnOff2On) {
//      bRst = true;
//    }
//    return bRst;
//}
#if BOOTLOADER_CODE_FLAG == 0


bool CheckIsCanBusoff(void)
{
    bool bRst = false;

    if (vehicleInfo2uds.BusoffCnt) {
        bRst = true;
    }

    return bRst;
}

#endif