#include "DSP28_Device.h"

//ControllerCommand Table[command index][command data]
//to controller command format:
//	<Command><SP> <Parameter><CR>
//from controller message format:
//	<Message> <CR> <LF>
//	CR = 0x0D = Carriage Return
//	LF = 0x0A = Line Feed
//	SP = 0x20 = Space
char CtrlCmdTable[32][32]=
{
	"\\1",
	"\\2",
	"ANZERO",
	"CLREEPROM",
	"CONFIG",
	"DIS",
	"EN",
	"ENCINIT",
	"ENCSTART",
	"ERR",
	"FLTCLR",
	"FLTHIST",
	"J 00000",
	"K",
	"MA 000000 0000",
	"MH",
	"MI 000000 0000",
	"RECORD 000 0000 000 000",
	"RSTVAR",
	"S",
	"SAVE",
	"SETP 0000 0000",
	"STOP",
	"T 000",
	"TUNE 000 0 0000"
};

//send command to azimuth and elevation controller
void SendCommand(int cmd_index)
{
	int cmd_cnt = 0;
	while(CtrlCmdTable[cmd_index][cmd_cnt] != 0)
	{
		SciSend(SLAV, CtrlCmdTable[cmd_index][cmd_cnt]);
		cmd_cnt ++;
	}
	SciSend(SLAV, 0x0D);	//CR
	return ;
}

void SendJogCmd(int Velocity)
{
	int vel;
	int VelBuff[4];	

	SciSend(SLAV, 'J');
	SciSend(SLAV, ' ');
	
	if(Velocity > 3000)
		Velocity = 3000;
	if(Velocity < -3000)
		Velocity = -3000;
	
	if(Velocity < 0)
	{
		Velocity = -Velocity;
		SciSend(SLAV, '-');
	}
	
	vel = Velocity%10;
	VelBuff[3] = 0x30+vel;
	vel = (Velocity/10)%10;
	VelBuff[2] = 0x30+vel;
	vel = (Velocity/100)%10;	
	VelBuff[1] = 0x30+vel;	
	vel = (Velocity/1000)%10;	
	VelBuff[0] = 0x30+vel;
	
	if(VelBuff[0] != 0x30)
	{
		SciSend(SLAV, VelBuff[0]);
		SciSend(SLAV, VelBuff[1]);
		SciSend(SLAV, VelBuff[2]);
		SciSend(SLAV, VelBuff[3]);
	}
	else if(VelBuff[1] != 0x30)
	{
		SciSend(SLAV, VelBuff[1]);
		SciSend(SLAV, VelBuff[2]);
		SciSend(SLAV, VelBuff[3]);
	}
	else if(VelBuff[2] != 0x30)
	{
		SciSend(SLAV, VelBuff[2]);
		SciSend(SLAV, VelBuff[3]);
	}
	else
	{
		SciSend(SLAV, VelBuff[3]);
	}	
	SciSend(SLAV, 0x0D);	
}

int CurrentController = AZ;

void SetCurrentController(int axis)
{
	if(axis == AZ)
	{
		CurrentController = AZ;	
		SendCommand(0);
	}
	if(axis == EL)
	{
		CurrentController = EL;	
		SendCommand(1);	
	}
}	

//Receiver controller status
int AzCtrlCnt = 0;
int ElCtrlCnt = 0;
char AzCtrlSts[32];
char ElCtrlSts[32];
int AzCtrlErrCode = 0;
int ElCtrlErrCode = 0;

int RecvCtrlSts(void)
{
	int recv_ch;
	int index_cnt;
	while(SciRecv(SLAV, &recv_ch) == 1)
	{
		if(CurrentController == AZ)
		{
			if(recv_ch != 0x0D)
			{
				AzCtrlSts[AzCtrlCnt] = recv_ch;
				AzCtrlCnt ++;
				if(AzCtrlCnt >= 32)
					AzCtrlCnt = 0;
			}
			else
			{
				for(index_cnt = 0; index_cnt < (AzCtrlCnt-4); index_cnt ++)
				{
					if((AzCtrlSts[index_cnt] == 'E') 
					   && (AzCtrlSts[index_cnt+1] == 'R')
					   && (AzCtrlSts[index_cnt+2] == 'R'))
					{
						AzCtrlErrCode = AzCtrlSts[index_cnt+3]*10
										+AzCtrlSts[index_cnt+4];
						AzCtrlCnt = 0;
						return -1;
					}
				}
			
			}
		}
		if(CurrentController == EL)
		{
			if(recv_ch != 0x0D)
			{
				ElCtrlSts[ElCtrlCnt] = recv_ch;
				ElCtrlCnt ++;
				if(ElCtrlCnt >= 32)
					ElCtrlCnt = 0;
			}
			else
			{
				for(index_cnt = 0; index_cnt < (ElCtrlCnt-4); index_cnt ++)
				{
					if((ElCtrlSts[index_cnt] == 'E') 
					   && (ElCtrlSts[index_cnt+1] == 'R')
					   && (ElCtrlSts[index_cnt+2] == 'R'))
					{
						ElCtrlErrCode = ElCtrlSts[index_cnt+3]*10
										+ElCtrlSts[index_cnt+4];
						ElCtrlCnt = 0;
						return -1;
					}
				}
				
			}
		}
	}
	return 0;
}

