#include "LE9xxCommand.h"

#include <unistd.h>

// SOF
#define SOF_CMD 0xAA // Command
#define SOF_RES 0x55 // Response

typedef enum
{
	FRM_RECV_SOF = 0,
	FRM_RECV_CMD_CODE,
	FRM_RECV_SUB_RES_CODE,
	FRM_RECV_LENGTH_H,
	FRM_RECV_LENGTH_L,
	FRM_RECV_DATA,
	FRM_RECV_CHECKSUM,
	FRM_RECV_COMPLETE,
	FRM_RECV_NUM
} FRM_RECV_STEP;

LE9xxCommand::LE9xxCommand(CComm *comm)
	: thread_(), mutex_(), terminated_(false), comm_(comm)
{
	pthread_mutex_init(&mutex_, NULL);

	// Create response list
	AddResponseList(CMD_CONNECT);
	AddResponseList(CMD_DISCONNECT);
	AddResponseList(CMD_DIN_GET);
	AddResponseList(CMD_AD_SPS_SET);
	AddResponseList(CMD_AD_RANGE_SET);
	AddResponseList(CMD_AD_CYCLE_SET);
	AddResponseList(CMD_AD_VALUE_GET);
	AddResponseList(CMD_AD_RUN);
	AddResponseList(CMD_AD_STOP);
	AddResponseList(CMD_DA_OUT_SET);
	AddResponseList(CMD_DA_OUT_GET);
	AddResponseList(CMD_DA_SWEEP_SET);
	AddResponseList(CMD_TEMP_SET);

	// Create response list
	AddCommandList(CMD_AD_RUN_RESULT);
	AddCommandList(CMD_AD_STOP_RESULT);
	AddCommandList(CMD_AD_TRANSFER_DATA);

	// Create a analize thread
	pthread_create(&thread_, NULL, LE9xxCommand::ThreadFunc, this);
}

LE9xxCommand::~LE9xxCommand()
{
	pthread_mutex_destroy(&mutex_);
}

void *LE9xxCommand::ThreadFunc(void *p)
{
	LE9xxCommand *this_ptr = reinterpret_cast<LE9xxCommand *>(p);
	return reinterpret_cast<void *>(this_ptr->AnalyzeFrameData());
}

// Assemble frame data from receive buffer data.
// The parsed frame is stored in the command / response list.
uint32_t LE9xxCommand::AnalyzeFrameData()
{
	static FRM_RECV_STEP step = FRM_RECV_SOF;
	static FRAME recv;
	static uint32_t length_wk;
	uint8_t data, buff[1024], *pbuff;
	uint32_t buff_length;

	while (!terminated_)
	{
		buff_length = comm_->GetReceiveData(buff);
		pbuff = buff;

		while (buff_length-- != 0)
		{
			data = *(pbuff++);

			switch (step)
			{
			// SOF
			case FRM_RECV_SOF:
				if ((data == SOF_CMD) || (data == SOF_RES))
				{
					recv.sof = data;
					recv.sum = 0;
					step = FRM_RECV_CMD_CODE;
				}
				break;
			// Command code
			case FRM_RECV_CMD_CODE:
				recv.cmd = data;
				step = FRM_RECV_SUB_RES_CODE;
				break;
			// Sub command code
			case FRM_RECV_SUB_RES_CODE:
				recv.sub = data;
				step = FRM_RECV_LENGTH_H;
				break;
			// Data length (High byte)
			case FRM_RECV_LENGTH_H:
				recv.length = (uint32_t)data << 8;
				step = FRM_RECV_LENGTH_L;
				break;
			// Data length (Low byte)
			case FRM_RECV_LENGTH_L:
				recv.length |= (uint32_t)data;
				length_wk = 0;
				if (recv.length > 0)
				{
					step = FRM_RECV_DATA;
				}
				else
				{
					step = FRM_RECV_CHECKSUM;
				}
				break;
			// Data
			case FRM_RECV_DATA:
				if (length_wk < sizeof(recv.data))
				{
					recv.data[length_wk++] = data;
					if (length_wk == recv.length)
					{
						step = FRM_RECV_CHECKSUM;
					}
				}
				break;
			// Checksum
			case FRM_RECV_CHECKSUM:
				recv.sum += 1;
				if (recv.sum == data)
				{
					if (recv.sof == SOF_CMD)
					{
						// Set comanndo list
						SetCommandFrame(recv.cmd, &recv);
					}
					else if (recv.sof == SOF_RES)
					{
						// Set response list
						SetResponseFrame(recv.cmd, &recv);
					}
				}
				step = FRM_RECV_SOF;
				break;
			default:
				step = FRM_RECV_SOF;
				break;
			}

			recv.sum += data;
		}
		usleep(10000);
	}

	return 0;
}

