#include "bsp.h"
#include "bsp_custom.h"
#include "osasys.h"
#include "ostask.h"
#include "queue.h"
#include "ps_event_callback.h"
#include "cmisim.h"
#include "cmimm.h"
#include "cmips.h"
#include "sockets.h"
#include "psifevent.h"
#include "ps_lib_api.h"
#include "lwip/netdb.h"
#include <cis_def.h>
#include "debug_log.h"
#include "slpman_ec616.h"
#include "plat_config.h"
#include "ec_tcpip_api.h"
#include "hal_module_adapter.h"
#include "UartTask.h"
#include "MainTask.h"
#include "app.h"
#include "CANTask.h"
#include "flash_ec616_rt.h"

#include "UDSService.h"


//
extern UartReadMsgType UartReadMsg;
extern osMutexId_t UartMutex;
extern AppSocketConnectionContext socContext;
extern UINT8 GpsData[16];

void UDS_Service(void);
void UDS_CAN_Update(void);
BOOL UDSClearFotaDownloadRegion(void);
BOOL UDSAskforDownLoadData(void);
UINT8 UDSPositiveAnswer(UINT8 answerLen,UINT8 messageIndex,UINT32 posCode);
UINT8 UDSNegtiveAnswer(UINT8 answerLen,UINT8 messageIndex, UINT32 negCode);
extern void appSaveConfigValue(void);


UINT8	UDSService[2];
UINT8	UDSSubService[2];
UINT8	UDSSubServiceActionCode[2];
UINT8 	UDSSwitch = 0;
UINT8   UDSDialogMode = 01;
UINT8   UDSBattSN[BATT_SN_LEN];

UINT32  updateDifferDataByteLen = 0;

//each package means 100 bytes, max PackageCounter = updateDifferDataByteLen%100 + 1
UINT16  updateDifferDataPackageCounter = 0;  

//each slice means 4 bytes, max SliceCounter = 25;
UINT32  updateDifferDataSliceCounter = 0;
static BOOL downloadReady = FALSE;

UINT8   updateDataBuffer[100];