int Bcd2Bin(unsigned char *buff)
{
	int bin, decade;

	decade = buff[0]&0x0F;
	bin = decade*10000;
	decade = (buff[1]>>4)&0x0F;
	bin += decade*1000;
	decade = buff[1] &0x0F;
	bin += decade*100;
	decade = (buff[2]>>4)&0x0F;
	bin += decade*10;
	decade = buff[2]&0x0F;
	bin += decade;

	return bin;
}

//convert a integer to BCD code
void Bin2Bcd(int bin, unsigned char *buff)
{
	int decade;
	
	decade= bin%100;
	buff[2] = decade%10;
	buff[2] |= (decade/10)<<4;
	decade = (bin%10000)/100;
	buff[1] = decade%10;
	buff[1] |= (decade/10)<<4;
	buff[0] = bin/10000;

	return;
}

//send encoder value to center computer	
void SendEncoder(int axis, int code)
{
	unsigned char code_buff[3];
	
	Bin2Bcd(code, code_buff);
	if(axis == AZ)
	{
		code_buff[0] &= 0x0F;
		code_buff[0] |= 0xA0;
		SciSend(PRIM, code_buff[0]);
		SciSend(PRIM, code_buff[1]);	
		SciSend(PRIM, code_buff[2]);
	}
	if(axis == EL)
	{
		code_buff[0] &= 0x0F;
		code_buff[0] |= 0xE0;
		SciSend(PRIM, code_buff[0]);
		SciSend(PRIM, code_buff[1]);	
		SciSend(PRIM, code_buff[2]);
	}	
	
	return;
}

int AzRecvCnt = 0;
int ElRecvCnt = 0;
int CmdRecvCnt = 0;
int AzCmdFlag = 0;
int ElCmdFlag = 0;
int CmdFlag = 0;
int Azimuth = 0;
int Elevation = 0;
int Command = 0;
unsigned char AzBuff[3];
unsigned char ElBuff[3];
unsigned char CmdBuff[3];


//Receive command from center computer
int RecvCmd(void)
{
	int rch;
	
	if(SciRecv(PRIM, &rch) == 1)
	{
		if((rch&0x0F0) == 0x0A0)
		{
			AzRecvCnt = 1;
			ElRecvCnt = 0;
			CmdRecvCnt = 0;
			AzBuff[0] = rch;
		}
		else if((rch&0x0F0) == 0x0E0)
		{
			ElRecvCnt = 1;
			AzRecvCnt = 0;
			CmdRecvCnt = 0;
			ElBuff[0] = rch;
		}
		else if((rch&0x0F0) == 0x0C0)
		{
			AzRecvCnt = 0;
			ElRecvCnt = 0;
			CmdRecvCnt = 1;	
			CmdBuff[0] = rch;		
		}
		else if(AzRecvCnt >= 1)
		{
			
			AzBuff[AzRecvCnt] = rch;
			AzRecvCnt ++;
			if(AzRecvCnt >= 3)
			{
				Azimuth = Bcd2Bin(AzBuff);
				AzRecvCnt = 0;
				AzCmdFlag = 1;
			}
		}
		else if(ElRecvCnt >= 1)
		{
			ElBuff[ElRecvCnt] = rch;
			ElRecvCnt ++;
			if(ElRecvCnt >= 3)
			{
				Elevation = Bcd2Bin(ElBuff);
				ElRecvCnt = 0;
				ElCmdFlag = 1;
			}
		}
		else if(CmdRecvCnt >= 1)
		{
			CmdBuff[CmdRecvCnt] = rch;
			CmdRecvCnt ++;
			if(CmdRecvCnt >= 3)
			{
				CmdRecvCnt = 0;
				CmdFlag = 1;
			}		
		}
		else
		{
			//NOP
		}		
	}

	return 0;
}


int ReadAzCmd(int *val)
{
	if(AzCmdFlag == 1)
	{
		AzCmdFlag = 0;
		*val = Azimuth;
		return 1;
	}
	else
		return 0;
}

int ReadElCmd(int *val)
{
	if(ElCmdFlag == 1)
	{
		ElCmdFlag = 0;
		*val = Elevation;
		return 1;
	}
	else
		return 0;
}

int ReadCmd(int *val)
{
	if(CmdFlag == 1)
	{
		CmdFlag = 0;
		*val = Command;
		return 1;
	}
	else
		return 0;
}