// File: Moves.c
//------------------------------------------------------------------------------
// Author: Giovanni de Sanctis
// Email: info@lateral-technologies.com
//------------------------------------------------------------------------------
// Date: Dec 2018
// Manages the tail moves and LED patterns
//------------------------------------------------------------------------------


#include "Moves.h"
#include "Servos.h"
#include "Power.h"
#include "LEDs.h"
#include "mcc_generated_files/device_config.h"

#include <stdlib.h>
#include <string.h>			//For memset

#define	 N_ATTEMPTS 100		//#attempts to draw a random number in range

#define  DD		15			//Delay for morse dot
#define  DL		30			//Delay for morse line
#define  DP		60			//Delay for morse pause
#define  DF0	10			//Delay for flame
#define  DF1	20			//Delay for flame
#define  DF2	40			//Delay for flame

const MoveType TAIL_HOME=	{ 1,PL,P8, 0, 0, 0,	  PL,P0, 0, 0, 0,	100, 0, 0, 0, 0};
const MoveType SLOW_WAG1=	{ 3,P1,P7, 0, 0, 0,   P1,P7, 0, 0, 0,	75, 75, 0, 0, 0};
const MoveType SLOW_WAG2=	{ 5,P2,P6, 0, 0, 0,	  P2,P6, 0, 0, 0,	25, 25, 0, 0, 0};
const MoveType SLOW_WAG3=	{ 5,P2,P6, 0, 0, 0,	  P2,P6, 0, 0, 0,	33, 33, 0, 0, 0};

const MoveType FAST_WAG =	{20,P0,P8, 0, 0, 0,	  P0,P8, 0, 0, 0,	10, 10, 0, 0, 0};
const MoveType SHORT_WAG=	{20,P2,P6, 0, 0, 0,	  P2,P6, 0, 0, 0,	 7,  7, 0, 0, 0};
const MoveType HAPPY_WAG=	{10,P8,P4,P8,P8,P8,	  P0,P0,P0,P2,P0,	10, 10,10,10, 0};

const MoveType ERECT	=	{ 1,P0,P0, 0, 0, 0,	  P8,P8, 0, 0, 0,  -100,100, 0, 0, 0};
const MoveType ERECT_PULSE=	{10,P8,P0, 0, 0, 0,	  P8,P0, 0, 0, 0,	20, 20, 0, 0, 0};
const MoveType TREMBLE1 =	{25,P8,P7,P8,P8,P8,	  P0,P0,P0,P1,P0,	 3,  1, 1, 1, 1};
const MoveType TREMBLE2 =	{10,P8,P8,P3,P3, 0,	  P0,P0,P1,P1, 0,	10,	 1,10, 0, 0};
const MoveType ERECT_TREM =	{30,P3,P3,P7,P7, 0,	  P5,P5,P2,P2, 0,	 3,  3, 0, 0, 0};

//Move groups (used in RF mode)
const MoveType* const moveGrpSlow[MAX_GRP_SLOW] = {&SLOW_WAG1,&SLOW_WAG2,&SLOW_WAG3};
const MoveType* const moveGrpFast[MAX_GRP_FAST] = {&FAST_WAG,&SHORT_WAG,&HAPPY_WAG};
const MoveType* const moveGrpTrem[MAX_GRP_TREM] = {&TREMBLE1,&TREMBLE2,&ERECT_TREM};
const MoveType* const moveGrpErec[MAX_GRP_EREC] = {&ERECT};

//Move groups for 'auto moves' (used in BT mode)
const MoveType* const moveGrpBT[MAX_GRP_BT][MAX_MOV_BT] =
{
  {&SLOW_WAG1,	&SLOW_WAG2,	&SLOW_WAG3,	&SLOW_WAG2},		//Relaxed
  {&FAST_WAG,	&SHORT_WAG,	&HAPPY_WAG,	&ERECT},			//Excited
  {&TREMBLE1,	&TREMBLE2,	&ERECT_TREM,&ERECT_PULSE}		//Tense
};

