#include "UDS.h"
#include <stdio.h>

//bootloader
uint8_t DataFormatIdentifier = 0;
uint8_t addressAndLengthFormatIdentifier = 0;

const uint16_t MaxNumberOfBlockLength = 128;

uint32_t load_MemoryAddress = 0;
uint32_t load_MemorySize = 0;
uint32_t load_BlockNum = 0;
uint32_t load_BlockCount = 0;

uint32_t earse_MemoryAddress = 0;
uint32_t earse_MemorySize = 0;

uint8_t load_sequence_state = 0;//0=请求升级，1=擦除成功进入数据传输流程，2=请求下载数据，3=正在下载数据，
uint16_t BlockSequenceCounter = 0;
uint16_t BlockSequenceCounter_pre = 0;

uint32 CRC32_value = 0;
uint32 CRC32_value_app2 = 0;
uint32 WriteFlashAlreadySize = 0;

uint8_t	DiagnosticSessionType = 0x02;

extern uint8_t ota_start_flag;

uint8_t app2_crc_flag = 0;//crc校验成功标志位，成功后应答版本号位新的。

void NegativeResponse(uint8_t sid,uint8_t NegativeResponseCode)
{
	i15765_t msg_req;
	uint8_t buf[8];
	uint8_t status_rq;

	msg_req.ID = UDS_TX_ID;
	msg_req.buf = buf;

	buf[0] = 0x7F;//NegativeResponseServiceIdentifier
	buf[1] = sid;
	buf[2] = NegativeResponseCode;

	msg_req.buf_len = 3;

	/* transmit message */
	i15765_tx_app(&msg_req, &status_rq);
}

void SID_11_function(i15765_t *msg)
{
	i15765_t msg_req;
	uint8_t buf[8];
	uint8_t status_rq;
	uint8_t PositiveResponseEnable = 0;

	uint8_t SuppressPosRspMsgIndicationBit;
	uint8_t ResetType;

	/* basic stuff */
	msg_req.buf = buf;
	msg_req.pri = 6;
	msg_req.ta = 0x0C;

	if(msg->tat == I15765_TAT_NF11)
	{
		msg_req.tat = I15765_TAT_NF11;
	}
	else
	{
		msg_req.tat = I15765_TAT_NP11;
	}

#if    0
	if(DiagnosticSessionType==0x02)
	{
		NegativeResponse(msg->buf[0],0x7F);
		return;
	}
#endif

	if(DiagnosticSessionType==0x01)
	{
		NegativeResponse(msg->buf[0],0x7F);
		return;
	}


	if(msg->buf_len!=2)
	{
		NegativeResponse(msg->buf[0],0x13);
		return;
	}

	SuppressPosRspMsgIndicationBit = msg->buf[1]&0x80;
	ResetType = msg->buf[1]&0x7F;

	switch(ResetType)
	{
		case 0x01:
		{
			if(1)
			{
				PositiveResponseEnable = 1;
				buf[0] = msg->buf[0]+0x40;// PositiveResponseServiceIdentifier
				buf[1] = msg->buf[1];// Sub-Function=ResetType

				msg_req.buf_len = 2;
				if(SuppressPosRspMsgIndicationBit != 0x80)
				{
					i15765_tx_app(&msg_req, &status_rq);
				}
			}
			else
			{
				PositiveResponseEnable = 0;
				NegativeResponse(msg->buf[0],0x22);
				return;
			}

			break;
		}
		case 0x03:
		{

			if(1)
			{
				PositiveResponseEnable = 1;
				buf[0] = msg->buf[0]+0x40;// PositiveResponseServiceIdentifier
				buf[1] = msg->buf[1];// Sub-Function=ResetType

				msg_req.buf_len = 2;
				if(SuppressPosRspMsgIndicationBit != 0x80)
				{
					i15765_tx_app(&msg_req, &status_rq);
				}
			}
			else
			{
				PositiveResponseEnable = 0;
				NegativeResponse(msg->buf[0],0x22);
				return;
			}

			break;
		}
		default:
		{
			NegativeResponse(msg->buf[0],0x12);
			return;
		}
	}


	if(PositiveResponseEnable==1)
	{
		if(ResetType == 0x01)//HardReset
		{
			//JumpReset();
		}

		if(ResetType == 0x03)//SoftReset
		{
			//JumpReset();
		}
	}
}

