/**
 ******************************************************************************
 *
 * @file       MKSerial.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Interfacing with MK via serial port
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "openpilot.h"
#include "MkSerial.h"

#include "attitudeactual.h" // object that will be updated by the module
#include "positionactual.h"
#include "flightbatterystate.h"



//
// Configuration
//
#define PORT			PIOS_COM_AUX
#define DEBUG_PORT		PIOS_COM_GPS
#define STACK_SIZE		1024
#define TASK_PRIORITY	(tskIDLE_PRIORITY + 3)
#define MAX_NB_PARS 	100
//#define ENABLE_DEBUG_MSG
//#define GENERATE_BATTERY_INFO		// The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state

#if PORT == PIOS_COM_AUX
	#ifndef PIOS_ENABLE_AUX_UART
		#error "This module cannot be included without the AUX UART enabled"
	#endif
#endif
//
// Private constants
//
#define MSGCMD_ANY				0
#define MSGCMD_GET_DEBUG		'd'
#define MSGCMD_DEBUG			'D'
#define MSGCMD_GET_VERSION		'v'
#define MSGCMD_VERSION			'V'
#define MSGCMD_GET_OSD			'o'
#define MSGCMD_OSD				'O'


#define DEBUG_MSG_NICK_IDX	(2+2*2)
#define DEBUG_MSG_ROLL_IDX	(2+3*2)

#define OSD_MSG_CURRPOS_IDX		1
#define OSD_MSG_NB_SATS_IDX 	50
#define OSD_MSG_BATT_IDX		57
#define OSD_MSG_COMPHEADING_IDX	62
#define OSD_MSG_NICK_IDX		64
#define OSD_MSG_ROLL_IDX		65

#ifdef ENABLE_DEBUG_MSG
	#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
#else
	#define DEBUG_MSG(format, ...)
#endif


//
// Private types
//
typedef struct
{
	uint8_t address;
	uint8_t cmd;
	uint8_t nbPars;
	uint8_t pars[MAX_NB_PARS];
} MkMsg_t;

typedef struct
{
  float longitute;
  float latitude;
  float altitude;
  uint8_t status;
} GpsPosition_t;

enum
{
	MK_ADDR_ALL = 0,
	MK_ADDR_FC = 1,
	MK_ADDR_NC = 2,
	MK_ADDR_MAG = 3,
};

//
// Private variables
//

//
// Private functions
//
static void OnError(int line);
//static void PrintMsg(const MkMsg_t* msg);
static int16_t Par2Int16(const MkMsg_t* msg, uint8_t index);
static int32_t Par2Int32(const MkMsg_t* msg, uint8_t index);
static int8_t Par2Int8(const MkMsg_t* msg, uint8_t index);
static void GetGpsPos(const MkMsg_t* msg, uint8_t index, GpsPosition_t* pos);
static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait);
static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait);
static void SendMsg(const MkMsg_t* msg);
static void SendMsgParNone(uint8_t address, uint8_t cmd);
static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0);
static void MkSerialTask(void* parameters);




static void OnError(int line)
{
	DEBUG_MSG("MKProcol error %d\n\r", line);
}


#if 0
static void PrintMsg(const MkMsg_t* msg)
{
	switch (msg->address)
	{
	case MK_ADDR_ALL:
		DEBUG_MSG("ALL ");
		break;
	case MK_ADDR_FC:
		DEBUG_MSG("FC  ");
		break;
	case MK_ADDR_NC:
		DEBUG_MSG("NC  ");
		break;
	case MK_ADDR_MAG:
		DEBUG_MSG("MAG ");
		break;
	default:
		DEBUG_MSG("??? ");
		break;
	}

	DEBUG_MSG("%c ", msg->cmd);

	for (int i = 0; i < msg->nbPars; i++)
	{
		DEBUG_MSG("%02x ", msg->pars[i]);
	}
	DEBUG_MSG("\n\r");
}
#endif

static int16_t Par2Int16(const MkMsg_t* msg, uint8_t index)
{
	int16_t res;

	res = (int) (msg->pars[index + 1]) * 256 + msg->pars[index];
	if (res > 0xFFFF / 2)
		res -= 0xFFFF;
	return res;
}

static int32_t Par2Int32(const MkMsg_t* msg, uint8_t index)
{
	uint32_t val = 0;

	val = (((int)msg->pars[index])<<0)+(((int)msg->pars[index+1])<<8);
	val += (((int)msg->pars[index+2])<<16)+((int)msg->pars[index+3]<<24);
	if (val > 0xFFFFFFFF/2)
		val -= 0xFFFFFFFF;
	return (int32_t)val;
}

static int8_t Par2Int8(const MkMsg_t* msg, uint8_t index)
{
	if (msg->pars[index] > 127)
		return msg->pars[index]-256;
	else
		return msg->pars[index];
}

static void GetGpsPos(const MkMsg_t* msg, uint8_t index, GpsPosition_t* pos)
{
  pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7;
  pos->latitude = (float)Par2Int32(msg, index+4) * (float)1e-7;
  pos->altitude = (float)Par2Int32(msg, index+8) * (float)1e-3 ;
  pos->status = msg->pars[index+12];
}


static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait)
{
	uint8_t nbBytesLeft = nbBytes;
	xTimeOutType xTimeOut;

	vTaskSetTimeOutState(&xTimeOut);

	// Loop until
	// - all bytes are received
	// - \r is seen
	// - Timeout occurs
	do
	{
		// Check if timeout occured
		if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait))
			break;

		// Check if there are some bytes
		if (PIOS_COM_ReceiveBufferUsed(PORT))
		{
			*buf = PIOS_COM_ReceiveBuffer(PORT);

			nbBytesLeft--;
			if (buf[0] == '\r')
				break;
			buf++;
		}
		else
		{
			// Avoid tight loop
			// FIXME: should be blocking
			vTaskDelay(5);
		}
	} while (nbBytesLeft);

	return nbBytes - nbBytesLeft;
}

static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait)
{
	uint8_t buf[10];
	uint8_t n;
	bool done = FALSE;
	bool error = FALSE;
	unsigned int checkVal;
	xTimeOutType xTimeOut;


	vTaskSetTimeOutState(&xTimeOut);

	while (!done && !error)
	{
		// When we are here, it means we did not encounter the message we are waiting for
		// Check if we did not timeout yet.


		// Wait for start
		buf[0] = 0;
		do
		{
			if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait))
			{
				return FALSE;
			}
			WaitForBytes(buf, 1, 100 / portTICK_RATE_MS);
		} while (buf[0] != '#');

		// Wait for cmd and address
		if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2)
		{
			OnError(__LINE__);
			continue;
		}

		// Is this the command we are waiting for?
		if (cmd == 0 || cmd == buf[1])
		{
			// OK follow this message to the end
			msg->address = buf[0] - 'a';
			msg->cmd = buf[1];

			checkVal = '#' + buf[0] + buf[1];

			// Parse parameters
			msg->nbPars = 0;
			while (!done && !error)
			{
				n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS);
				if (n > 0 && buf[n - 1] == '\r')
				{
					n--;
					// This is the end of the message
					// Get check bytes
					if (n >= 2)
					{
						unsigned int msgCeckVal;
						msgCeckVal = (buf[n-1]-'=') + (buf[n-2]-'=')*64;
						//printf("%x %x\n", msgCeckVal, checkVal&0xFFF);
						n -= 2;

						if (msgCeckVal == (checkVal & 0xFFF))
						{
							done = TRUE;
						}
						else
						{
							OnError(__LINE__);
							error = TRUE;
						}
					}
					else
					{
						OnError(__LINE__);
						error = TRUE;
					}
				}
				else if (n == 4)
				{
					// Parse parameters
					int i;
					for (i = 0; i < 4; i++)
					{
						checkVal += buf[i];
						buf[i] -= '=';
					}
					if (msg->nbPars < MAX_NB_PARS)
					{
						msg->pars[msg->nbPars] = (((buf[0]<<2)&0xFF) | ((buf[1] >> 4)));
						msg->nbPars++;
					}
					if (msg->nbPars < MAX_NB_PARS)
					{
						msg->pars[msg->nbPars] = ((buf[1] & 0x0F)<< 4 | (buf[2] >> 2));
						msg->nbPars++;
					}
					if (msg->nbPars < MAX_NB_PARS)
					{
						msg->pars[msg->nbPars] = ((buf[2] & 0x03)<< 6 | buf[3]);
						msg->nbPars++;
					}
				}
				else
				{
					OnError(__LINE__);
					error = TRUE;
				}
			}
		}
	}

	return (done && !error);
}

static void SendMsg(const MkMsg_t* msg)
{
	uint8_t buf[10];
	uint16_t checkVal;
	uint8_t nbParsRemaining;
	const uint8_t* pPar;

	// Header
	buf[0] = '#';
	buf[1] = msg->address + 'a';
	buf[2] = msg->cmd;
	PIOS_COM_SendBuffer(PORT, buf, 3);
	checkVal = (unsigned int) '#' + buf[1] + buf[2];

	// Parameters
	nbParsRemaining = msg->nbPars;
	pPar = msg->pars;
	while (nbParsRemaining)
	{
		uint8_t a, b, c;

		a = *pPar;
		b = 0;
		c = 0;

		nbParsRemaining--;
		pPar++;
		if (nbParsRemaining)
		{
			b = *pPar;
			nbParsRemaining--;
			pPar++;
			if (nbParsRemaining)
			{
				c = *pPar;
				nbParsRemaining--;
				pPar++;
			}
		}

		buf[0] = (a >> 2) + '=';
		buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '=';
		buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '=';
		buf[3] = (c & 0x3f) + '=';
		checkVal += buf[0];
		checkVal += buf[1];
		checkVal += buf[2];
		checkVal += buf[3];

		PIOS_COM_SendBuffer(PORT, buf, 4);
	}

	checkVal &= 0xFFF;
	buf[0] = (checkVal / 64) + '=';
	buf[1] = (checkVal % 64) + '=';
	buf[2] = '\r';
	PIOS_COM_SendBuffer(PORT, buf, 3);
}

static void SendMsgParNone(uint8_t address, uint8_t cmd)
{
	MkMsg_t msg;

	msg.address = address;
	msg.cmd = cmd;
	msg.nbPars = 0;

	SendMsg(&msg);
}

static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0)
{
	MkMsg_t msg;

	msg.address = address;
	msg.cmd = cmd;
	msg.nbPars = 1;
	msg.pars[0] = par0;

	SendMsg(&msg);
}


static uint16_t VersionMsg_GetVersion(const MkMsg_t* msg)
{
	return msg->pars[0] * 100 + msg->pars[1];
}


static void DoConnectedToFC(void)
{
	AttitudeActualData attitudeData;
	MkMsg_t msg;

	DEBUG_MSG("FC\n\r");

	memset(&attitudeData, 0, sizeof(attitudeData));

	// Configure FC for fast reporting of the debug-message
	SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10);

	while (TRUE)
	{
		if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS))
		{
			int16_t nick;
			int16_t roll;

			//PrintMsg(&msg);
			nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX);
			roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX);

			DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll);

			attitudeData.seq++;
			attitudeData.Pitch = -(float)nick/10;
			attitudeData.Roll = -(float)roll/10;
			AttitudeActualSet(&attitudeData);
		}
		else
		{
			DEBUG_MSG("TO\n\r");
			break;
		}
	}
}

static void DoConnectedToNC(void)
{
	MkMsg_t msg;
	GpsPosition_t pos;
	AttitudeActualData attitudeData;
	PositionActualData positionData;
	FlightBatteryStateData flightBatteryData;
#ifdef GENERATE_BATTERY_INFO
	uint8_t battStateCnt=0;
#endif

	DEBUG_MSG("NC\n\r");

	memset(&attitudeData, 0, sizeof(attitudeData));
	memset(&positionData, 0, sizeof(positionData));
	memset(&flightBatteryData, 0, sizeof(flightBatteryData));

	// Configure NC for fast reporting of the osd-message
	SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10);

	while (TRUE)
	{
		if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS))
		{
			//PrintMsg(&msg);
			GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos);

#if 0
			DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]);
			DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg,OSD_MSG_NICK_IDX), Par2Int8(&msg,OSD_MSG_ROLL_IDX));
			DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, (int)pos.longitute, (int)pos.altitude);
#else
			DEBUG_MSG(".");
#endif
			attitudeData.seq++;
			attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX);
			attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX);
			AttitudeActualSet(&attitudeData);

			positionData.Longitude = pos.longitute;
			positionData.Latitude = pos.latitude;
			positionData.Altitude = pos.altitude;
			positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX];
			positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX);
			if (positionData.Satellites<5)
			{
				positionData.Status = POSITIONACTUAL_STATUS_NOFIX;
			}
			else
			{
				positionData.Status = POSITIONACTUAL_STATUS_FIX3D;
			}
			PositionActualSet(&positionData);

#if GENERATE_BATTERY_INFO
			if (++battStateCnt > 2)
			{
				flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX]/10;
				FlightBatteryStateSet(&flightBatteryData);
				battStateCnt = 0;
			}
#endif
		}
		else
		{
			DEBUG_MSG("TO\n\r");
			break;
		}
	}
}


static void MkSerialTask(void* parameters)
{
	MkMsg_t msg;
	uint32_t version;
	bool connectionOk = FALSE;

	PIOS_COM_ChangeBaud(PORT, 57600);
	PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);

	DEBUG_MSG("MKSerial Started\n\r");

	while (1)
	{
		// Wait until we get version from MK
		while (!connectionOk)
		{
			SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION);
			DEBUG_MSG("Version... ");
			if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS))
			{
				version = VersionMsg_GetVersion(&msg);
				DEBUG_MSG("%d\n\r", version);
				connectionOk = TRUE;
			}
			else
			{
				DEBUG_MSG("TO\n\r");
			}
		}

		// Dependent on version, decide it we are connected to NC or FC
		if (version < 60)
		{
			DoConnectedToNC();	// Will only return after an error
		}
		else
		{
			DoConnectedToFC();	// Will only return after an error
		}

		connectionOk = FALSE;

		vTaskDelay(250 / portTICK_RATE_MS);
	}
}


/**
 * Initialise the module
 * \return -1 if initialisation failed
 * \return 0 on success
 */
int32_t MKSerialInitialize(void)
{
	// Start gps task
	xTaskCreate(MkSerialTask, (signed char*) "MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL);

	return 0;
}