#include "stm32f10x.h"

#include <string.h>
#include "CoAP_Util.h"

static uint16_t CoAP_Length;
static uint16_t Current_Option_Number;
static uint8_t * CoAP_Message;

void Insert(uint8_t * dest, uint16_t dest_start, uint8_t * source, uint16_t source_start, uint16_t len)
{
	uint16_t i = 0;
	while(i < len)
	{
		dest[dest_start + i] = source[source_start + i];
		i++;
	}
}
void insert(uint16_t start, uint8_t * source, uint16_t len)
{
	Insert(CoAP_Message, start, source, 0, len);
}
uint16_t CoAP_Get_Length()
{
	return CoAP_Length;
}
void CoAP_Init(uint8_t * coap_message)
{
	CoAP_Message = coap_message;
	CoAP_Length = 4;
	Current_Option_Number = 0;
	CoAP_Message[0] |= 0x01 << 6; // 协议版本，固定为1
}
// 报文类型，00CON，01NON，10ACK，11Rest
void CoAP_Set_T(uint8_t t)
{
	CoAP_Message[0] |= t << 4;
}
// 请求类型
void CoAP_Set_Code(uint8_t code)
{
	CoAP_Message[1] = code;
}
// messageID
void CoAP_Set_MessageID(uint16_t messageID)
{
	uint8_t message_id[2];
	message_id[0] = (messageID >> 8) & 0xFF;
	message_id[1] = messageID & 0xFF;
	insert(2, message_id, sizeof(message_id));
}
// token  len不能大于8
void CoAP_Set_Token(uint8_t * token, uint8_t len)
{
	if(len > 8)
	{
		len = 8;
	}
	CoAP_Message[0] |= len & 0xFF;
	insert(CoAP_Length, token, len);
	CoAP_Length += len;
}
// 设置option
void CoAP_Set_Option(uint16_t number, uint16_t length, uint8_t * value)
{
	uint16_t pos_start = CoAP_Length, opt_length = 1;
	int32_t delta = number - Current_Option_Number;
	
	if (delta < 0){
		return;
	} else if (delta < 13){
		CoAP_Message[pos_start] |= (delta & 0xFF) << 4;
	}else if (delta < 255+13){
		CoAP_Message[pos_start] |= (0x0D & 0xFF) << 4;
		uint8_t ext = delta - 0x0D;
		CoAP_Message[pos_start + opt_length++] = ext;
	}else if (delta < 65535){
		CoAP_Message[pos_start] |= (0x0E & 0xFF) << 4;
		uint16_t ext = (delta - 0x0E - 0xFF);
		CoAP_Message[pos_start + opt_length++] = (ext >> 8) & 0xFF;
		CoAP_Message[pos_start + opt_length++] = ext & 0xFF;
	}
	// length
	if (length < 1){
		return;
	}else if (length < 13){
		CoAP_Message[pos_start] |= (length & 0xFF);
	}else if (length < 255+13){
		CoAP_Message[pos_start] |= (0x0D & 0xFF);
		uint8_t ext = length - 0x0D;
		CoAP_Message[pos_start + opt_length++] = ext;
	}else if (length < 65535){
		CoAP_Message[pos_start] |= (0x0E & 0xFF);
		uint16_t ext = (length - 0x0E - 0xFF);
		CoAP_Message[pos_start + opt_length++] = (ext >> 8) & 0xFF;
		CoAP_Message[pos_start + opt_length++] = ext & 0xFF;
	}
	
	insert(pos_start + opt_length, value, length);
	opt_length += length;
	CoAP_Length += opt_length;
	Current_Option_Number = number;
}
// 设置字符串类型的option
void CoAP_Set_Option_Str(uint16_t number, char * message)
{
	uint16_t len = strlen(message);
	uint8_t value[len];
	for(uint16_t i = 0; i < len; i++)
	{
		value[i] = message[i];
	}
	CoAP_Set_Option(number, len, value);
}