//Random moves (used in RF mode)
const MoveType* const moveRndGrp[MAX_GRP_RAND] =
{
  &SLOW_WAG1,   &SHORT_WAG,  &FAST_WAG,   &SLOW_WAG2,
  &HAPPY_WAG,   &FAST_WAG,   &SHORT_WAG,  &SLOW_WAG3,
  &SLOW_WAG2,   &SLOW_WAG3,  &HAPPY_WAG,  &SHORT_WAG,
  &FAST_WAG,   &SLOW_WAG3,   &SLOW_WAG1,  &SLOW_WAG2
};

//Time intervals for random moves
const uint8_t moveRndGrpT[MAX_GRP_RAND]=
{
  35,17,26,40,18,17,36,32,25,34,28,31,31,42,32,33
};

const LedsType LEDS_OFF =	{1, 
  0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
  100,  0,  0,	0,	0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};
const LedsType LEDS_ON = {10,  //10 cycles*100*20ms = 20 seconds
  0xF,0xF,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
  100,  0,  0,	0,	0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};
const LedsType LEDS_RECT=	{15, //15 cycles*(50+50)*20ms = 30 seconds
  0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
  -50,-50,	0,	0,	0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};
const LedsType LEDS_TRI =	{15, //15 cycles*(50+50)*20ms = 30 seconds
  0x0,0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
   50, 50,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};
const LedsType LEDS_SAW =	{15, //15 cycles*(50+50)*20ms = 30 seconds
  0x0,0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
  100,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};
const LedsType LEDS_BEA =	{15, 
  0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
   -5,-95,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0
};
const LedsType LEDS_SOS =	{3, 
  0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
  -DD,-DD,-DD,-DD,-DD,-DP,-DL,-DL,-DL,-DL,-DL,-DP,-DD,-DD,-DD,-DD,-DD,-DP,-DP,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};
const LedsType LEDS_FLA =	{2, 
  0x7,0x8,0x6,0x5,0x3,0x7,0x8,0x7,0x5,0x2,0x6,0x8,0x6,0x5,0x8,0x5,0x2,0x4,0x8,0x5,0x1,0x7,0x8,0x7,0x4,0x8,0x7,0x8,0x8,0x4,0x5,0x3,
  DF1,DF2,DF1,DF2,DF0,DF2,DF0,DF2,DF0,DF0,DF0,DF2,DF2,DF0,DF1,DF1,DF1,DF1,DF2,DF1,DF0,DF2,DF2,DF2,DF0,DF2,DF2,DF2,DF1,DF2,DF0,DF0
};

const LedsType LEDS_STR =	{250, 
  0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
   -3, -3,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,	0
};

const LedsType* const ledsGrpA[MAX_LED_GRPA] = {&LEDS_ON, &LEDS_OFF};
const LedsType* const ledsGrpB[MAX_LED_GRPB] = {&LEDS_RECT, &LEDS_TRI, &LEDS_SAW};
const LedsType* const ledsGrpC[MAX_LED_GRPC] = {&LEDS_BEA, &LEDS_STR};


MoveType userMove[MAX_USER_MOVES];
LedsType userLeds[MAX_USER_LEDS];
MoveStruct currMove;
LedsStruct currLeds;

AutoStruct autoMode;

const MoveCmd MOVE_CMDS[MAX_MOVE_CMDS]=
{
  "TAILHM",&TAIL_HOME,
  "TAILS1",&SLOW_WAG1,
  "TAILS2",&SLOW_WAG2,
  "TAILS3",&SLOW_WAG3,
  "TAILFA",&FAST_WAG,
  "TAILSH",&SHORT_WAG,
  "TAILHA",&HAPPY_WAG,
  "TAILER",&ERECT,
  "TAILEP",&ERECT_PULSE,
  "TAILT1",&TREMBLE1,
  "TAILT2",&TREMBLE2,
  "TAILET",&ERECT_TREM,
  "TAILU1",&userMove[0],
  "TAILU2",&userMove[1],
  "TAILU3",&userMove[2],
  "TAILU4",&userMove[3],
};

const LedsCmd LEDS_CMDS[MAX_LEDS_CMDS]=
{
  "LEDOFF",&LEDS_OFF,
  "LEDON",&LEDS_ON,
  "LEDREC",&LEDS_RECT,
  "LEDTRI",&LEDS_TRI,
  "LEDSAW",&LEDS_SAW,
  "LEDSOS",&LEDS_SOS,
  "LEDFLA",&LEDS_FLA,
  "LEDSTR",&LEDS_STR,
  "LEDBEA",&LEDS_BEA,
  "LEDUS1",&userLeds[0],
  "LEDUS2",&userLeds[1],
  "LEDUS3",&userLeds[2],
  "LEDUS4",&userLeds[3]
};

