// File: Terminal.c
//------------------------------------------------------------------------------
// Author: Giovanni de Sanctis
// Email: info@lateral-technologies.com
//------------------------------------------------------------------------------
// Date: Dec 2018
// Implements the serial interface
//------------------------------------------------------------------------------

#include "mcc_generated_files/device_config.h"  //For __delay_ms
#include "Terminal.h"
#include "Definitions.h"
#include "Power.h"
#include "Servos.h"
#include "LEDs.h"

#include "JDY08.h"

#include <string.h>

enum {
  ST_IDLE,
  ST_USER_MOVE_INIT,
  ST_USER_LEDS_INIT,
  ST_USER_MOVE,
  ST_USER_LEDS,
  ST_AUTO
} st;

#define MAX_USER_PRESETS	4u
#define MAX_AUTO_G			3u	  //We need 3 groups max for automode
#define LEN_AUTO_T			3u	  //We need 3 timing parameters for automode
#define MIN_AUTO_T			15u	  //Min time that can be specified (seconds)
#define MAX_AUTO_T			240u  //Max time that can be specified (seconds)

#define ERR_NOERR			0u
#define ERR_OUTOFRANGE		1u
#define ERR_UNKNOWNTYPE		2u
#define ERR_UNEXPECTED		3u
#define ERR_MISSINGTOKEN	4u

//Error messages
#define ERR_OUTOFRANGE_STR		  "OUT OF RANGE"
#define ERR_UNKNOWN_TYPE_STR	  "UNKNOWN TYPE"
#define ERR_UNEXPECTED_TOKEN_STR  "UNEXPECTED TOKEN"
#define ERR_MISSING_TOKEN_STR	  "MISSING PARAMETER"
#define ERR_ERROR_STR			  "ERROR"

#define MAX_PFX_LEN				  6u
#define MSG_BEGIN				  "BEGIN "
#define MSG_END					  "END "

//Commands
char const * const CMD_SHUTDN = "SHUTDOWN";
char const * const CMD_PING	  =	"PING";
char const * const CMD_BATT	  = "BATT";
char const * const CMD_VER	  = "VER";
char const * const CMD_ULEDS  =	"USERLEDS";
char const * const CMD_UMOVE  =	"USERMOVE";
char const * const CMD_AUTO   = "AUTOMODE";
char const * const CMD_ABORT  =	"STOPAUTO";

//Responses
char const * const RES_OK ="OK";
char const * const RES_ERR="ERR";

uint8_t parCycles;				//Number of cycles in the parsed move or leds pattern
uint8_t parUserMem;				//User mem preset to host the specified move
uint8_t parNum;					//Number of positions to expect
uint8_t parPtrA;				//Pointer to the servo1 positions (<parNum)
uint8_t parPtrB;				//Pointer to the servo2 positions (<parNum)
uint8_t parPtrG;				//Counter for the number of groups specified in a cmd line
uint8_t parPtrS;				//Pointer to the step values (<parNum)
uint8_t parPtrT;				//Pointer to the timing values (<LEN_AUTO_T)
int8_t error;

union ParPos					//Buffers to contain the parsed move/led pattern positions
{
  struct Leds
  {
	int8_t pos[MAX_PATTERNS];
  }leds;
  struct Move
  {
	int8_t pos1[MAX_MOVES];
	int8_t pos2[MAX_MOVES];
  }move;
  struct AutM
  {
	uint8_t t[LEN_AUTO_T];
	uint8_t grps;
  }autm;
}parPos;

char currMoveStr[MAX_CMD_LEN+1]="";
char currLedsStr[MAX_CMD_LEN+1]="";

#if (MAX_MOVES>MAX_PATTERNS)
int8_t parStep[MAX_MOVES];
#else
int8_t parStep[MAX_PATTERNS];
#endif

//Send a concatenation of 2 strings to the BT module
//This is used to send a BEGIN/END+CmdName message
void sendCompMex(const char* str1, char* str2)
{
  char str[MAX_PFX_LEN+MAX_CMD_LEN+1];
  uint8_t pd,ps;
  pd=0;
  for (ps=0; ps<=MAX_PFX_LEN && str1[ps]; ps++) 
	str[pd++]=str1[ps];
  for (ps=0; ps<MAX_CMD_LEN && str2[ps]; ps++) 
	str[pd++]=str2[ps];
  str[pd]='\0';
  btSendStr(str,0);
}