void UDS_Service()
{
	UINT8 i,j= 0;
	UINT32 k=0;
	INT8 ret,errorCount = 0;
	BOOL boolRet = false;
	static UINT8 snFlag = 0;
	static UINT32 sliceCounterFlag = 0;
	UINT32 tempSliceCounter=0;	
	static UINT8 counter=0;  
	CAN_Msg_Type UDSAnswer ={0};
	static UINT8 battInfo[8*14] = {0};
	UINT32 posCode,negCode;
	
	//UartReadMsg Uart_Recv_Msg;
	//UINT8 temp00[94] ={0x00,0x0E,0x00,0x05,0x0D,0xDE,0x0D,0xDE,0x0D,0xDE,0x0D,0xDE,0x0D,0x7A,0x0D,0xDE,0x0D,0xDE,0x0D,0xDE,0x0D,0xDE,0x0D,0xDE,0x0E,0x42,0x0D,0xDE,0x0D,0xDE,0x0D,0xDE,0x13,0x38,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x21,0x00,0x24,0x00,0x26,0x00,0x22,0x00,0x24,0x00,0x00,0x00,0x00,0x6e,0x78,0x00,0x00,0x00,0x00,0x00,0x5F,0x00,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x13,0x74,0x0E,0x42,0x0D,0x7A,0x00,0x00};
	//MEMCPY(battInfo, temp00, 94);
	
	//while(true)
	//{
		//if(UDSSwitch == 1)
		{
			for(i=0; i<2; i++)
			{
				if(CanRxMsg[i].Id == 0x7A0)
					{
					switch (UDSService[i])
						{
							case 0x10:
								if(UDSSubService[i] == 0x01)
								{
									if((UDSDialogMode == 0x01 || UDSDialogMode==0x02)||UDSSwitch == 0)
									{									
										UDSPositiveAnswer(0x04,i,0x00);
										UDSSwitch = 0;
										UDSDialogMode = 1;
									}
									else
									{
										UDSNegtiveAnswer(0x05,i,0xFF);
									}
								}
								else if(UDSSubService[i] == 0x02)
								{
									UDSPositiveAnswer(0x04,i,0x00);
									UDSSwitch = 1;
									UDSDialogMode = 2;									
								}
								else if(UDSSubService[i] == 0x03)
								{
									if(UDSSwitch == 1)
									{
										UDSPositiveAnswer(0x04,i,0x00);
										UDSSwitch = 1;
										UDSDialogMode = 3;
			                		}
			                		else
			                		{
										UDSNegtiveAnswer(0x05,i,0xFF);
			                		}

								}
								else if(UDSSubService[i] == 0x11)   //make NB software reset
								{
									if(UDSDialogMode == 2)
									{
										UDSPositiveAnswer(0x04,i,0x00);										
										osDelay(100);
										appSaveNVMData();
										appSetCFUN(0);
										osDelay(1000);
			                			EC_SystemReset();
			                		}
			                		else
			                		{
										UDSNegtiveAnswer(0x05,i,0xFF);  
			                		}
								}								
								else
								{
									UDSNegtiveAnswer(0x05,i,0xEE);
								}
								break;
							case 0x22:
								if(UDSDialogMode == 2)
								{
									if(UDSSubService[i] == 0x01)		//check the sw of NB
									{
										UDSPositiveAnswer(0x08,i,APPSWVERSION);
									}
									else if(UDSSubService[i] == 0x02)  //check the hw of NB
									{
										UDSPositiveAnswer(0x06,i,HWVERSION);										
									}
									else if(UDSSubService[i] == 0x03)  //check the SN number
									{	
										UDSAnswer.DLC = 8;
										switch(UDSSubServiceActionCode[i])
										{
											case 00:
												UDSAnswer.Id = 0x20;
												for(k=0; k<8; k++)
												{
													UDSAnswer.Data[k] = AppNVMData.battSN[k+8*0];
												}
												ret = HAL_Can_Transmit(UDSAnswer);
												break;
												
											case 01:
												UDSAnswer.Id = 0x21;
												for(k=0; k<8; k++)
												{
													UDSAnswer.Data[k] = AppNVMData.battSN[k+8*1];
												}
												ret = HAL_Can_Transmit(UDSAnswer);
												break;

											case 02:
												UDSAnswer.Id = 0x22;
												UDSAnswer.Data[0] = AppNVMData.battSN[16];;
												for(k=1; k<8; k++)
												{
													UDSAnswer.Data[k] = 0x00;
												}
												ret = HAL_Can_Transmit(UDSAnswer);
												break;
											default:
											break;											
										}									
									}
									else if(UDSSubService[i] == 0x04)  //check the batt message
									{									
										osStatus_t result = osMutexAcquire(UartMutex, osWaitForever);
										if(CanRxMsg[i].Data[2] == 0x00)
										{
											MEMCPY(battInfo,UartReadMsg.data,14*8);
										/*
											#ifdef USING_PRINTF
											printf("battInfo=\n");
											for(int jj = 0;jj<14*8;jj++)
												printf("%x ",battInfo[jj]);
											printf("\n");
											#endif
										*/
										}
										osMutexRelease(UartMutex);

										UDSAnswer.Id = 0x30+UDSSubServiceActionCode[i];
										UDSAnswer.DLC = 8;

										MEMCPY(UDSAnswer.Data, &(battInfo[UDSSubServiceActionCode[i]*8]),8);
										ret = HAL_Can_Transmit(UDSAnswer);										
									}
									else if(UDSSubService[i] == 0x05)  //check the enviroment temp of NB
									{
										UDSPositiveAnswer(0x08,i,0x00);  //ntcvalue										
									}									
									else if(UDSSubService[i] == 0x06)	//check the tcp link of NB
									{
										UDSPositiveAnswer(0x06,i,socContext.status);  //TcpconnectStatus
									}
									else if(UDSSubService[i] == 0x07)  //check the GPS link of NB
									{
										posCode =(UINT32) GpsData[1];
										UDSPositiveAnswer(0x05,i,posCode);  //gps satellite num(uint8), should be modified
									}	
									else
									{
										UDSNegtiveAnswer(0x05,i,0xEE);
									}
								}
								else	//the service is not surpported in current dialog mode
								{
									UDSNegtiveAnswer(0x05,i,0xFF);
								}
								break;
							case 0x2E:   //write service
								if(UDSDialogMode == 2)
								{
									if(UDSSubService[i] == 0x03)  // write the battSN
									{
										if(UDSSubServiceActionCode[i] == 0x00 && snFlag == 0x00)
										{
											for(j=0; j<5; j++)
											{
												UDSBattSN[j+5*0] = CanRxMsg[i].Data[j+3];
											}	
											snFlag = snFlag|0x01;
										}
										else if(UDSSubServiceActionCode[i] == 0x01 && snFlag == 0x01)
										{
											for(j=0; j<5; j++)
											{
												UDSBattSN[j+5*1] = CanRxMsg[i].Data[j+3];
											}
											snFlag = snFlag|0x02;
										}
										else if(UDSSubServiceActionCode[i] == 0x02 && snFlag == 0x03)
										{
											for(j=0; j<5; j++)
											{
												UDSBattSN[j+5*2] = CanRxMsg[i].Data[j+3];
											}
											snFlag = snFlag|0x04;
										}
										else if(UDSSubServiceActionCode[i] == 0x03 && snFlag == 0x07)
										{
											for(j=0; j<2; j++)
											{
												UDSBattSN[j+5*3] = CanRxMsg[i].Data[j+3];
											}
											snFlag = snFlag|0x08;
										}

										if(snFlag == 0x0F)
										{
											snFlag = 0;
											UDSPositiveAnswer(0x04,i,00);
											MEMCPY(AppNVMData.battSN, UDSBattSN, BATT_SN_LEN);
											AppNVMData.EOLState = 1; //SN号写入完成，表明已经进行过下线配置
											appSaveNVMData();
										}
										else
										{
											UDSAnswer.Id = 0x7A8;
											UDSAnswer.DLC = 8;
											UDSAnswer.Data[0] = 0x05;
											UDSAnswer.Data[1] = 0x3E;
											UDSAnswer.Data[2] = UDSService[i];
											UDSAnswer.Data[3] = UDSSubService[i];
											UDSAnswer.Data[4] = snFlag;	
											UDSAnswer.Data[5] = 0x00;
											UDSAnswer.Data[6] = 0x00;
											UDSAnswer.Data[7] = 0x00;
											ret = HAL_Can_Transmit(UDSAnswer);
										}										
									}
									else if(UDSSubService[i] == 0x0F) //write the update config:updateDifferDataByteLen
									{										
										updateDifferDataByteLen = (CanRxMsg[i].Data[2]<<16)|(CanRxMsg[i].Data[3]<<8)|CanRxMsg[i].Data[4];										
										UDSPositiveAnswer(0x04,i,00);
									}
									else
									{
										UDSNegtiveAnswer(0x05,i,0xEE);
										
									}
									break;
								}								
								else
								{
									UDSNegtiveAnswer(0x05,i,0xFF);
								}
								break;
							case 0x31:  //clear the flash service
								if(UDSDialogMode == 3)  
								{
									if(UDSSubService[i] == 0x0F)  //clear the fota flash
									{
										if(UDSSubServiceActionCode[i] == 0x01)
										{
											//Clear the Flash
											boolRet = UDSClearFotaDownloadRegion();
											//if ok											
											UDSPositiveAnswer(0x05,i,(UINT32)boolRet);
										}
										else
										{
											UDSNegtiveAnswer(0x06,i,UDSSubServiceActionCode[i]<<8|0xFF);  //重点测试
										}										
									}
									else
									{
										UDSNegtiveAnswer(0x05,i,0xEE);  //the subservice is not surpported
									}
								}
								else
								{
									UDSNegtiveAnswer(0x05,i,0xFF);  //the survie is not surpported in current dialog mode
								}
								break;
							case 0x34:  //prepare for some one process
								if(UDSDialogMode == 3)
								{
									if(UDSSubService[i] == 0x0F)  //ask for download update data
									{									
										boolRet = UDSAskforDownLoadData();
										//if ok											
										UDSPositiveAnswer(0x05,i,(UINT32)boolRet);
									}
									else
									{
										UDSNegtiveAnswer(0x05,i,0xEE);
									}
								}
								else
								{
									UDSNegtiveAnswer(0x05,i,0xFF);
								}
								break;
							case 0x36:  //download the update data
								if(UDSDialogMode == 3)
								{
									if(downloadReady == TRUE)
									{
										counter++;  //记录报文数量									
										tempSliceCounter =  (CanRxMsg[i].Data[1]<<16)|(CanRxMsg[i].Data[2]<<8)|(CanRxMsg[i].Data[3]);
									/*
									#ifdef USING_PRINTF
										printf("tempSliceCounter = %x\n",tempSliceCounter);
									#endif
									*/										
										if(tempSliceCounter < (updateDifferDataPackageCounter+1)*25 && tempSliceCounter>=updateDifferDataPackageCounter*25)
										{
											updateDifferDataSliceCounter = tempSliceCounter % 25;
											for(j=0;j<4;j++)
											{
												updateDataBuffer[updateDifferDataSliceCounter*4+j] = CanRxMsg[i].Data[4+j];
											}								
											
											if(updateDifferDataSliceCounter>0)
											{
												sliceCounterFlag = sliceCounterFlag |(0x01<<updateDifferDataSliceCounter);
											}
											else
											{
												sliceCounterFlag = sliceCounterFlag |0x01;
											}
											/*
										#ifdef USING_PRINTF
											printf("sliceCounterFlag = %x  counter=%d\n",sliceCounterFlag,counter);
										#endif
										*/											
										}
										if(counter == 25)
										{
											counter = 0;
											
											if(sliceCounterFlag == 0x1FFFFFF)  //received all the 25 message of current package
											{
												sliceCounterFlag = 0x0;
												updateDifferDataPackageCounter++;
											/*
											#ifdef USING_PRINTF
												printf("updateDifferDataPackageCounter = %d\n",updateDifferDataPackageCounter);

												printf("updateDataBuffer\n");
												for(k=0;k<100;k++)
												{
													printf("%x  ",updateDataBuffer[k]);
												}
												printf("\n");
												
												printf("startAddr = 0x%x,endAddr = 0x%x\n",
													FLASH_FOTA_REGION_START+(updateDifferDataPackageCounter-1)*100,FLASH_FOTA_REGION_START+updateDifferDataPackageCounter*100-1);
											#endif
											*/			
												//write the buffer(100 byte) to flash
												ret = (uint8_t)BSP_QSPI_Write_Safe(updateDataBuffer,FLASH_FOTA_REGION_START+(updateDifferDataPackageCounter-1)*100,100);
												errorCount = 0;
												while(ret != QSPI_OK && errorCount<3)  //try to write most 3 times
												{
													errorCount++;
													BSP_QSPI_Erase_Safe(FLASH_FOTA_REGION_START+(updateDifferDataPackageCounter-1)*100,100);
													ret = (uint8_t)BSP_QSPI_Write_Safe(updateDataBuffer,FLASH_FOTA_REGION_START+(updateDifferDataPackageCounter-1)*100,100);
												}
												if(ret == QSPI_OK)
												{
													UDSPositiveAnswer(0x08,i,tempSliceCounter);												
												}
												else
												{
													UDSNegtiveAnswer(0x08,i,0xFFFFFFFF);  //failed to write this package to flash
												}
												memset(updateDataBuffer,0, 100);
												sliceCounterFlag = 0x0;
												
											}
											else
											{
												//received 25 messages, but lose one or more messages
												//in this case, updater will try to send the package again(most 4 times)
												UDSNegtiveAnswer(0x08,i,sliceCounterFlag);  
												//sliceCounterFlag = 0x0;
												/*
											#ifdef USING_PRINTF
												printf("updateDifferDataPackageCounter = %d\n",updateDifferDataPackageCounter+1);
												printf("updateDataBuffer:\n");
												for(k=0;k<100;k++)
												{
													printf("%x  ",updateDataBuffer[k]);
												}
												printf("\n");
											#endif
											*/
												//memset(updateDataBuffer,0, 100);
											}
										}
									}
									else
									{
										UDSNegtiveAnswer(0x05,i,0xEF); //the download process is not ready
									}
								}
								else
								{
									UDSNegtiveAnswer(0x05,i,0xFF);  //the service is not surpported in current dialog mode
								}
								break;
							case 0x37:  //exit some one process
								if(UDSDialogMode == 3)
								{
									if(UDSSubService[i] == 0x0F)  // exit the download 
									 {
										UDSPositiveAnswer(0x04,i,0x00);
										downloadReady = FALSE;
									 }
									 else
									 {
										UDSNegtiveAnswer(0x05,i,0xEE);
									}
								}
								else
								{
									UDSNegtiveAnswer(0x05,i,0xFF);
								}
								break;
							default:								
								UDSNegtiveAnswer(0x04,i,0xFF);  //the service is not surpported
								break;
								
						}
					}
				}
			}
	//}
	for(i=0; i<2; i++)
	{
		UDSService[i] = 0;
		UDSSubService[i] = 0;
	}
}