void movesInit()
{
  initServos();	  //Inits the PWM peripherals
  //setMove(&TAIL_HOME);
  startLEDs();
  setLEDsBrightness(0);
  stopLEDs();
  autoMode.timeout=0;		//Makes sure 'auto mode' is off
}

uint8_t setUserMove(uint8_t preset,uint8_t len,uint8_t cycles,int8_t *pos1,int8_t *pos2,int8_t *steps)
{
  uint8_t i;
  
  if (preset>=MAX_USER_MOVES) return 1;
  memset(&userMove[preset],0,sizeof(MoveType));
  userMove[preset].cycles=cycles;
  if (len>MAX_MOVES) len=MAX_MOVES;
  for (i=0; i<len; i++)
  {
	userMove[preset].pos1[i]=pos1[i];
	userMove[preset].pos2[i]=pos2[i];
	userMove[preset].steps[i]=steps[i];
  }
  return 0;
}

uint8_t setUserLeds(uint8_t preset,uint8_t len,uint8_t cycles,int8_t *pos,int8_t *steps)
{
  uint8_t i;
  
  if (preset>=MAX_USER_LEDS) return 1;
  memset(&userLeds[preset],0,sizeof(LedsType));
  userLeds[preset].cycles=cycles;
  if (len>MAX_MOVES) len=MAX_MOVES;
  for (i=0; i<len; i++)
  {
	userLeds[preset].pos[i]=pos[i];
	userLeds[preset].steps[i]=steps[i];
  }
  return 0;
}

void setRandMove()
{
  uint8_t i,g,m;
  
  //Select group
  for (i=0; i<N_ATTEMPTS; i++)
  {
	m = (uint8_t)rand();
	g = (m >> 2)&0x03u;
	if ((1<<g)&autoMode.grps) break;
  }
  if ((1<<g)&autoMode.grps == 0) g=1;
  
  //Select move within group
  m = (m & 0x03u);
  
  setMove(moveGrpBT[g][m]);
}

void setRandPause()
{
  uint8_t i,p;
  for (i=0; i<N_ATTEMPTS; i++)
  {
	p = (uint8_t)rand();
	if (p>=autoMode.t1 && p<=autoMode.t2) break;
  }
  if (p<autoMode.t1 || p>autoMode.t2) p=autoMode.t1;
  
  autoMode.pause = p * F_SERVO;
}

//Sets up automatic mode (random moves for a specified length of time)
void setAuto(uint8_t t1, uint8_t t2, uint8_t dur, uint8_t grps)
{
  autoMode.timeout = (uint16_t)dur * F_SERVO;
  autoMode.t1 = t1;
  autoMode.t2 = t2;
  autoMode.pause = 1;				//Will start by setting up a move
  autoMode.grps = grps;
}

void abortAuto()
{
  autoMode.timeout = 0;
}

//Sets up move variables for the ISR 
void setMove(const MoveType* movePtr)
{
  currMove.movePtr = movePtr;
  currMove.cycIter=movePtr->cycles;
  currMove.moveIter=0;
  currMove.stepIter=0;
  //if (movePtr!=&TAIL_HOME)
  //{
//	currMove.pos1=POS2SERVO(SERVO1_HOME);
//	currMove.pos2=POS2SERVO(SERVO2_HOME);
//  }
  startServos();
}

