#include "elctr.h"
#include "../setbaud.c"


 /******************************************

 High Level convert interface between program
 and connection for stearing motors.
 
 *******************************************/




/* --- the interfaces ---*/

/*
#include "ctrencoder.c"
*/
#include "ctrserial.c"
#include "ctrparallel.c"

/* ----------------------*/


double ctrif_azhs  = 0.0;
double ctrif_alths = 0.0;
double ctrif_frhs  = 0.0;

void ctrif_alert( char * txt )
{
	fprintf(stderr,"%s\n",txt);
}


void ctrif_alert_type( void )
{
	fprintf(stderr,"Invalid type of interface used\n");
}

void ctrif_alert_connection( void )
{
	fprintf(stderr,"Error during connected operation\n");
}


void ctrif_msg_connection( void )
{
	fprintf(stderr,"You must first start connection\n");
}





/* 
this can be used by every interface 
waits microsecs
*/
void ctrif_waitmicrosecs( long ms )
{
	struct timeval tv;
	struct timezone tz;
	long t,sec,usec;

	if( ms <= 0 ){
		return;
	}
		
	gettimeofday(&tv,&tz);
	sec  = tv.tv_sec;
	usec = tv.tv_usec;
	 
	t = 0;
	while( t < ms ){
		gettimeofday(&tv,&tz);
		if( ( usec != tv.tv_usec ) || ( sec != tv.tv_sec ) ){
			t++;
			sec  = tv.tv_sec;
			usec = tv.tv_usec;
		}
	}
		
}





/* 
this can be used by every interface
waits millisecs
*/
void ctrif_waitmillisecs( long ms ) {
	struct timeb tpa;
	long t;
	time_t sec;
	unsigned short msec;

	if(ms <= 0){
		return;
	}

	ftime(&tpa);
	sec  = tpa.time;
	msec = tpa.millitm;
	 
	t = 0;
	while( t < ms ){
		ftime(&tpa);
		if( ( msec != tpa.millitm ) || ( sec != tpa.time ) ){
			t++;
			sec  = tpa.time;
			msec = tpa.millitm;
		}
	}

}







int ctrif_StartConnection( void ) {

	if(elctr.connection){
		return(1);
	}
	
	elctr.motorrun     = MNORUN;

	elctr.azleft  = -1;
	elctr.altdown = -1;
	
	switch(elctr.iftype){
	
		case IF_SERIAL:		

				if(serial_openConnection() == 0 ) {
					ctrif_alert("Cannot start connection");
					return(0);
				}
				elctr.connection = 1;				
				return( 1 );

		case IF_PARALLEL:		

				if(par_openConnection() == 0 ) {
					ctrif_alert("Cannot start connection");
					return(0);
				}
				elctr.connection = 1;
				return( 1 );


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);
}





