#define HSECONDS 1.0 / 360000.0

static unsigned char sbufout[12] = "0000000000\0";
static unsigned char sbufin[36]  = "00000000000000000000000000000000000\0";
static UInt16   srefNum = 0;
static Err      serr;
static Boolean  sisopen = false;

static Boolean initBoardConnection( void );
static Boolean closeBoardConnection( void );
static Boolean serialOperationStopMotors( void );
static Boolean serialOperationStopMotorPower( void );

static Boolean serialopen(void);
static Boolean serialclose(void);
static Boolean serialsequenceout(void);
static Boolean singleserialsequenceout(void);
static Boolean serialsequencein( UInt32 num );

static unsigned char SLOWBYTE( unsigned char val );
static unsigned char SHIBYTE( unsigned char val );
static unsigned char SFBYTE( unsigned char low, unsigned char high );

static void setserialoutbuffer( unsigned char cmd, unsigned char v1, unsigned char v2, unsigned char v3, unsigned char v4);

static void doubletoserial24( unsigned char cmd, double val );
static double serial24todouble( char *src );
static unsigned char getdoublebyteval( double val );

static void ticksleep( int ticks );
static unsigned char calculateramp( unsigned char speed );


static double serialOperationGetAzKoor( void );
static double serialOperationGetAltKoor( void );
static Boolean serialOperationResetKoors ( double az, double alt );
static int serialOperationGoto ( double az, double alt );

static Boolean serialOperationCorrectionMode( double ct, double azsp, int azdir, double altsp, int altdir, double frsp, int frdir );

static Boolean serialOperationKoorsapproach( struct guidevalues * gv );
static Boolean serialOperationStartGuideing( void );
static Boolean serialOperationGuide( void );

static guidevalues guidevals;

SerSettingsType serSettings;

static double guideAzhs;
static double guideAlths;

static UInt32 debugticks1 = 0;
static UInt32 debugticks2 = 0;
static UInt32 debugticks3 = 0;



static void ticksleep( int ticks ) {
	UInt32 t;
	t = TimGetTicks();
	t = t + ticks;
	while( TimGetTicks() < t ){
		;
	}
}


static unsigned char calculateramp( unsigned char speed )
{
	if(speed >= 20 ){
		return(speed);
	}

	if(speed >= 10 ){
		return(20);
	}

	return(254);
}



static unsigned char SLOWBYTE( unsigned char val ) {
	unsigned char v;
	v = val & 0x0f;
	v = v + 0x30;
	return(v);
}

static unsigned char SHIBYTE( unsigned char val ) {
	unsigned char v;
	v = val >> 4;
	v = v & 0x0f;
	v = v + 0x30;
	return(v);
}


static unsigned char SFBYTE( unsigned char low, unsigned char high ) {

	unsigned char v,l,h;
	l = low - 0x30;
	l = l & 0x0f;
	h = high - 0x30;
	h = h << 4;
	h = h & 0xf0;
	v = l | h;
	return(v);
}


static void setserialoutbuffer( unsigned char cmd, unsigned char v1, unsigned char v2, unsigned char v3, unsigned char v4) {

	sbufout[0] = SLOWBYTE(cmd);
	sbufout[1] = SHIBYTE(cmd);

	sbufout[2] = SLOWBYTE(v1);
	sbufout[3] = SHIBYTE(v1);

	sbufout[4] = SLOWBYTE(v2);
	sbufout[5] = SHIBYTE(v2);

	sbufout[6] = SLOWBYTE(v3);
	sbufout[7] = SHIBYTE(v3);
	
	sbufout[8] = SLOWBYTE(v4);
	sbufout[9] = SHIBYTE(v4);

}




static unsigned char getdoublebyteval( double val )
{
	double v;
	UInt32 w;
	unsigned char *p;

	v = makeAbsoluteDouble(val);
	w = (UInt32) v;
	p = (unsigned char *)&w;

	return(p[3]);
}




static void doubletoserial24( unsigned char cmd, double val )
{
	double v;
	UInt32 w;
	unsigned char *p;

	v = makeAbsoluteDouble(val);
	w = (UInt32) v;
	p = (unsigned char *)&w;
	
	setserialoutbuffer(cmd,p[3],p[2],p[1],0x3f);

}