void SID_2E_function(i15765_t *msg)
{
	//用于应答
	i15765_t msg_req;
	uint8_t buf[8];
	uint8_t status_rq;
	uint8_t PositiveResponseEnable = 0;//判断是否为肯定响应  1：肯定响应  0：否定响应
	uint16_t DID = 0;

	msg_req.buf = buf;

	//2 数据长度错误
	if(msg->buf_len < 4)
	{
		NegativeResponse(msg->buf[0],0x13);
	}

	//扩展诊断回话 或 编程会话
	if((DiagnosticSessionType==0x02)||(DiagnosticSessionType==0x03))
	{
		DID = (uint16_t)msg->buf[1]<<8 | msg->buf[2];

		//版本信息全读
		//EEPROM_read(VERSION_ADRESS,256,VersionInfo);

		switch(DID)
		{
			//VIN: 17 byte
			case 0xF190:
			{

				break;
			}
			default:
			{
				NegativeResponse(msg->buf[0],0x31);//请求超出范围
				break;
			}
		}
	}

	if(PositiveResponseEnable==1)
	{
//		EEPROM_write(VERSION_ADRESS,256,VersionInfo);

		buf[0] = msg->buf[0]+0x40;	// PositiveResponseServiceIdentifier
		buf[1] = msg->buf[1];		// DID MSB
		buf[2] = msg->buf[2];		// DID LSB

		msg_req.buf_len = 3;

		/* transmit message */
		i15765_tx_app(&msg_req, &status_rq);
	}
}