// 设置short类型的option
void CoAP_Set_Option_Short(uint16_t number, uint16_t num)
{
	if(num < 256)
	{
		uint8_t value[1];
		value[0] = num;
		CoAP_Set_Option(number, sizeof(value), value);
	}
	else
	{
		uint8_t value[2];
		value[0] = (num >> 8) & 0xFF;
		value[1] = num & 0xFF;
		CoAP_Set_Option(number, sizeof(value), value);
	}
}

// 设置payload
void CoAP_Set_Payload(uint8_t * payload, uint16_t length)
{
	uint16_t pos = CoAP_Length;
	CoAP_Message[pos] = 0xFF;
	insert(pos + 1, payload, length);
	CoAP_Length += length + 1;
}

void CoAP_Set_Payload_Str(char * payload)
{
	uint16_t len = strlen(payload);
	uint8_t value[len];
	for(uint16_t i = 0; i < len; i++)
	{
		value[i] = payload[i];
	}
	CoAP_Set_Payload(value, len);
}








// 报文解析
// 获取版本号
uint8_t CoAP_Get_Ver(uint8_t * coap_message)
{
	uint8_t ver = (coap_message[0] & 0xC0) >> 6;
	return ver;
}
// 获取报文类型
uint8_t CoAP_Get_T(uint8_t * coap_message)
{
	uint8_t t = (coap_message[0] & 0x30) >> 4;
	return t;
}
// 获取tkl
uint8_t CoAP_Get_Tkl(uint8_t * coap_message)
{
	uint8_t tkl = coap_message[0] & 0x0F;
	return tkl;
}
// 获取请求方式
uint8_t CoAP_Get_Code(uint8_t * coap_message)
{
	uint8_t code = coap_message[1];
	return code;
}
// 获取messageID
uint16_t CoAP_Get_MessageID(uint8_t * coap_message)
{
	uint16_t messageID = 0;
	messageID = (messageID | coap_message[2]) << 8;
	messageID = messageID | coap_message[3];
	return messageID;
}
// 获取token
uint8_t * CoAP_Get_Token(uint8_t * coap_message, uint8_t * token)
{
	uint8_t tkl = CoAP_Get_Tkl(coap_message);
	for(uint8_t i = 0; i < tkl; i++)
	{
		token[i] = coap_message[4 + i];
	}
	return token;
}


struct Option_Struct
{
	uint8_t valid; // 是否合法
	uint16_t number; // 编号
	uint16_t length; // 长度
	uint16_t pos; // 位置
	uint8_t delta_ext; // 编号扩展，字节
	uint8_t length_ext; // 长度扩展，字节
};

struct Option_Struct Get_Option(uint8_t * coap_message, uint16_t pos, uint16_t number)
{
	struct Option_Struct option_struct = {
		.valid = 1,
		.number = number,
		.length = coap_message[pos] & 0x0F,
		.pos = pos,
		.delta_ext = 0,
		.length_ext = 0,
	};
	uint8_t delta = (coap_message[pos] & 0xF0) >> 4;
	if(delta < 0x0D)
	{
		option_struct.delta_ext = 0;
		option_struct.number += delta;
	}
	else if(delta == 0x0D)
	{
		option_struct.delta_ext = 1;
		uint8_t num = coap_message[pos + 1];
		option_struct.number += (0x0D + num);
	}
	else if(delta == 0x0E)
	{
		option_struct.delta_ext = 2;
		uint16_t num = 0;
		num = (num | coap_message[pos + 1]) << 8;
		num = num | coap_message[pos + 2];
		option_struct.number += (0x0E + 0xFF + num);
	}
	else
	{
		option_struct.valid = 0;
		return option_struct;
	}
	
	if(option_struct.length < 0x0D)
	{
		option_struct.length_ext = 0;
	}
	else if(option_struct.length == 0x0D)
	{
		option_struct.length_ext = 1;
		uint8_t num = coap_message[pos + option_struct.delta_ext + 1];
		option_struct.length = (0x0D + num);
	}
	else if(option_struct.length == 0x0E)
	{
		option_struct.length_ext = 2;
		uint16_t num = 0;
		num = (num | coap_message[pos + option_struct.delta_ext + 1]) << 8;
		num = num | coap_message[pos + option_struct.delta_ext + 2];
		option_struct.length = (0x0E + 0xFF + num);
	}
	else
	{
		option_struct.valid = 0;
		return option_struct;
	}
	return option_struct;
}