static double serial24todouble( char *src ) {
	double d;
	UInt32 w[2];
	unsigned char *p;
	unsigned char v;
	
	d = 0;
	w[0] = 0;
	w[1] = 0;
	p = (unsigned char *)&w[0];

	v = SFBYTE( src[0], src[1]);
	p[3] = v;

	v = SFBYTE( src[2], src[3]);
	p[2] = v;

	v = SFBYTE( src[4], src[5]);
	p[1] = v;

	d = ( double )w[0];	
	return( d );
}



static Boolean serialopen(void) {

	if(sisopen == true){
		return(true);
	}

	if( srefNum == 0 ){
		if(SysLibFind("Serial Library",&srefNum) != 0) {
			FrmAlert(alertID_serialliberror);		
			return(false);
		}
	}

		
	if( SerOpen( srefNum , 0 , 19200)  == 0 ){

		/*
		serSettings.baudRate = 19200;
		serSettings.flags    = serSettingsFlagBitsPerChar8 | serSettingsFlagStopBits1 | serSettingsFlagRTSAutoM | serSettingsFlagXonXoffM | serSettingsFlagCTSAutoM;
		serSettings.ctsTimeout = SysTicksPerSecond();

		if(SerSetSettings( srefNum, &serSettings ) != 0){
			SerClose(srefNum);
			return(false);
		}
		*/
				
		sisopen = true;
		return(true);
	}

	if(serr == serErrLineErr){
		SerClearErr(srefNum);
	}
	return(false);
}







static Boolean serialclose(void) {

	if(sisopen == false){
		return(true);
	}
	
	if( SerClose(srefNum) == 0 ) {
		sisopen = false;
		return(true);
	}

	if(serr == serErrLineErr){
		SerClearErr(srefNum);
	}

	return(false);
}




static Boolean serialsequenceout(void) {

	if(sisopen == false ){
		return(false);
	}

	if( SerSend(srefNum,(VoidPtr)&sbufout, 10, &serr) == 10 ){
		if(serr == 0 ){
		
		    if(SerSendWait( srefNum,200) == 0){
				return(true);
			}

			serialclose();
			return(false);
		}
	}

	
	if(serr == serErrLineErr){
		SerClearErr(srefNum);
	}
	serialclose();
	return(false);	
}




static Boolean singleserialsequenceout(void) {

	if(sisopen == false ){
		if(serialopen() == false){
			return(false);
		}
	}

	if( SerSend(srefNum,(VoidPtr)&sbufout, 10, &serr) == 10 ){
		if(serr == 0 ){
		
		    if(SerSendWait( srefNum,200) == 0){
		    	serialclose();
				return(true);
			}

			serialclose();
			return(false);
		}
	}

	if(serr == serErrLineErr){
		SerClearErr(srefNum);
	}
	serialclose();
	return(false);	
}





static Boolean serialsequencein( UInt32 num ) {

	if(sisopen == false ){
		return(false);
	}

	serr = SerReceiveWait( srefNum, num, 200 );

	if(serr == 0 ){

		if( SerReceive (srefNum, ( void * )&sbufin, num, 200 , &serr) == num ){
			if( serr == 0 ){
				return(true);
			}
		}
	}
	
	if(serr == serErrLineErr){
		SerClearErr(srefNum);
	}
	
	serialclose();
	
	return(false);
}





/*******************************************
	High level board-operations
********************************************/




static Boolean serialOperationStopMotors( void ) {

	if(serialopen() == false){
		return(false);
	}

	setserialoutbuffer(0x20,0x3f,0x3f,0x3f,0x3f);
	
	if(serialsequenceout() == false){
		return(false);
	}
	return(serialclose());
}


static Boolean serialOperationStopMotorPower( void ) {

	if(serialopen() == false){
		return(false);
	}

	setserialoutbuffer(0x3d,0x3f,0x3f,0x3f,0x3f);
	
	if(serialsequenceout() == false){
		return(false);
	}
	return(serialclose());
}



static double serialOperationGetAzKoor( void ) {
	double d;

	if(serialopen() == false){
		return(-1);
	}

	setserialoutbuffer(0x22,0x3f,0x3f,0x3f,0x3f);
	
	if(serialsequenceout() == false){
		return(-1);
	}

	if(serialsequencein( 10 ) == false){
		return(-1);
	}

	if(serialclose() == false){
		return(-1);
	}

	d = serial24todouble( &sbufin[2] );


	if(scope.az_halfstepsize > 0 ){
		d = d * scope.az_halfstepsize;
	}

	
	return( d );
}