void SID_31_function(i15765_t *msg)
{
    i15765_t msg_req = {0};
    uint8_t response_buf[20] = {0};
    uint8_t status_rq = 0;
    uint8_t PositiveResponseEnable = 0;
	int oReturnCheck;//
    uint8_t result_code = 0x00;
    // 初始化请求消息
	msg_req.pri = 6;
	msg_req.ta = 0x0C;
	msg_req.buf = response_buf;
	if(msg->tat == I15765_TAT_NF11)
	{
		msg_req.tat = I15765_TAT_NF11;
	}
	else
	{
		msg_req.tat = I15765_TAT_NP11;
	}


    // 参数检查
    if (msg == NULL || msg->buf == NULL || msg->buf_len < 4)
    {
        NegativeResponse(0x31, 0x13); // 参数长度错误
        return;
    }

    // 检查RoutineControl类型
    uint8_t RoutineControlType = msg->buf[1] & 0x7F;
    if (RoutineControlType != 0x01) // 只支持StartRoutine
    {
        NegativeResponse(msg->buf[0], 0x12); // 不支持的服务类型
        return;
    }

    uint16_t RoutineIdentifier = (uint16_t)msg->buf[2] << 8 | msg->buf[3];

    // 处理不同的RoutineIdentifier
    switch (RoutineIdentifier)
    {
        case 0x0203: // 请求升级
            if (msg->buf_len != 4)
            {
                NegativeResponse(msg->buf[0], 0x13);
                return;
            }
            ota_start_flag = 1;
            result_code = 0x00;
            load_sequence_state = 0x00;
            app2_crc_flag = 0;
            PositiveResponseEnable = 1;
            break;

        case 0xFF00: // 擦除Flash
            if (msg->buf_len != 13)
            {
                NegativeResponse(msg->buf[0], 0x13);
                return;
            }

            // 提取擦除地址和大小
            earse_MemoryAddress = (uint32_t)((msg->buf[5] << 24) | (msg->buf[6] << 16) | (msg->buf[7] << 8) | msg->buf[8]);
            earse_MemorySize = (uint32_t)((msg->buf[9] << 24) | (msg->buf[10] << 16) | (msg->buf[11] << 8) | msg->buf[12]);

            // 验证地址范围
            //if ((earse_MemoryAddress != APP_START_ADDRESS) || (earse_MemorySize > (APP_STOP_ADDRESS - APP_START_ADDRESS)))
			//if ((earse_MemoryAddress != APP2_ADDRESS) || (earse_MemorySize > (APP2_ADDRESS - APP1_ADDRESS)))
			if ((earse_MemoryAddress != APP1_ADDRESS) || (earse_MemorySize > (APP2_ADDRESS - APP1_ADDRESS)))
            {
                NegativeResponse(msg->buf[0], 0x31);
                return;
            }

            // 执行擦除操作
            oReturnCheck = flash_erase_app(2);

            // 根据擦除结果设置响应
            result_code = (oReturnCheck == 0) ? 0x00 : 0x01;
            if (result_code == 0x00)
            {
                load_sequence_state = 0x01; // 进入数据传输流程
            }

            PositiveResponseEnable = 1;
            break;

        case 0x0202: // CRC32验证
            if (msg->buf_len != 8)
            {
                NegativeResponse(msg->buf[0], 0x13);
                return;
            }

            // 提取CRC32值
            CRC32_value = (uint32_t)(
                (msg->buf[4] << 24) |
                (msg->buf[5] << 16) |
                (msg->buf[6] << 8) |
                msg->buf[7]
            );

            // 验证CRC32
            result_code = (CRC32_value == crc32val) ? 0x00 : 0x01;
            if(result_code==0)//校验成功
            {
            	app2_crc_flag = 1;
            }
            else
            {
            	app2_crc_flag = 0;
            }
            PositiveResponseEnable = 1;
            break;

        case 0x0E0E: // 切换旧app版本
            if (msg->buf_len != 4)
            {
                NegativeResponse(msg->buf[0], 0x13);
                return;
            }

			response_buf[0] = msg->buf[0] + 0x40; // 正响应服务ID
			response_buf[1] = msg->buf[1];        // RoutineControlType
			response_buf[2] = msg->buf[2];        // RoutineIdentifier高字节
			response_buf[3] = msg->buf[3];        // RoutineIdentifier低字节
			response_buf[4] = 0;        // 结果代码

			// 设置响应消息长度
			msg_req.buf_len = 5;

			// 发送响应
			i15765_tx_app(&msg_req, &status_rq);
			jump_to_app(APP1_ADDRESS);
            break;

        case 0x0E0F: // 切换新app版本
            if (msg->buf_len != 4)
            {
                NegativeResponse(msg->buf[0], 0x13);
                return;
            }
#if 1
			//计算固件大小，判断是否可以双备份升级
			if(earse_MemorySize < (APP2_ADDRESS - APP1_ADDRESS))
			{
				//flash_copy_app();
				app2_copy_to_app1();
				//校验CRC
				crc32val = 0xFFFFFFFF;
				uint8_t app2_data[1];
				for(uint32_t i = 0;i < earse_MemorySize; i++)
				{
					app2_data[0] = *(volatile uint8_t*)(APP1_ADDRESS + i);
					crc32val = crc32(crc32val, app2_data, 1);
				}
				if(CRC32_value == crc32val)
				{
	                response_buf[0] = msg->buf[0] + 0x40; // 正响应服务ID
	                response_buf[1] = msg->buf[1];        // RoutineControlType
	                response_buf[2] = msg->buf[2];        // RoutineIdentifier高字节
	                response_buf[3] = msg->buf[3];        // RoutineIdentifier低字节
	                response_buf[4] = 0;        // 结果代码

	                // 设置响应消息长度
	                msg_req.buf_len = 5;

	                // 发送响应
	                i15765_tx_app(&msg_req, &status_rq);

					app_status_set(1,true);
					jump_to_app(APP1_ADDRESS);
				}
				else
				{
	                response_buf[0] = msg->buf[0] + 0x40; // 正响应服务ID
	                response_buf[1] = msg->buf[1];        // RoutineControlType
	                response_buf[2] = msg->buf[2];        // RoutineIdentifier高字节
	                response_buf[3] = msg->buf[3];        // RoutineIdentifier低字节
	                response_buf[4] = 1;        // 结果代码

	                // 设置响应消息长度
	                msg_req.buf_len = 5;

	                // 发送响应
	                i15765_tx_app(&msg_req, &status_rq);
	                return;
				}
			}
#endif

        default:
            NegativeResponse(msg->buf[0], 0x31); // 不支持的服务
            return;
    }

    // 发送正响应
    if (PositiveResponseEnable == 1)
    {
        // 构建响应数据
        response_buf[0] = msg->buf[0] + 0x40; // 正响应服务ID
        response_buf[1] = msg->buf[1];        // RoutineControlType
        response_buf[2] = msg->buf[2];        // RoutineIdentifier高字节
        response_buf[3] = msg->buf[3];        // RoutineIdentifier低字节
        response_buf[4] = result_code;        // 结果代码

        // 设置响应消息长度
        msg_req.buf_len = 5;

        // 发送响应
        i15765_tx_app(&msg_req, &status_rq);
    }
}