// 获取option长度
struct Option_Struct CoAP_Get_Option(uint8_t * coap_message, uint16_t coap_length, uint16_t number)
{
	uint8_t tkl = CoAP_Get_Tkl(coap_message);
	uint16_t pos = 4 + tkl;
	uint16_t current_number = 0;
	struct Option_Struct option_struct;
	
	while(pos < coap_length)
	{
		option_struct = Get_Option(coap_message, pos, current_number);
		if(option_struct.valid == 0 || option_struct.number == number)
		{
			break;
		}
		else
		{
			current_number = option_struct.number;
			pos = option_struct.pos + option_struct.delta_ext + option_struct.length_ext + option_struct.length + 1; // 下一个循环的位置起点
		}
	}
	return option_struct;
}



// 获取option长度
uint16_t CoAP_Get_Option_Length(uint8_t * coap_message, uint16_t coap_length, uint16_t number)
{
	uint16_t length = 0;
	struct Option_Struct option_struct = CoAP_Get_Option(coap_message, coap_length, number);
	if(option_struct.valid)
	{
		length = option_struct.length;
	}
	return length;
}
// 获取option value
uint8_t * CoAP_Get_Option_Value(uint8_t * coap_message, uint16_t coap_length, uint16_t number, uint8_t * value)
{
	struct Option_Struct option_struct = CoAP_Get_Option(coap_message, coap_length, number);
	if(option_struct.valid)
	{
		for(uint8_t i = 0; i < option_struct.length; i++)
		{
			value[i] = coap_message[option_struct.pos + option_struct.delta_ext + option_struct.length_ext + 1 + i];
		}
	}
	else
	{
//		value[0] = 0x99;
	}
	return value;
}

struct Payload_Struct
{
	uint8_t valid;
	uint16_t pos;
	uint16_t length;
};
// 获取payload
struct Payload_Struct Get_Payload(uint8_t * coap_message, uint16_t coap_length)
{
	uint8_t tkl = CoAP_Get_Tkl(coap_message);
	uint16_t pos = 4 + tkl;
	uint16_t current_number = 0;
	struct Option_Struct option_struct;
	struct Payload_Struct payload_struct = {
		.valid = 1,
		.pos = 0,
		.length = 0
	};
	
	while(pos < coap_length)
	{
		if(coap_message[pos] == 0xFF)
		{
			payload_struct.valid = 1;
			payload_struct.pos = pos + 1;
			payload_struct.length = coap_length - pos - 1;
			break;
		}
		option_struct = Get_Option(coap_message, pos, current_number);
		if(option_struct.valid == 0)
		{
			payload_struct.valid = 0;
			payload_struct.length = 0;
			payload_struct.pos = 0;
			break;
		}
		else
		{
			current_number = option_struct.number;
			pos = option_struct.pos + option_struct.delta_ext + option_struct.length_ext + option_struct.length + 1; // 下一个循环的位置起点
		}
	}
	return payload_struct;
}


// 获取payload长度
uint16_t CoAP_Get_Payload_Length(uint8_t * coap_message, uint16_t coap_length)
{
	struct Payload_Struct payload_struct = Get_Payload(coap_message, coap_length);
	return payload_struct.length;
}

// 获取payload值
uint16_t CoAP_Get_Payload(uint8_t * coap_message, uint16_t coap_length, uint8_t * payload)
{
	struct Payload_Struct payload_struct = Get_Payload(coap_message, coap_length);
	if(payload_struct.valid)
	{
		for(uint8_t i = 0; i < payload_struct.length; i++)
		{
			payload[i] = coap_message[payload_struct.pos + i];
		}
	}
	else
	{
		
	}
	return payload_struct.length;
}