// Send command or response.
// The checksum is calculated automatically.
// @param[in]	sof : Start of frame(0x55 or 0xAA)
// @param[in]	cmd : Comanndo code
// @param[in]	subres : Sub comannd code or Response code
// @param[in]	length : FRAME data length
// @param[in]	pdata : Transmission data
void LE9xxCommand::TransmitCommandResponse(uint8_t sof, uint8_t cmd, uint8_t subres, uint32_t length, uint8_t *pdata)
{
	uint8_t sum, sendbuff[1024], *pBuff = sendbuff;

	// Create frame data & Calculate checksum
	*pBuff = sof;
	sum = *pBuff++;
	*pBuff = cmd;
	sum += *pBuff++;
	*pBuff = subres;
	sum += *pBuff++;
	*pBuff = (uint8_t)(length >> 8);
	sum += *pBuff++;
	*pBuff = (uint8_t)(length);
	sum += *pBuff++;
	if ((length >= 0) && (pdata != NULL))
	{
		for (uint32_t i = 0; i < length; i++)
		{
			*pBuff = *(pdata + i);
			sum += *pBuff++;
		}
	}
	*pBuff = sum + 1;

	// Transmit frame
	comm_->Transmit(sendbuff, length + 6);
}

// Send command.
void LE9xxCommand::TransmitCommand(uint8_t cmd, uint8_t sub, uint32_t length, uint8_t *pdata)
{
	// Transmit frame
	TransmitCommandResponse(SOF_CMD, cmd, sub, length, pdata);
}

// Send response.
void LE9xxCommand::TransmitResponse(uint8_t cmd, uint8_t sub, uint32_t length, uint8_t *pdata)
{
	// Transmit frame
	TransmitCommandResponse(SOF_RES, cmd, sub, length, pdata);
}

// Send command & Waiting for a response.
void LE9xxCommand::TransmitCommandWait(uint8_t cmd, uint8_t sub, uint32_t length, uint8_t *pdata, FRAME *pRes)
{
	// Transmit frame
	TransmitCommandResponse(SOF_CMD, cmd, sub, length, pdata);

	// Waiting for a response
	while (!GetResponseFrame(cmd, pRes))
		;
}

// Waiting for a command form LE-9x0.
void LE9xxCommand::ReceiveCommandWait(uint8_t cmd, FRAME *pRes)
{
	// Waiting for a command
	while (!GetCommandFrame(cmd, pRes))
		;
}

