// SlControl.cpp: implementation of the CSlControl class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "SerialTester.h"
#include "SlControl.h"
#include "SlPara.h"
#include "SerialComm.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW

#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
const long TIMEOUT = 20000000;


/*
The constructor. Instances for CSLPara and CSerialComm are created here.
The pointers are for inter-class communication.
*/

CSlControl::CSlControl(CMainDlg *maindlg, CSerialParaDlg *pCommsDlg, CSlParaDlg *pParaDlgA, CSlParaDlg *pParaDlgB,
					   CSlParaDlg *pParaDlgC)
{
	m_pSlPara = new CSlPara();
	m_pComms = new CSerialComm();
	m_pMain = maindlg;

	pCommsDlg->UseThis(m_pComms);
	pParaDlgA->UseThis(m_pSlPara);
	pParaDlgB->UseThis(m_pSlPara);
	pParaDlgC->UseThis(m_pSlPara);

}

CSlControl::~CSlControl()
{
	delete m_pSlPara;
	delete m_pComms;
}

/*
Starts a motor.
If the calibration velocity is still in effect the normal velocity parameter
is restored. Then, StressLogic commands for setting the target position
and starting the motor are got from a file. The motor and position parameters
are added to the string.
Finally, the string will be passed on to CMainDlg::SendString(),
which stores it in the log and sends it to the controller.
*/

void CSlControl::StartMotor(char Motor, string Position)
{
	if (!m_pMain->bVelocity) 
	{
		SetParameters(Motor, true);
		m_pMain->bVelocity=true;
	}

	string s_Command;

	s_Command =m_pSlPara->GetCommand("MTargetPos") + Motor + Position;
	m_pMain->SendString(s_Command,"motor");

	s_Command = m_pSlPara->GetCommand("MStart") + Motor;
	m_pMain->SendString(s_Command,"motor");
}

/*
Stops a motor.
*/

void CSlControl::StopMotor(char Motor)
{
	string s_Command =m_pSlPara->GetCommand("MStop") + Motor;
	m_pMain->SendString(s_Command, "motor") ;

}

/*
	SetParameters asks m_pSlPara the parameters to send to Controller and 
	calls SendString function with command. 
*/
void CSlControl::SetParameters(char motor,bool vel)
{
    string s_Command = m_pSlPara->GetCommand("MParam");
	s_Command += motor;
    s_Command += m_pSlPara->SendMotorParams(motor,vel);
	m_pMain->SendString(s_Command,"params") ;
}

/*
Sends the command requesting the status information for a motor.
*/

void CSlControl::ReadMotorStatus(string Motor)
{
	string s_Command =m_pSlPara->GetCommand("MStatus") + Motor;
	m_pMain->SendString(s_Command,"MStatus"+Motor) ;
}

/*
Sets the motor power on or off according to a boolean value.
*/

void CSlControl::MotorPower(bool status)
{
	string s_Command;
	if (status) 
		s_Command =m_pSlPara->GetCommand("MPower") + '1';
	else
		s_Command =m_pSlPara->GetCommand("MPower") + '0';
	m_pMain->SendString(s_Command,"MPower") ;
}

/*
These two methods are for setting the output status bits of the controller on or off.
The nOutput parameter is the output LED number 1-16. Since the OUT ports
in SL are in two ports, the port is selected based on the LED number (i.e 2 is port 0, 10 is port 1)
*/

void CSlControl::SetControllerOutOn(int nOutput)
{
	int nPort=0;
	char cPort[MAX_SL_OUT_CHARS];
	char cOutput[MAX_SL_OUT_CHARS];

	string s_Command =m_pSlPara->GetCommand("DOutOn");

	if (nOutput > NUM_PORT_BITS) 
	{
		nPort=1;
		nOutput-=NUM_PORT_BITS+1;
	} 
	else 
		nOutput--;

	sprintf(cPort, "%d",nPort);
	sprintf(cOutput, "%d",nOutput);

	s_Command=s_Command + cPort + cOutput;
	m_pMain->SendString(s_Command,"DOutOn") ;
}

void CSlControl::SetControllerOutOff(int nOutput)
{
	int nPort=0;
	char cPort[MAX_SL_OUT_CHARS];
	char cOutput[MAX_SL_OUT_CHARS];
	string s_Command =m_pSlPara->GetCommand("DOutOff");

	if (nOutput > NUM_PORT_BITS) 
	{
		nPort=1;
		nOutput-=NUM_PORT_BITS+1;
	} 
	else 
		nOutput--;

	sprintf(cPort, "%d",nPort);
	sprintf(cOutput, "%d",nOutput);

	s_Command=s_Command + cPort + cOutput;
	m_pMain->SendString(s_Command,"DOutOff");
}

/*
Sends the command requesting the status information for the StressLogic input LEDs.
*/

void CSlControl::ReadControllerStatus(string sPort)
{
	string s_Command =m_pSlPara->GetCommand("ReadDIn")+sPort;
	m_pMain->SendString(s_Command,"ReadDIn"+sPort) ;
}