UINT8 UDSPositiveAnswer(UINT8 answerLen,UINT8 messageIndex,UINT32 posCode)
{
	CAN_Msg_Type UDSAnswer;
	UINT8 ret;
	UDSAnswer.Id = 0x7A8;
	UDSAnswer.DLC = 8;
	UDSAnswer.Data[0] = answerLen;
	UDSAnswer.Data[1] = 0x78;
	UDSAnswer.Data[2] = UDSService[messageIndex];
	UDSAnswer.Data[3] = UDSSubService[messageIndex];
	if(answerLen == 4)
	{
		UDSAnswer.Data[4] = 0x00;
		UDSAnswer.Data[5] = 0x00;
		UDSAnswer.Data[6] = 0x00;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==5)
	{	UDSAnswer.Data[4] = posCode;
		UDSAnswer.Data[5] = 0x00;
		UDSAnswer.Data[6] = 0x00;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==6)
	{
		UDSAnswer.Data[4] = posCode>>8;
		UDSAnswer.Data[5] = posCode;
		UDSAnswer.Data[6] = 0x00;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==7)
	{
		UDSAnswer.Data[4] = posCode>>16;
		UDSAnswer.Data[5] = posCode>>8;
		UDSAnswer.Data[6] = posCode;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==8)
	{
		UDSAnswer.Data[4] = posCode>>24;
		UDSAnswer.Data[5] = posCode>>16;
		UDSAnswer.Data[6] = posCode>>8;
		UDSAnswer.Data[7] = posCode;
	}
	ret = HAL_Can_Transmit(UDSAnswer);
	return ret;
}