// Convert the AD value of LE-910R to a real value.
// @param[in]	range : Measurement range
// @param[in]	ad : LE-910R analog readings
// @return		Calculated voltage / current / temperature
float LE9xxCommand::ConvertAnalogDataFromLE910(LE910_RANGE range, uint32_t ad)
{
	float tmp = 0;

	switch (range)
	{
	case LE910_RANGE_100mV:
		if (ad & 0x800000)
			tmp = -100 * (float)((~ad + 1) & 0xFFFFFF) / (float)0x7FFFFF;
		else
			tmp = 100 * (float)ad / (float)0x7FFFFF;
		break;
	case LE910_RANGE_1V:
		if (ad & 0x800000)
			tmp = -1 * (float)((~ad + 1) & 0xFFFFFF) / (float)0x7FFFFF;
		else
			tmp = 1 * (float)ad / (float)0x7FFFFF;
		break;
	case LE910_RANGE_10V:
		if (ad & 0x800000)
			tmp = -10 * (float)((~ad + 1) & 0xFFFFFF) / (float)0x7FFFFF;
		else
			tmp = 10 * (float)ad / (float)0x7FFFFF;
		break;
	case LE910_RANGE_30V:
		if (ad & 0x800000)
			tmp = -30 * (float)((~ad + 1) & 0xFFFFFF) / (float)0x7FFFFF;
		else
			tmp = 30 * (float)ad / (float)0x7FFFFF;
		break;

	case LE910_RANGE_I_250ohm:
	case LE910_RANGE_I_50ohm:
		tmp = 20 * ad / (float)0x7FFFFF;
		break;

	case LE910_RANGE_TC:
		if ((ad == 0x800000) || (ad == 0x7FFFFF))
		{ // Disconnection
			tmp = (float)0x800000;
		}
		else
		{
			if (ad & 0x800000)
				tmp = -1 * (float)(((float)(~ad & 0xFFFFFF) + 1) / 2560.);
			else
				tmp = (float)((float)ad / 2560.);
		}
		break;

	default:
		break;
	}

	return tmp;
}

// Convert the AD value of LE-928R to a real value.
// @param[in]	range : Measurement range
// @param[in]	ad : LE-928R analog readings
// @return		Calculated voltage
float LE9xxCommand::ConvertAnalogDataFromLE928(LE928_RANGE range, uint32_t ad)
{
	float tmp = 0;
	const int vmax[LE928_RANGE_NUM] = { 4, 8, 16, 30, 60 };

	if( ad & 0x800000 )
		tmp = -1 * vmax[range] * (float)((~ad + 1) & 0xFFFFFF) / (float)0x7FFFFF;
	else
		tmp = vmax[range] * (float)ad / (float)0x7FFFFF;

	return tmp;
}

// Convert the AD value of LE-930R to a real value.
// @param[in]	type : Output type
// @param[in]	ad : LE-930R analog readings
// @return		Calculated voltage / current
float LE9xxCommand::ConvertAnalogDataFromLE930(LE930_TYPE type, uint32_t ad)
{
	float tmp = 0;

	switch (type)
	{
	case LE930_TYPE_VOLTAGE_100mV:
		if (ad & 0x8000)
			tmp = -100 * (float)((~ad + 1) & 0xFFFF) / (float)0x7FFF;
		else
			tmp = 100 * (float)ad / (float)0x7FFF;
		break;

	case LE930_TYPE_VOLTAGE_10V:
		if (ad & 0x8000)
			tmp = -10 * (float)((~ad + 1) & 0xFFFF) / (float)0x7FFF;
		else
			tmp = 10 * (float)ad / (float)0x7FFF;
		break;

	case LE930_TYPE_CURRENT_INT:
	case LE930_TYPE_CURRENT_EXT:
		tmp = 20 * ad / (float)0x7FFF;
		break;
	}

	return tmp;
}

// Convert real values to AD values for LE-930R.
// @param[in]	type : Output type
// @param[in]	val : Voltage or Current value
// @return		AD value to set to LE-930R
uint32_t LE9xxCommand::ConvertRealDataLE930(LE930_TYPE type, float val)
{
	uint32_t ad = 0;

	switch (type)
	{
	case LE930_TYPE_VOLTAGE_100mV:
		if (val >= 0)
		{
			ad = (uint32_t)((0x7FFF * val + 50) / 100);
		}
		else
		{
			ad = (uint32_t)(((0x8000 * (-1 * val) + 50) / 100) - 1);
			ad = ~ad & 0xFFFF;
		}
		break;
	case LE930_TYPE_VOLTAGE_10V:
		if (val >= 0)
		{
			ad = (uint32_t)((0x7FFF * val + 5) / 10);
		}
		else
		{
			ad = (uint32_t)(((0x8000 * (-1 * val) + 5) / 10) - 1);
			ad = ~ad & 0xFFFF;
		}
		break;
	case LE930_TYPE_CURRENT_INT:
	case LE930_TYPE_CURRENT_EXT:
		ad = (uint32_t)((0x7FFF * val + 10) / 20);
		break;
	}

	return ad;
}