/*
	SendToController sends string to Controller. At the same time it pushs 
	all information needed (sender/receiver and time) to commandqueue. 
*/
void CSlControl::SendToController(string Line,string sender)
{
	SYSTEMTIME SysTime;
	GetLocalTime(&SysTime);
    LONG returnvalue = m_pComms->SendString(Line+"\n");
	pair<string,SYSTEMTIME> p1(sender,SysTime);
	if (returnvalue ==0) 
		commandqueue.push(p1);
}

/*
	ReadResponse asks m_pComms to return all text that is in port. 
	Then it examines the text given to it. If it finds "\r\n" it sends string 
	to method SelectDestination. Otherwise it stores the text to string variable
	response.
*/
void CSlControl::ReadResponse()
{
	string resp,cache;
	resp = m_pComms->ReceiveString();
	if (resp != "Read failure.")	
	{
		response = response + resp;
		int i = response.find("\r\n");
		if (i != string::npos)		
		{
			cache = response.substr(i+2);
			
			response = response.substr(0,i);
			SelectDestination(response);
			response="";
			//Remember to check if empty
		}
	}
	while (cache != "") 
	{
		int i = cache.find("\r\n");
		if (i != string::npos) 
		{
			SelectDestination(cache.substr(0,i));
			cache = cache.substr(i+2);
		}
		else 
		{
			response = cache;
			cache="";
		}
	}
}


void CSlControl::InitComms()
{
	m_pComms->InitComms((CMainDlg*)AfxGetMainWnd());
}

void CSlControl::InitController()
{
	m_pMain->SendString(m_pSlPara->GetCommand("init"),"initmenu");
	m_pMain->SetParamsSent('A',false);
	m_pMain->SetParamsSent('B',false);
	m_pMain->SetParamsSent('C',false);
}

void CSlControl::CloseComms()
{
	m_pComms->CloseComms();
}

BOOL CSlControl::IsConnectionReady()
{
	return m_pComms->IsConnectionReady();
};

/*
	ReadResponse calls this method when it has received a string that ends with "\r\n"
	This method checks the first item in queue and if its something that is supposed
	to return something (except init that corresponds command IN), 
	it sends the string to some particular method. 
*/
void CSlControl::SelectDestination(string resp)
{
	if(!commandqueue.empty()) 
	{
		pair<string,SYSTEMTIME> p1;
		p1 = commandqueue.front(); commandqueue.pop();
		string receiver = p1.first;
		if (receiver == "MStatusA")
		{
			m_pMain->UpdateMotorStatus("A",resp);
		}
		else if (receiver == "MStatusB")
		{
			m_pMain->UpdateMotorStatus("B",resp);
		}
		else if (receiver == "MStatusC")
		{
			m_pMain->UpdateMotorStatus("C",resp);
		}
		else if (receiver == "ReadDIn0")
		{
			m_pMain->UpdateControllerStatus(resp,0);
		}
		else if (receiver == "ReadDIn1")
		{
			m_pMain->UpdateControllerStatus(resp,1);
		}
		else if (receiver == "initmenu")
		{
			m_pMain->UpdateControllerStatus(resp,1);
		}
	}
	m_pMain->ReceiveString(resp); 
	resp = "";
}

/*
The method first sends the calibration velocity parameter as velocity to SL and
then sends the calibrate command with the motor and position parameters.
*/

void CSlControl::CalibrateMotor(char motor, string position)
{
	if (m_pMain->bVelocity) 
		SetParameters(motor, false);
	m_pMain->bVelocity=false;
	string s_Command;
	s_Command =m_pSlPara->GetCommand("MCalibrate") + motor + position;
	m_pMain->SendString(s_Command,"motor");

}

/*
This sets the SL:s motor position value to 0.
*/

void CSlControl::SetMotorToZero(char motor)
{
	string s_Command =m_pSlPara->GetCommand("MToZero")+motor;
	m_pMain->SendString(s_Command,"tozero");

}

/*
  GetInt64FromFileTime takes FILETIME as parameter and converts it to __int64
*/
inline LARGE_INTEGER& GetInt64FromFileTime (FILETIME& ft)
{
  return *reinterpret_cast<LARGE_INTEGER*> (&ft);
}

/*
	CheckTimeOut() method checks if some of the "members" or pairs in 
	commandqueue are older than TIMEOUT (2 seconds in this case). If they are, it
	removes them from the queue and informs in StatusBar that Controller is not responding. 
*/
void CSlControl::CheckTimeout()
{
	SYSTEMTIME SysTime, Queuetime;
	FILETIME FSysTime, QueueFtime;
	GetLocalTime(&SysTime);
	SystemTimeToFileTime(&SysTime,&FSysTime);
	
	pair<string,SYSTEMTIME> p1;
	while (!commandqueue.empty()) 
	{
		p1 = commandqueue.front();
		Queuetime= p1.second;
		SystemTimeToFileTime(&Queuetime, &QueueFtime);
		if ( GetInt64FromFileTime(FSysTime).QuadPart -  GetInt64FromFileTime(QueueFtime).QuadPart > TIMEOUT) {
			commandqueue.pop();
			m_pMain->m_StatusBar.SetWindowText("Controller not responding");
		}
		else break;
	} 

}