void SID_34_function(i15765_t *msg)
{
	i15765_t msg_req;
	uint8_t buf[8];
	uint8_t status_rq;
	uint8_t PositiveResponseEnable = 0;

	msg_req.buf = buf;

	if(msg->buf_len!=11)
	{
		NegativeResponse(msg->buf[0],0x13);
		return;
	}

	//3
	if(load_sequence_state != 0x01)
	{
		NegativeResponse(msg->buf[0],0x24);	//(24)
		return;
	}

	DataFormatIdentifier = msg->buf[1];
	addressAndLengthFormatIdentifier = msg->buf[2];//0x44
	//Flashַ
	load_MemoryAddress = (uint32_t)((uint32_t)msg->buf[3]<<24)|((uint32_t)msg->buf[4]<<16)|((uint32_t)msg->buf[5]<<8)|msg->buf[6];
	load_MemorySize = (uint32_t)((uint32_t)msg->buf[7]<<24)|((uint32_t)msg->buf[8]<<16)|((uint32_t)msg->buf[9]<<8)|msg->buf[10];

	if(load_MemorySize%MaxNumberOfBlockLength == 0)
	{
		load_BlockNum = load_MemorySize/MaxNumberOfBlockLength;
	}
	else
	{
		load_BlockNum = load_MemorySize/MaxNumberOfBlockLength + 1;
	}

#if   0
	//
	if(load_MemoryAddress != load_MemoryAddress_next)
	{
		NegativeResponse(msg->buf[0],0x31);	//
		return;
	}
	load_MemoryAddress_next = load_MemoryAddress + load_MemorySize;
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

#endif

	if((load_MemoryAddress == APP1_ADDRESS)&& load_MemorySize <= (APP2_ADDRESS - APP1_ADDRESS))
	{
		load_MemoryAddress = APP2_ADDRESS;//虽是APP1的固件地址，但写在备份区APP2地址
		PositiveResponseEnable = 1;
		buf[0] = msg->buf[0]+0x40;	// PositiveResponseServiceIdentifier
		buf[1] = msg->buf[1];		// LengthFormatIdentifier

		if(MaxNumberOfBlockLength > 0xFF)
		{
			//MaxNumberOfBlockLength
			buf[2] = (uint8)((MaxNumberOfBlockLength>>8)&0xFF);
			buf[3] = (uint8)(MaxNumberOfBlockLength&0xFF);
			msg_req.buf_len = 4;
		}
		else
		{
			//MaxNumberOfBlockLength
			buf[2] = (uint8) MaxNumberOfBlockLength;
			msg_req.buf_len = 3;
		}

		load_sequence_state = 0x02;
		BlockSequenceCounter_pre = 0;
		crc32val = 0xFFFFFFFF;
		load_BlockCount = 0;
	}
	else
	{
		NegativeResponse(msg->buf[0],31);//
		return;
	}


	if(PositiveResponseEnable==1)
	{
		/* transmit message */
		i15765_tx_app(&msg_req, &status_rq);
	}
}