int ctrif_StopConnection( void ) {
	
	if(! elctr.connection ) {
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}
	
	switch(elctr.iftype){
	
		case IF_SERIAL:		
				
				if(serial_closeConnection() == 0 ) {
					ctrif_alert("Error when ending connection");
					return(0);
				}
				elctr.connection = 0;
				elctr.motorrun   = MNORUN;
				return(1);		

		case IF_PARALLEL:		
				
				if(par_closeConnection() == 0 ) {
					ctrif_alert("Error when ending connection");
					return(0);
				}
				elctr.connection = 0;
				elctr.motorrun   = MNORUN;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}








int ctrif_StopMotors( void )
{

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(! elctr.motorrun ){
		return(1);
	}
	
	switch(elctr.iftype){
	
		case IF_SERIAL:		

				if(serial_stopMotors() == 0 ) {
					ctrif_alert("Error when stopping motors");
					return(0);
				}
				elctr.motorrun = MNORUN;
				return(1);		

		case IF_PARALLEL:		

				if(par_stopMotors() == 0 ) {
					ctrif_alert("Error when stopping motors");
					return(0);
				}
				elctr.motorrun = MNORUN;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}







double ctrif_GetCurrentAzHS( void )
{

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(0.0);
	}
	
	switch(elctr.iftype){
	
		case IF_SERIAL:		
				if( ( ctrif_azhs = serial_getCurrentAzKoor()) < 0.0 ) {
				
					if(ctrif_azhs == -1.0){
						ctrif_alert("Error when starting receiving of az-koordinates");
					}
					else {
						ctrif_alert("Error when getting current az-koordinates");
					}
				
				}
				return( ctrif_azhs );		

		case IF_PARALLEL:		
				if( ( ctrif_azhs = par_getCurrentAzKoor()) < 0.0 ) {
						ctrif_alert("Invalid AZ-koordinates");
				}
				return( ctrif_azhs );		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0.0);

}




double ctrif_GetCurrentAltHS(void )
{	

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(0.0);
	}

	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				if( ( ctrif_alths = serial_getCurrentAltKoor()) < 0.0 ) {
					if( ctrif_alths == -1.0){
						ctrif_alert("Error when starting receiving of alt-koordinates");
					}
					else {
						ctrif_alert("Error when getting current alt-koordinates");
					}
				}
				return( ctrif_alths );		

		case IF_PARALLEL:
				if( ( ctrif_alths = par_getCurrentAltKoor()) < 0.0 ) {
						ctrif_alert("Invalid ALT-koordinates");
				}
				return( ctrif_alths );		

		default:break;
	}
	
	ctrif_alert_type();	
	return(0.0);

}



double ctrif_GetCurrentFrHS(void )
{	

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(0.0);
	}

	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				if( ( ctrif_frhs = serial_getCurrentFrKoor()) < 0.0 ) {
					if( ctrif_frhs == -1.0){
						ctrif_alert("Error when starting receiving of fr-koordinates");
					}
					else {
						ctrif_alert("Error when getting current fr-koordinates");
					}
					return(ctrif_frhs);
				}

				return( ctrif_frhs );		

		case IF_PARALLEL:
				if( ( ctrif_frhs = par_getCurrentFrKoor()) < 0.0 ) {
						ctrif_alert("Invalid FR-koordinates");
				}
				return( ctrif_frhs );		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0.0);

}