void terminal(AckEnum ack)
{
  //Check for new BT messages
  char* res=btReadStr();
  uint8_t i;
  
  if (res)
	
  {
	for (i=0; i<MAX_MOVE_CMDS; i++)
	{
	  if (btRxStrContains(MOVE_CMDS[i].cmdStr))
	  {
		//If a move is still running (currMoveStr not empty) then send
		// end move message
		if (currMoveStr[0]) 
		{
		  sendCompMex(MSG_END,currMoveStr);
		  //__delay_ms(1000);
		}
		memcpy(currMoveStr,MOVE_CMDS[i].cmdStr,MAX_CMD_LEN+1);
		sendCompMex(MSG_BEGIN,currMoveStr);
		setMove((MoveType*)MOVE_CMDS[i].cmd);
		return;
	  }
	}

	for (i=0; i<MAX_LEDS_CMDS; i++)
	{
	  if (btRxStrContains(LEDS_CMDS[i].cmdStr))
	  {
		//If leds are still running (currLedsStr not empty) then send
		// end leds message
		if (currLedsStr[0]) sendCompMex(MSG_END,currLedsStr);
		memcpy(currLedsStr,LEDS_CMDS[i].cmdStr,MAX_CMD_LEN+1);
		sendCompMex(MSG_BEGIN,currLedsStr);
		setLEDs(LEDS_CMDS[i].cmd);
		return;
	  }
	}

	if (btRxStrContains(CMD_SHUTDN))
	{
	  shutDown();
	  return;
	}
	
	if (btRxStrContains(CMD_PING))
	{
	  btSendStr(RES_OK,0);
	  return;
	}
		
	if (btRxStrContains(CMD_VER))
	{
	  btSendStr(STR_VER,0);
	  return;
	}
	
	if (btRxStrContains(CMD_ABORT))
	{
	  abortAuto();
	  btSendStr(RES_OK,0);
	  return;
	}
	
	if (btRxStrContains(CMD_BATT))
	{
	  char buff[5];
	  buff[0]='B';
	  buff[1]='A';
	  buff[2]='T';
	  buff[3]='0'+getBattBars();
	  buff[4]='\0';
	  btSendStr(buff,0);
	  return;
	}

	if (btRxStrContains(CMD_UMOVE)) st=ST_USER_MOVE;
	else if (btRxStrContains(CMD_ULEDS)) st=ST_USER_LEDS;
	else if (btRxStrContains(CMD_AUTO))
	{
	  st=ST_AUTO;
	}
	
	if (st==ST_USER_MOVE || st==ST_USER_LEDS || st==ST_AUTO)
	{
	  char type='\0';
	  uint16_t val;

	  //Clear the temp buffers and pointers before parsing new move/pattern
	  memset(&parPos,0,sizeof(parPos));
	  memset(parStep,0,sizeof(parStep));
	  parUserMem=0;
	  parNum=0;
	  parCycles=0;
	  parPtrA=0;	  //Pointer for servo1/leds positions
	  parPtrB=0;	  //Pointer for servo2 positions
	  parPtrG=0;	  //Pointer for groups (auto)
	  parPtrS=0;	  //Pointer for step values
	  parPtrT=0;	  //Pointer for timing values (auto)
	  error = 0;	  //No errors so far
	  
	  while (btRxValue(&val,&type,9999)==0 && error==0)
	  {
		switch(type)
		{
		  case 'U': //User preset number
			if (parUserMem==0) 
			{
			  if (val<MAX_USER_PRESETS) parUserMem=(uint8_t)val;
			  else error = ERR_OUTOFRANGE;
			}
		    break;
		  case 'P': //Positions
		    if (parNum==0) 
			{
			  if ((st==ST_USER_MOVE && val<MAX_MOVES)||(st==ST_USER_LEDS && val<MAX_PATTERNS))
				parNum=(uint8_t)val;
			  else error = ERR_OUTOFRANGE;
			}
			break;
		  case 'N': //Number of cycles
			if (parCycles==0)
			{
			  if (val<MAX_CYCLES) parCycles=(uint8_t)val;
			  else error = ERR_OUTOFRANGE;
			}
			break;
		  case 'A':  //Servo 1/LEDs position
			if (parPtrA<parNum)
			  if (st==ST_USER_MOVE)
				if (val<MAX_MOVE_POS) parPos.move.pos1[parPtrA++]=(int8_t)val;
				else error = ERR_OUTOFRANGE;
			  else 
				if (val<MAX_LEDS_POS) parPos.leds.pos[parPtrA++]=(int8_t)val;
				else error = ERR_OUTOFRANGE;
			else error = ERR_UNEXPECTED;
			break;
			
		  case 'B':  //Servo 2 position
			if (st==ST_USER_MOVE && parPtrB<parNum)
			  if (val<MAX_MOVE_POS) parPos.move.pos2[parPtrB++]=(int8_t)val;
			  else error = ERR_OUTOFRANGE;
			else error = ERR_UNEXPECTED;
			break;
			
		  case 'G':  //Move Group parameter
			if (st==ST_AUTO)
			  if (val && val<=MAX_AUTO_G) parPos.autm.grps |= 1u<<(uint8_t)(val-1);
			  else error = ERR_OUTOFRANGE;
			else error = ERR_UNEXPECTED;
			break;	
			
		  case 'L':  //Step parameter with linear variation
			if (parPtrS<parNum)
			  if (val<=MAX_STEP_VAL)
				parStep[parPtrS++]=(int8_t)val;
			  else error = ERR_OUTOFRANGE;
			else error = ERR_UNEXPECTED;
			break;	
			
		  case 'S': //Step parameter with step variation (val is negative)
			if (parPtrS<parNum)
			  if (val<=MAX_STEP_VAL)
				parStep[parPtrS++]=(int8_t)-val;
			  else error = ERR_OUTOFRANGE;
			else error = ERR_UNEXPECTED;
			break;
			
		  case 'T':  //Timing parameter
			if (st==ST_AUTO && parPtrT<LEN_AUTO_T)
			  if (val>=MIN_AUTO_T && val<=MAX_AUTO_T) parPos.autm.t[parPtrT++]=(uint8_t)val;
			  else error = ERR_OUTOFRANGE;
			else error = ERR_UNEXPECTED;
			break;
			
		  default:
			error = ERR_UNKNOWNTYPE;
			break;
		}
	  }
	  if (error==0)
	  {
		if (st==ST_USER_MOVE)
		{
		  setUserMove(parUserMem-1u,parNum,parCycles,parPos.move.pos1,parPos.move.pos2,parStep);
		}
		else if (st==ST_USER_LEDS)
		{
		  setUserLeds(parUserMem-1u,parNum,parCycles,parPos.leds.pos,parStep);
		}
		else
		{
		  setAuto(parPos.autm.t[0],parPos.autm.t[1],parPos.autm.t[2],parPos.autm.grps);
		}
	  }
	  //Check that all the positions and step timings have been received
	  if (error==0 && st!=ST_AUTO)
	  {
		if (parNum>parPtrA || parNum>parPtrS) error=ERR_MISSINGTOKEN;
		//If defining a move then check the pointer for the Servo2 as well
		if (st==ST_USER_MOVE && parNum>parPtrS) error=ERR_MISSINGTOKEN;
	  }
	  switch (error)
	  {
		case ERR_NOERR:		  btSendStr(RES_OK,0); break;
		case ERR_OUTOFRANGE:  btSendStr(ERR_OUTOFRANGE_STR,0); break;
		case ERR_UNKNOWNTYPE: btSendStr(ERR_UNKNOWN_TYPE_STR,0); break;
		case ERR_UNEXPECTED:  btSendStr(ERR_UNEXPECTED_TOKEN_STR,0); break;
		case ERR_MISSINGTOKEN:btSendStr(ERR_MISSING_TOKEN_STR,0); break;
		default:			  btSendStr(ERR_ERROR_STR,0); break;
	  }
	}
  }
  
  //If ack for move end then send out END MOVE message
  // and clear current move string
  if (ack==ACK_END_MOVE)
  {
	sendCompMex(MSG_END,currMoveStr);
	currMoveStr[0]=0;
  }
  
  //If ack for leds end then send out END LEDS message
  // and clear current leds string
  if (ack==ACK_END_LEDS)
  {
	sendCompMex(MSG_END,currLedsStr);
	currLedsStr[0]=0;
  }
}