#include "DSP28_Device.h"

#define SENDCODE 1

int DisplayType = AZ;

struct PidData AzPidCtrl;
struct PidData ElPidCtrl;

int CtrlEnFlag = 0;

int az_cmd =0 , el_cmd = 0;

int CurrentAxis = AZ;

int FaultFlag = 0;

void MainLoop(void)
{
	unsigned int di_val;
	int az_code, el_code;
	int az_cmd_pos, el_cmd_pos;

	//Read Resover to Digital Convert
	az_code = ReadRDC(AZ);
	el_code = ReadRDC(EL);
	
	//Send Azimuth and Elevation Encode to Center computer
#if SENDCODE
	SendEncoder(AZ, az_code);
	SendEncoder(EL, el_code);
#endif
	
	di_val = ReadDigitalInput();	//Read Digital Input
	ProcessDigInput(di_val);
			
	//Read Command from Center computer
	RecvCmd();
	if(ReadAzCmd(&az_cmd_pos) == 1)
	{
		az_cmd = (az_code - az_cmd_pos)/11;	//Get Servo Control Command
	}
	if(ReadElCmd(&el_cmd_pos) == 1)
	{
		el_cmd = (el_code - el_cmd_pos)/11;//Get Servo Control Command
	}	
	
	if(CurrentAxis != AZ)
	{
		CheckRangeSendCmd(AZ, di_val, az_cmd);//check the output range and send command to controller
		CurrentAxis = AZ;
	}
	else 
	{
		CurrentAxis = EL;
		CheckRangeSendCmd(EL, di_val, el_cmd);
	}
	
	//Process Key Input and send affirm to center computer
	if(KeyInput() == 1)
	{
		SciSend(PRIM, 0xDD);
		SendEncoder(PRIM, AzZeroPos);
		SendEncoder(PRIM, ElZeroPos);
	}

	//Display
	if(DisplayType == AZ)
	{
		Display(AZ, az_code);
	}
	if(DisplayType == EL)
	{
		Display(EL, el_code);
	}
	if(DisplayType == 3)
	{
		Display(3, AzZeroPos);
	}
	if(DisplayType == 4)
	{
		Display(4, ElZeroPos);
	}
	if(DisplayType == 5)
	{
		if(BreakFlag == 1)
			Display(5, 11111);
		else
			Display(5, 0);
	}
	if(DisplayType == 0)
	{
		Display(0, PressedKeyValue);
	}

	
	//Build In Test
	if(RecvCtrlSts() == -1)
	{
		if(AzCtrlErrCode != 0)
		{
			if(CtrlEnFlag == 1)
			{
				StopController();
			}
			FaultFlag = 1;
			//Send Fault Code to center computer		
			SciSend(PRIM, 0xFA);
		}
		if(ElCtrlErrCode != 0)
		{
			if(CtrlEnFlag == 1)
			{
				StopController();
			}
			FaultFlag = 1;
			//Send Fault Code to center computer
			SciSend(PRIM, 0xFE);
		}
	}

	
}

void StartController(void)
{
	SendCommand(1);		//select elevation controller 
	SendCommand(6);		//enable controller
	SendCommand(0);		//select azimuth controller
	SendCommand(6);		//enable controller
	CtrlEnFlag = 1;
	CurrentController = AZ;
}

void StopController(void)
{
	SendCommand(1);		//select elevation controller 
	SendCommand(5);		//disable controller
	SendCommand(0);		//select azimuth controller
	SendCommand(5);		//disable controller
	CtrlEnFlag = 0;
	CurrentController = AZ;
}

int CheckRangeSendCmd(int axis, unsigned di_value, int output_val)
{
	int cmd_val;
	
	cmd_val =  -500;//output_val;
//	Final Conditions
//	if(((di_value & AZ_LOCK) != 0) || ((di_value & AZ_HAND) != 0)
//	   || ((di_value & EL_HAND) != 0) || ((di_value & AZ_EN) == 0)
//	   || ((di_value & EL_EN) == 0) || (di_value & ENABLED) != 0)
// Debug Conditions	   	
	if(((di_value & AZ_LOCK) != 0) || ((di_value & AZ_HAND) != 0)
	   || ((di_value & EL_HAND) != 0) || ((di_value & AZ_EN) != 0)
	   || ((di_value & EL_EN) != 0) || ((di_value & ENABLED) !=0)
	   || (BreakFlag == 1))
	{
		if(CtrlEnFlag == 1)
			StopController();
	}
	else
	{
		if((CtrlEnFlag == 0) && (FaultFlag == 0) && (BreakFlag == 0))
			StartController();	
		if((axis == AZ) && (CtrlEnFlag == 1))
		{
			if(((di_value & LEFT_DETENT) != 0) && (cmd_val < 0))
			{
				cmd_val = 0;
			}
			if(((di_value & RIGHT_DETENT) != 0) && (cmd_val > 0))
			{
				cmd_val = 0;
			}
			CurrentController = AZ;
			SendCommand(0);	//Select Azimuth controller
			SendJogCmd(cmd_val);
		}
		if((axis == EL) && (CtrlEnFlag == 1))
		{
			if(((di_value & DOWN_DETENT) != 0) && (cmd_val < 0))
			{
				cmd_val = 0;
			}
			if(((di_value & UP_DETENT) != 0) && (cmd_val > 0))
			{
				cmd_val = 0;
			}
			CurrentController = EL;
			SendCommand(1);	//Select elevation controller
			SendJogCmd(cmd_val);
		}
	}
	return 0;		
}

/*====================================
	no more
====================================*/