// Convert real values to AD values for LE-940R.
// @param[in]	type : Output type
// @param[in]	val : Voltage or Current value
// @return		AD value to set to LE-940R
uint32_t LE9xxCommand::ConvertRealDataLE940(LE940_TYPE type, float val)
{
	uint32_t ad = 0;

	switch (type)
	{
	case LE940_TYPE_VOLTAGE_32V:
	case LE940_TYPE_VOLTAGE_32V_1:
		if (val >= 0)
		{
			ad = (uint32_t)((0x7FFF * val + 16) / 32);
		}
		else
		{
			ad = (uint32_t)(((0x8000 * (-1 * val) + 16) / 32) - 1);
			ad = ~ad & 0xFFFF;
		}
		break;
	case LE940_TYPE_CURRENT:
	case LE940_TYPE_CURRENT_1:
		ad = (uint32_t)((0x7FFF * val + 10) / 20);
		break;
	}

	return ad;
}

// Add response list.
uint32_t LE9xxCommand::AddResponseList(uint8_t code)
{
	CMDRES_STAT stat;
	FRAME FRAME = {0};

	stat.fSet = false;
	stat.bCode = code;
	stat.Frame = FRAME;

	response_list_.push_back(stat);
	return response_list_.size();
}

// Set frame data in the response list.
void LE9xxCommand::SetResponseFrame(uint8_t code, FRAME *pframe)
{
	pthread_mutex_lock(&mutex_);

	for (uint32_t i = 0; i < response_list_.size(); i++)
	{
		if (response_list_[i].bCode == code)
		{
			if (!response_list_[i].fSet)
			{
				response_list_[i].Frame = *pframe;
				response_list_[i].fSet = true;
			}
			break;
		}
	}

	pthread_mutex_unlock(&mutex_);
}

// Get frame data in the response list.
bool LE9xxCommand::GetResponseFrame(uint8_t code, FRAME *pframe)
{
	bool ret = false;

	pthread_mutex_lock(&mutex_);

	for (uint32_t i = 0; i < response_list_.size(); i++)
	{
		if (response_list_[i].bCode == code)
		{
			if (response_list_[i].fSet)
			{
				*pframe = response_list_[i].Frame;
				response_list_[i].fSet = false;
				ret = true;
				break;
				;
			}
		}
	}

	pthread_mutex_unlock(&mutex_);

	return ret;
}

// Add command list.
uint32_t LE9xxCommand::AddCommandList(uint8_t code)
{
	CMDRES_STAT stat;
	FRAME FRAME = {0};

	stat.fSet = false;
	stat.bCode = code;
	stat.Frame = FRAME;

	command_list_.push_back(stat);
	return command_list_.size();
}

// Set frame data in the command list.
void LE9xxCommand::SetCommandFrame(uint8_t code, FRAME *pframe)
{
	pthread_mutex_lock(&mutex_);

	for (uint32_t i = 0; i < command_list_.size(); i++)
	{
		if (command_list_[i].bCode == code)
		{
			if (!command_list_[i].fSet)
			{
				command_list_[i].Frame = *pframe;
				command_list_[i].fSet = true;
			}
			break;
		}
	}

	pthread_mutex_unlock(&mutex_);
}

// Get frame data in the command list.
bool LE9xxCommand::GetCommandFrame(uint8_t code, FRAME *pframe)
{
	bool ret = false;

	pthread_mutex_lock(&mutex_);

	for (uint32_t i = 0; i < command_list_.size(); i++)
	{
		if (command_list_[i].bCode == code)
		{
			if (command_list_[i].fSet)
			{
				*pframe = command_list_[i].Frame;
				command_list_[i].fSet = false;
				ret = true;
				break;
				;
			}
		}
	}

	pthread_mutex_unlock(&mutex_);

	return ret;
}