AckEnum updateMove()
{
  if (autoMode.timeout)
  {
	--autoMode.timeout;
	if (autoMode.timeout == 0) return ACK_END_AUTO;
	
	if (autoMode.pause) 
	{
	  --autoMode.pause;
	  if (autoMode.pause == 0) setRandMove();
	  //return ACK_RUNNING;
	}
  }
  		  
  if (currMove.movePtr && currMove.cycIter)
  {
	if (currMove.stepIter==0)
	{
	  int16_t pos;
	  //Copy new pos, unless it's a PL (keeps the last position)
	  pos=currMove.movePtr->pos1[currMove.moveIter];
	  if (pos!=PL) currMove.pos1=POS2SERVO(pos);
	  pos=currMove.movePtr->pos2[currMove.moveIter];
	  if (pos!=PL) currMove.pos2=POS2SERVO(pos);
	  	  
	  currMove.steps=(int16_t)currMove.movePtr->steps[currMove.moveIter];
	  	  
	  if (currMove.steps>0)
	  {
		uint8_t p1=currMove.moveIter+1u;
		if (p1>=MAX_MOVES) /*|| currMove.movePtr->steps[p1]==0)*/ p1=0;
		currMove.d1=(POS2SERVO(currMove.movePtr->pos1[p1])-currMove.pos1)/currMove.steps;
		currMove.d2=(POS2SERVO(currMove.movePtr->pos2[p1])-currMove.pos2)/currMove.steps;
	  }
	  else
	  {
		currMove.steps = -currMove.steps;
		currMove.d1 = 0;
		currMove.d2 = 0;
	  }	
	}
	
	setServo1(currMove.pos1);
	setServo2(currMove.pos2);
	currMove.pos1+=currMove.d1;
	currMove.pos2+=currMove.d2;
	currMove.stepIter++;
	
	
	//if (currMove.stepIter>=currMove.movePtr->steps[currMove.moveIter] || currMove.steps==0 || currMove.pos1==0 || currMove.pos2==0)
	if (currMove.stepIter>=currMove.steps || currMove.steps==0)
	{
	  currMove.stepIter=0;
	  currMove.moveIter++;
	  //if (currMove.moveIter>=MAX_MOVES || currMove.steps==0)
	  if (currMove.moveIter>=MAX_MOVES || currMove.movePtr->steps[currMove.moveIter]==0)
	  {
		currMove.moveIter=0;
		currMove.cycIter--;
		if (currMove.cycIter==0) 
		{
		  if (currMove.movePtr==&TAIL_HOME) 
		  {
			currMove.movePtr=0;
			if (autoMode.timeout) setRandPause();
			stopServos();
			return ACK_END_MOVE;
		  }
		  else 
		  {
			setMove(&TAIL_HOME);
			return ACK_RUNNING;
		  }
		}
	  }
	} 
	return ACK_RUNNING;
  }
  else stopServos();
  return ACK_WAITING;
}

//Sets up led pattern variables for updateLeds()
void setLEDs(const LedsType* ledsPtr)
{
  currLeds.ledsPtr = ledsPtr;
  currLeds.cycIter=ledsPtr->cycles;
  currLeds.moveIter=0;
  currLeds.stepIter=0;
  startLEDs();
}

AckEnum updateLEDs()
{
  if (currLeds.ledsPtr && currLeds.cycIter)
  {
	if (currLeds.stepIter==0)
	{
	  currLeds.pos=POS2LEDS(currLeds.ledsPtr->pos[currLeds.moveIter]);
	  currLeds.steps=currLeds.ledsPtr->steps[currLeds.moveIter];
	  
	  if (currLeds.steps>0)
	  {
		uint8_t p1=currLeds.moveIter+1u;
		if (p1>=MAX_PATTERNS) /*|| currLeds.ledsPtr->steps[p1]==0)*/ p1=0;
		currLeds.d=(POS2LEDS(currLeds.ledsPtr->pos[p1])-currLeds.pos)/currLeds.steps;
	  }
	  else
	  {
		currLeds.steps = -currLeds.steps;
		currLeds.d = 0;
	  }
	}
	setLEDsBrightness2(currLeds.pos);
	currLeds.pos+=currLeds.d;
	currLeds.stepIter++;
	
	//if (currLeds.stepIter>=currLeds.steps || currLeds.steps==0)
	if (currLeds.stepIter>=currLeds.steps || currLeds.ledsPtr->steps[currLeds.moveIter]==0)
	{
	  currLeds.stepIter=0;
	  currLeds.moveIter++;
	  if (currLeds.moveIter>=(MAX_PATTERNS) || currLeds.steps==0) 
	  {
		currLeds.moveIter=0;
		currLeds.cycIter--;
		if (currLeds.cycIter==0) 
		{
		  currLeds.ledsPtr=0; 
		  return ACK_END_LEDS;
		}
	  }
	}
	return ACK_RUNNING;                                                                                                                     return ACK_RUNNING;
  }
  else stopLEDs();
  return ACK_WAITING;
}