int ctrif_GoLeft_Fast( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}
 
 	elctr.azleft = -1;
 		
	switch(elctr.iftype){
	
		case IF_SERIAL:	
				speed = elctr.az_gotospeed;
				if(serial_CorrectionMode( SPEED_FAST, speed, 1, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:		

				if(par_CorrectionMode( SPEED_FAST, elctr.az_gotospeed, 1, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);
}








int ctrif_GoUp_Fast( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}

	elctr.altdown = -1;
	 	
	switch(elctr.iftype){
	
		case IF_SERIAL:		
				speed = elctr.alt_gotospeed;
				if(serial_CorrectionMode( SPEED_FAST, -1, 0, speed, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:		

				if(par_CorrectionMode( SPEED_FAST, -1, 0, elctr.alt_gotospeed, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}






int ctrif_GoDown_Fast(void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}

 	elctr.altdown = -1;
 		
	switch(elctr.iftype){
	
		case IF_SERIAL:		
				speed = elctr.alt_gotospeed;
				if(serial_CorrectionMode( SPEED_FAST, -1, 0, speed, 1, -1, 0) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:		
					if(par_CorrectionMode( SPEED_FAST, -1, 0, elctr.alt_gotospeed, 1, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}






int ctrif_GoRight_Fast( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}

	elctr.azleft = -1;
	 	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.az_gotospeed;
				if(serial_CorrectionMode( SPEED_FAST, speed, 0, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:
				if(par_CorrectionMode( SPEED_FAST, elctr.az_gotospeed, 0, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_FAST;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}








int ctrif_GoLeft_Slow( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}


	if(ctrif_StopMotors() == 0){
		return(0);
	}

	elctr.azleft = -1;
	 	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.az_correctionspeed;
				if(serial_CorrectionMode( SPEED_SLOW, speed, 1, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		

		case IF_PARALLEL:
				if(par_CorrectionMode( SPEED_SLOW, elctr.az_correctionspeed, 1, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}




int ctrif_GoUp_Slow( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}


	if(ctrif_StopMotors() == 0){
		return(0);
	}

 	elctr.altdown = -1;
 		
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.alt_correctionspeed;
				if(serial_CorrectionMode( SPEED_SLOW, -1, 0, speed, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		

		case IF_PARALLEL:
					if(par_CorrectionMode( SPEED_SLOW, -1, 0, elctr.alt_correctionspeed, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}





int ctrif_GoDown_Slow( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}


	if(ctrif_StopMotors() == 0){
		return(0);
	}
 
 	elctr.altdown = -1;
 		
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.alt_correctionspeed;
				if(serial_CorrectionMode( SPEED_SLOW, -1, 0, speed, 1, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		

		case IF_PARALLEL:
				if(par_CorrectionMode( SPEED_SLOW, -1, 0, elctr.alt_correctionspeed, 1, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}






int ctrif_GoRight_Slow( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}


	if(ctrif_StopMotors() == 0){
		return(0);
	}
 	
 	elctr.azleft = -1;
 	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.az_correctionspeed;
				if(serial_CorrectionMode( SPEED_SLOW, speed, 0, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		

		case IF_PARALLEL:
					if(par_CorrectionMode( SPEED_SLOW, elctr.az_correctionspeed, 0, -1, 0, -1, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = SPEED_SLOW;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}





int ctrif_FrTurnRight(void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}
 	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.fr_gotospeed;
				if(serial_CorrectionMode( SPEED_FAST, -1, 0, -1, 0, speed, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MFR;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:
				if(par_CorrectionMode( SPEED_FAST, -1, 0, -1, 0, elctr.fr_gotospeed, 0 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MFR;
				elctr.motorspeed = SPEED_FAST;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}




int ctrif_FrTurnLeft( void )
{
	double speed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}
 	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				speed = elctr.fr_gotospeed;
				if(serial_CorrectionMode( SPEED_FAST, -1, 0, -1, 0, speed, 1 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MFR;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:
				if(par_CorrectionMode( SPEED_FAST, -1, 0, -1, 0, elctr.fr_gotospeed, 1 ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MFR;
				elctr.motorspeed = SPEED_FAST;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}








/*
speed must be one of SPEED_SLOW, or SPEED_FAST
az,alt,fr must indicate with 1 that this is used,
otherwithe set to 0.
azdir  0 = right   1 = left
latdir 0 = up      1 = down
frdir  0 = right   1 = left
*/
int ctrif_Move( int speed,int az, int azdir,int alt, int altdir,int fr, int frdir )
{
	double azspeed,altspeed,frspeed;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	} 

	if(ctrif_StopMotors() == 0){
		return(0);
	}
 	
 	azspeed  = -1;
 	altspeed = -1;
 	frspeed  = -1;
 	
 	if(az){
 		elctr.azleft = -1;
 		azspeed = elctr.az_correctionspeed;
 		if(speed == SPEED_FAST){
 			azspeed = elctr.az_gotospeed;
 		}
 	}

 	if(alt){
		elctr.altdown = -1;
 		altspeed = elctr.alt_correctionspeed;
 		if(speed == SPEED_FAST){
 			altspeed = elctr.alt_gotospeed;
 		}
 	}

 	if(fr){
 		frspeed = elctr.fr_correctionspeed;
 		if(speed == SPEED_FAST){
 			frspeed = elctr.fr_gotospeed;
 		}
 	}
 	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				if(serial_CorrectionMode( speed, azspeed, azdir, altspeed, altdir, frspeed, frdir ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun 	= MCORRECTION;
				elctr.motorspeed = speed;
				return(1);		

		case IF_PARALLEL:
				if(par_CorrectionMode( speed, azspeed, azdir, altspeed, altdir, frspeed, frdir ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				elctr.motorrun   = MCORRECTION;
				elctr.motorspeed = speed;
				return(1);		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}











int ctrif_AltAzGoto( double azhs, double alths) {

  	int       res;

	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}


	elctr.azleft  = -1;
	elctr.altdown = -1;
	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				res = serial_Goto( makeAbsoluteDouble( azhs ), makeAbsoluteDouble(alths) );
				if(	res == 1 ){
					ctrif_alert_connection();
					return(0);
				}
				if(	res == 2 ){
					ctrif_alert("Koordinates out of range");
					return(0);
				}		
				elctr.motorrun   = MGOTO;
				elctr.motorspeed = SPEED_FAST;
				return(1);		

		case IF_PARALLEL:
				if( par_Goto( makeAbsoluteDouble( azhs ), makeAbsoluteDouble(alths) ) == 0 ) {
					ctrif_alert("Koordinates out of range");
					return(0);
				}		
				elctr.motorrun   = MGOTO;
				elctr.motorspeed = SPEED_FAST;
				return(1);		




		default:break;
	}
	
	ctrif_alert_type();	
	return(0);
}










int ctrif_AltAzOrigin( double azhs, double alths ) {

	
	if(! elctr.connection ){
		ctrif_msg_connection();
		return(1);
	}

	if(ctrif_StopMotors() == 0){
		return(0);
	}

	elctr.azleft  = -1;
	elctr.altdown = -1;
	
	switch(elctr.iftype){
	
		case IF_SERIAL:
				if(	serial_ResetKoors( azhs, alths ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				/*encoders_reset( azhs, alths );*/
				return(1);		

		case IF_PARALLEL:
				if(	par_ResetKoors( azhs, alths ) == 0 ){
					ctrif_alert_connection();
					return(0);
				}
				/*encoders_reset( azhs, alths );*/
				return(1);		




		default:break;
	}
	
	ctrif_alert_type();	
	return(0);
}





/* this function only tests a valid connection
   for the serial interface,
   parallel, or joystickport can be get whitout a previous open*/
unsigned char ctrif_Get4InputSignals( void ) {
	
	switch(elctr.inputiftype){
	
		case IF_INPUTSERIAL:
				if(! elctr.connection ){
					ctrif_msg_connection();
					return(0);
				}
				return( serialgetinputval() >> 4 & 0x0f );		

		case IF_INPUTPARALLEL:
				return( read_port(parallelport2 + 1) >> 3 & 0x0f );	

		case IF_INPUTJOYSTICK:
				return( read_port(joystickport2) >> 4 & 0x0f );	

		default:break;
	}
	
	ctrif_alert_type();	
	return(0);
}





/*
 makes one step per motor ( az, alt, fr - set to 1 what you use, otherwise 0)
 to direction ( azleft, frleft 0 = right, 1 = left,   altdown 0 = up, 1 = down )
 function gets new halfstepvalues for az,alt, if answer if set to 1,
 and sets *azhs,*alths to this.
 In mostly case when using az,alt this should be use in every case, for knowing
 if step is ready done.
 For only using fr, this makes mostly no sense.
 This tests connection, only for serial interface.
 ms is a little timer in microsecs. Sometimes we need a little wait after
 doing halfsteps, for beeing sure, steps are ready. Normally for serial interface,
 if answer is 1, this makes no sense, but when not, or for parallel-interface,
 perphas we have to wait some microsecs.
*/
int ctrif_Step( int answer, int az, int azleft, int alt, int altdown, int fr, int frleft, double * azhs, double * alths, long ms )
{

	if(ctrif_StopMotors() == 0){
		return(0);
	}
 	
	switch(elctr.iftype){
	
		case IF_SERIAL:

				if(! elctr.connection ){
					ctrif_msg_connection();
					return(1);
				}

				return( serial_Step( answer, az, azleft, alt, altdown, fr, frleft, azhs, alths, ms ) );		

		case IF_PARALLEL:
				return( par_Step( answer, az, azleft, alt, altdown, fr, frleft, azhs, alths, ms ) );		


		default:break;
	}
	
	ctrif_alert_type();	
	return(0);

}