UINT8 UDSNegtiveAnswer(UINT8 answerLen,UINT8 messageIndex, UINT32 negCode)
{
	CAN_Msg_Type UDSAnswer;
	UINT8 ret;
	UDSAnswer.Id = 0x7A8;
	UDSAnswer.DLC = 8;
	UDSAnswer.Data[0] = answerLen;
	UDSAnswer.Data[1] = 0x7F;
	UDSAnswer.Data[2] = UDSService[messageIndex];
	UDSAnswer.Data[3] = UDSSubService[messageIndex];
	if(answerLen==4)
	{
		UDSAnswer.Data[3] = negCode;
		UDSAnswer.Data[4] = 0x00;
		UDSAnswer.Data[5] = 0x00;
		UDSAnswer.Data[6] = 0x00;
		UDSAnswer.Data[7] = 0x00;
	}
	if(answerLen==5)
	{	UDSAnswer.Data[4] = negCode;
		UDSAnswer.Data[5] = 0x00;
		UDSAnswer.Data[6] = 0x00;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==6)
	{
		UDSAnswer.Data[4] = negCode>>8;
		UDSAnswer.Data[5] = negCode;
		UDSAnswer.Data[6] = 0x00;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==7)
	{
		UDSAnswer.Data[4] = negCode>>16;
		UDSAnswer.Data[5] = negCode>>8;
		UDSAnswer.Data[6] = negCode;
		UDSAnswer.Data[7] = 0x00;
	}
	else if(answerLen==8)
	{
		UDSAnswer.Data[4] = negCode>>24;
		UDSAnswer.Data[5] = negCode>>16;
		UDSAnswer.Data[6] = negCode>>8;
		UDSAnswer.Data[7] = negCode;
	}
	ret = HAL_Can_Transmit(UDSAnswer);
	return ret;
}



BOOL UDSAskforDownLoadData()
{
	updateDifferDataPackageCounter = 0;
	downloadReady = true;
	
	return TRUE;
}

BOOL UDSClearFotaDownloadRegion()
{
	UINT8 ret = FALSE;
	ret = BSP_QSPI_Erase_Safe(FLASH_FOTA_REGION_START, 0x78000); //512k-32k = 480k -> 0x75300
	
	if(ret == QSPI_OK)
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}
}