static double serialOperationGetAltKoor( void ) {
	double d;

	if(serialopen() == false){
		return(-1);
	}

	setserialoutbuffer(0x23,0x3f,0x3f,0x3f,0x3f);
	
	if(serialsequenceout() == false){
		return(-1);
	}

	if(serialsequencein( 10 ) == false){
		return(-1);
	}


	if(serialclose() == false){
		return(-1);
	}
	
	d = serial24todouble( &sbufin[2] );


	if(scope.alt_halfstepsize > 0 ){
		d = d * scope.alt_halfstepsize;
	}

	
	return( d );

}



//init serial connection to eldob-cpu-board
//first test if available
static Boolean initBoardConnection( void )
{
			
	if(boardconnection){
		return(true);
	}

	if(serialopen() == false){
		return(false);
	}

	

	setserialoutbuffer(0x20,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );


	setserialoutbuffer(0x3d,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );


	doubletoserial24( 0x26, scope.hs_az360 );
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );


	doubletoserial24( 0x2b, scope.hs_altlb );
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep(2);


	doubletoserial24( 0x2c, scope.hs_altub );
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep(2);


	doubletoserial24( 0x31, scope.hs_fr360 );
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep(2);


	/*
	//0 is the default on board for TIMERINTVAL
	setserialoutbuffer(0x38,0x00,0x00,0x3f,0x3f);	
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep(2);

	*/
	

	/*
	//we do not change the current koordinates
	//every time we init the stuff, so the user
	//can shutoff all, and init again, and the
	//board has its old koordinates.
	//But,- after first init, minimum one origin-operation
	//must be done.	
	setserialoutbuffer(0x39,0x00,0x00,0x00,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );

	setserialoutbuffer(0x3A,0x00,0x00,0x00,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );

	setserialoutbuffer(0x3B,0x00,0x00,0x00,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );
	*/



	setserialoutbuffer(0x3c,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );
	
		
	if(serialclose() == false){
		return(false);
	}

	boardconnection = 1;
	return(true);
}




static Boolean serialOperationResetKoors ( double az, double alt ) {

	double v;

	if(serialopen() == false){
		return(false);
	}


	v = az;
	if( v != 0 ){
		if(scope.az_halfstepsize > 0 ){
			v = v / scope.az_halfstepsize;
		}
	}
	doubletoserial24( 0x39, v );
	if(serialsequenceout() == false){
		return(false);
	}
	ticksleep( 2 );


	v = alt;
	if( v != 0 ){
		if(scope.alt_halfstepsize > 0 ){
			v = v / scope.alt_halfstepsize;
		}
	}
	doubletoserial24( 0x3a, v );
	if(serialsequenceout() == false){
		return(false);
	}


	if(serialclose() == false){
		return(false);
	}

	return(true);
}