void SID_36_function(i15765_t *msg)
{
	i15765_t msg_req;
	uint8_t buf[8];
	uint8_t status_rq;
	uint8_t PositiveResponseEnable = 0;

	uint16_t i = 0;

	int oReturnCheck;

	uint8_t TransferRequestParameterRecord[4096];

	msg_req.buf = buf;

	if(load_sequence_state != 0x02)
	{
		NegativeResponse(msg->buf[0],0x24);
		return;
	}

	BlockSequenceCounter = msg->buf[1];
	if(BlockSequenceCounter == 0x00)
	{
		if(BlockSequenceCounter_pre != 0xFF)
		{
			NegativeResponse(msg->buf[0],0x73);//
			return;
		}
	}
	else if(BlockSequenceCounter != (BlockSequenceCounter_pre+1))
	{
		NegativeResponse(msg->buf[0],0x73);//
		return;
	}
	BlockSequenceCounter_pre = BlockSequenceCounter;
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	for(i=0; i < msg->buf_len-2;i++)
	{
		TransferRequestParameterRecord[i] = msg->buf[2+i];
	}
	crc32val = crc32(crc32val, TransferRequestParameterRecord, msg->buf_len-2);
	oReturnCheck = flash_write_page((load_MemoryAddress + load_BlockCount*MaxNumberOfBlockLength), TransferRequestParameterRecord, msg->buf_len-2, false);

	if(!oReturnCheck)
	{
		WriteFlashAlreadySize = WriteFlashAlreadySize + msg->buf_len-2;
		//
		PositiveResponseEnable = 1;
		buf[0] = msg->buf[0]+0x40;	// PositiveResponseServiceIdentifier
		buf[1] = msg->buf[1];		// BlockSequenceCounter
		buf[2] = (uint8)((crc32val>>24)&0xFF);
		buf[3] = (uint8)((crc32val>>16)&0xFF);
		buf[4] = (uint8)((crc32val>>8)&0xFF);
		buf[5] = (uint8)((crc32val>>0)&0xFF);
		msg_req.buf_len = 6;
		/////////////////////////////////////////////////////////////////

		load_BlockCount++;
		if(load_BlockCount==load_BlockNum)
		{
			load_sequence_state = 0x03;
		}
		else
		{
			load_sequence_state = 0x02;
		}
	}
	else
	{
		// Indicate that the flash programming failed.
		NegativeResponse(msg->buf[0],0x70);//
		return;
	}
	////////////////////////////////////////////////////////////////////

	if(PositiveResponseEnable==1)
	{
		/* transmit message */
		i15765_tx_app(&msg_req, &status_rq);
	}
}

void SID_37_function(i15765_t *msg)
{
	i15765_t msg_req;
	uint8_t buf[8];
	uint8_t status_rq;

	/* basic stuff */
	msg_req.buf = buf;
	msg_req.pri = 6;
	msg_req.ta = 0x0C;

	if(msg->buf_len != 1)
	{
		NegativeResponse(msg->buf[0],0x13);	//
		return;
	}

//	if(load_sequence_state != 0x02)
	if(load_sequence_state != 0x03)
	{
		NegativeResponse(msg->buf[0],0x24);	//(24)
		return;
	}
	else
	{
		buf[0] = msg->buf[0]+0x40;	// PositiveResponseServiceIdentifier

		msg_req.buf_len = 1;
		/* transmit message */
		i15765_tx_app(&msg_req, &status_rq);

		//
		load_sequence_state = 0x00;
		load_BlockCount = 0;
		BlockSequenceCounter_pre = 0;
	}
}