//return with 1 it connection error
//return with 2 if koordinatoutofrangeerror
//return with 0 if all ok
static int serialOperationGoto ( double az, double alt ) {


	double curaz,curalt,newaz,newalt,azevt,altevt;
	unsigned char settings;
	unsigned char speed,ramp;

	if(serialopen() == false){
		return(1);
	}


	setserialoutbuffer(0x22,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(1);
	}
	if(serialsequencein( 10 ) == false){
		return(1);
	}
	curaz = serial24todouble( &sbufin[2] );
	ticksleep( 1 );

	setserialoutbuffer(0x23,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(1);
	}
	if(serialsequencein( 10 ) == false){
		return(1);
	}	
	curalt = serial24todouble( &sbufin[2] );

	settings = 0;


	newaz = az;
	if( newaz != 0 ){
		if(scope.az_halfstepsize > 0 ){
			newaz = newaz / scope.az_halfstepsize;
		}
	}
	
	newalt = alt;
	if( newalt != 0 ){
		if(scope.alt_halfstepsize > 0 ){
			newalt = newalt / scope.alt_halfstepsize;
		}
	}

	if(serialclose() == false){
		return(1);
	}









	

	// now we have the current position with curaz, and curalt as number of halfsteps
	// and we have the new position with newaz, and newalt also as number of halfsteps
	// Now we must search the shortes way.


	if( curaz != newaz ){

		if(newaz > scope.hs_az360){
			return(2);
		}
	
		settings = settings | 1; // az motor on

		
		if( ( curaz >= 0 ) && ( curaz < scope.hs_az180 ) ){
			//scope is current between 0 and 180 degrees

			if( ( newaz >= 0 ) && ( newaz < scope.hs_az180 ) ){
				//new azval is in the same sector
				if(newaz < curaz){
					settings = settings | 16; // turn left
					azevt = curaz - newaz;	
				}
				else {
					// turn right
					azevt = newaz - curaz;
				}				
			}
			else {
				//new azval is in the opposite sector
				//normally turn right
				azevt = newaz - curaz;
				
				if( ( curaz < scope.hs_az90) && ( newaz >= scope.hs_az270 )  ){
					//scope is current between 0 and 90 degrees
					//and new az also north
					settings = settings | 16; // turn left
					azevt = ( scope.hs_az360 - newaz ) + curaz;
				}				
				
			}	
		
		}
		else {
			// scope is cuurrent between 180 and 360 degrees
	
			if( ( newaz >= scope.hs_az180 ) && ( newaz <= scope.hs_az360 ) ){
				//new azval is in the same sector

				if(newaz < curaz){
					settings = settings | 16; // turn left
					azevt = curaz - newaz;
				}
				else {
					//turn right
					azevt = newaz - curaz;
				}				

				
			}
			else {
				// new azval is in the opposite sector

				if( ( curaz >= scope.hs_az270 ) && ( newaz < scope.hs_az90 )  ){
					//scope is current between 270 and 360 degrees
					//and new az also north
					// turn right
					azevt = ( scope.hs_az360 - curaz ) + newaz;
				}				
				else {

					//normally turn left
					settings = settings | 16;
					azevt = curaz - newaz;

				}
				
			}		
	
		}
	
	}



	if(curalt != newalt ) {

		if(newalt <= scope.hs_altlb){
			return(2);
		}

		if(newalt >= scope.hs_altub){
			return(2);
		}
		
		settings = settings | 2;
		
		if( newalt < curalt ){
			//turn down
			settings = settings | 32;
			altevt = curalt - newalt;		
		}
		else {
			//turn up
			altevt = newalt - curalt;
		
		}
	
	
	}
	
	

	if(settings > 0 ){
		if(serialopen() == false){
			return(1);
		}


		doubletoserial24( 0x25, scope.timerintct_goto );
		if(serialsequenceout() == false){
			return(1);
		}
		ticksleep( 3 );


		speed = getdoublebyteval( scope.az_gotospeed );
		ramp  = calculateramp(speed);
		setserialoutbuffer(0x2a,speed,ramp,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(1);
		}
		ticksleep( 3 );


		speed = getdoublebyteval( scope.alt_gotospeed );
		ramp  = calculateramp(speed);
		setserialoutbuffer(0x30,speed,ramp,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(1);
		}
		ticksleep( 3 );

		doubletoserial24( 0x27, azevt );
		if(serialsequenceout() == false){
			return(1);
		}
		ticksleep( 3 );

		doubletoserial24( 0x2D, altevt );
		if(serialsequenceout() == false){
			return(1);
		}
		ticksleep( 3 );

		setserialoutbuffer(0x21,0x22,settings,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(1);
		}

		if(serialclose() == false){
			return(1);
		}
	}
	
	
	//all ok
	return(0);
}









//set speeds to value < 0, for motors not used.
//if all < 0, only timercounter will set to ct, expecting ct is also <0
//a dirvalue of 1 means left, 0 means right
static Boolean serialOperationCorrectionMode( double ct, double azsp, int azdir, double altsp, int altdir, double frsp, int frdir ) {

	unsigned char settings;
	unsigned char speed,ramp;

	settings = 0;

	if(serialopen() == false){
		return(false);
	}

	if( ct >= 0){
		doubletoserial24( 0x25, ct );
		if(serialsequenceout() == false){
			return(false);
		}
		ticksleep( 2 );
	}

	if(azsp >= 0){

		settings = settings | 1;
		if(azdir){
			settings = settings | 16;
		}
				

		speed = getdoublebyteval( azsp );
		ramp  = calculateramp(speed);
		setserialoutbuffer(0x2a,speed,ramp,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(false);
		}
		ticksleep( 2 );
	}

	if(altsp >= 0){

		settings = settings | 2;
		if(altdir){
			settings = settings | 32;
		}
				
		speed = getdoublebyteval( altsp );
		ramp  = calculateramp(speed);
		setserialoutbuffer(0x30,speed,ramp,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(false);
		}
		ticksleep( 2 );
	}

	if(frsp >= 0){

		settings = settings | 4;
		if(frdir){
			settings = settings | 64;
		}
				
		speed = getdoublebyteval( frsp );
		ramp  = calculateramp(speed);
		setserialoutbuffer(0x35,speed,ramp,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(false);
		}
		ticksleep( 2 );
	}

	if( settings > 0 ){
		setserialoutbuffer(0x21,0x21,settings,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(false);
		}
	}

	if(serialclose() == false){
		return(false);
	}

	return(true);
}




static Boolean closeBoardConnection( void )
{
	if(!boardconnection){
		return(true);
	}

	if(serialOperationStopMotors() == false) {
		return(false);
	}

	ticksleep( 2 );

	if(serialOperationStopMotorPower() == false) {
		return(false);
	}

	boardconnection = 0;
	return(true);
}







static Boolean serialOperationKoorsapproach( struct guidevalues *gv )
{
	unsigned char azsetting,altsetting,frsetting;
	double nextazhs,nextalths;

	nextazhs  = makeAbsoluteDouble(gv->nextazhs);
	nextalths = makeAbsoluteDouble(gv->nextalths);
	

	if( ( guideAzhs == nextazhs ) && ( guideAlths == nextalths ) ){
		return( true );
	}

	if(serialopen() == false){
		return(false);
	}

	while( ( guideAzhs != nextazhs ) || ( guideAlths != nextalths ) ) {

		if( guiderunning == 0) {
			break;
		}

		azsetting  = 0;
		altsetting = 0;
		frsetting  = 0;


		if( guideAzhs != nextazhs ) {

			if( nextazhs < guideAzhs ) {
				azsetting = azsetting | 2;
			}			

			if( (nextazhs >= scope.hs_az270 ) && ( guideAzhs <= scope.hs_az90) ) {
				azsetting = azsetting | 2;
			}
			else if( ( guideAzhs >= scope.hs_az270 ) && ( nextazhs <= scope.hs_az90 ) ) {
				azsetting = 0;	
			}
			
			azsetting = azsetting | 1;	
		}


		if( guideAlths != nextalths ) {
			altsetting = altsetting | 1;
			
			if( nextalths < guideAlths ) {
				altsetting = altsetting | 2;
			}
			
		}

		azsetting  = azsetting  | 4;
//		altsetting = altsetting | 4;


		setserialoutbuffer(0x1e,azsetting,altsetting,frsetting,0x3f);
		
		if(serialsequenceout() == false){
			return(false);
		}

		if(serialsequencein( 10 ) == false){
			return(false);
		}
		
		guideAzhs  = serial24todouble( &sbufin[2] );

		setserialoutbuffer(0x23,0x3f,0x3f,0x3f,0x3f);
		if(serialsequenceout() == false){
			return(false);
		}
		if(serialsequencein( 10 ) == false){
			return(false);
		}	
		guideAlths = serial24todouble( &sbufin[2] );

	}

	if(serialclose() == false){
		return(false);
	}

	return(true);
}









static Boolean serialOperationStartGuideing( void )
{

	if(serialopen() == false){
		return(false);
	}

	setserialoutbuffer(0x22,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	if(serialsequencein( 10 ) == false){
		return(false);
	}
	guideAzhs = serial24todouble( &sbufin[2] );

	setserialoutbuffer(0x23,0x3f,0x3f,0x3f,0x3f);
	if(serialsequenceout() == false){
		return(false);
	}
	if(serialsequencein( 10 ) == false){
		return(false);
	}	
	guideAlths = serial24todouble( &sbufin[2] );

	//@@@1//
	//setserialoutbuffer(0x24,0x3f,0x3f,0x3f,0x3f);
	//if(serialsequenceout() == false){
	//	return(false);
	//}
	//if(serialsequencein( 10 ) == false){
	//	return(false);
	//}	
	//guideFRHS = serial24todouble( &sbufin[2] );

		
	if(serialclose() == false){
		return(false);
	}

	gcalcStart( guideAzhs, guideAlths, &guidevals );


	/// Now, we have current board-koordinates in halfsteps
	/// and init all for following calls of serialOperationGuide

	return(true);
}







static Boolean serialOperationGuide( void )
{
	double d;


	if(gcalcNext( &guidevals ) == 0){
		return(true);
	}
	
	if(serialOperationKoorsapproach( &guidevals ) == false) {
		return(false);
	}

	return(true);
}