/*
    encoder.c

    reading own build encoders from joystickport,
    or parallelport

	and calculating the necessary values.
    
    This requires ioaddr.c
*/




#include "../ioaddr.c"





unsigned char read_parallelencoders( void)
{
	unsigned char val;

	val = read_port(parallelport2 + 1) >> 3 & 0x0f;

	return( val );
}


unsigned char read_joystickencoders( void)
{
	unsigned char val;

	val = read_port(joystickport2) >> 4 & 0x0f;

	return( val );
}





/*
	sets encoder.azpulse, and encoder.altpulse
	to new value of azhs, alths.
	this is only called by ResetKoors,
	when a new position is given.
*/
void encoders_reset( double azhs, double alths )
{
	double _azhs,_alths;
	
	if( encoder.type == NOENCODER ){
		return;
	}

	_azhs  = makeAbsoluteDouble( azhs );
	_alths = makeAbsoluteDouble( alths );

	encoder.azpulse   = _azhs  / encoder.azsighs; 
	encoder.altpulse  = _alths / encoder.altsighs;

	encoder.cazhs     = _azhs;
	encoder.calths    = _alths;
	
	encoder._azflag   = 0;
	encoder._altflag  = 0;
	
	encoder.azleft    = -1;
	encoder.altdown   = -1;

	encoder.az_errcount  = 0;
	encoder.alt_errcount = 0;
}






/*only called, if encodersignals changed, or a previous comparison is gone bad, and its called again*/
int encoders_compare_az( double azhs )
{
	double curpulse;
	
	if( (! encoder.active ) || ( encoder.azleft < 0 ) ){
		encoder.cazhs = azhs;
		encoder.az_errcount = 0;
		return(1);
	}

	curpulse = azhs / encoder.azsighs;
	
	if( curpulse  >= ( encoder.azpulse - encoder.aztolerance )){
		if(curpulse <= ( encoder.azpulse + encoder.aztolerance )){
			encoder.cazhs = azhs;
			encoder.az_errcount = 0;
			return(1);
		}
	}


	encoder.az_errcount = encoder.az_errcount + 1.0;

	/*
	  So, here we have a incorrect az-setting, the encoderposition is not the same
	  as the motorposition
	*/



	if( encoder.az_errcount > ( encoder.azsighs + encoder.aztolerance )){
	
		/*we have lost position for a complete range of encoderpulse*/
		/*Thats to much, we set position to current encoderposition*/

		encoder.correctazhs = encoder.azpulse * encoder.azsighs;
		encoder.az_errcount = 0;
		
		if( prog.debug ){
			fprintf(stderr,"Position - AZ lost for complete range of halfsteps per encoderpulse.\n");
			fprintf(stderr,"We set position to current encodervalue\n");
		}
		return(0);
	}


	/*
	It seems, we have only lost steps,
	so we use the last correct value
	*/

	encoder.correctazhs = encoder.cazhs;
		
	return(0);
}




/*only called, if encodersignals changed, or a previous comparison is gone bad, and its called again*/
int encoders_compare_alt( double alths )
{
	double curpulse;

	if( ( ! encoder.active ) || ( encoder.altdown < 0 ) ){
		encoder.calths = alths;
		encoder.alt_errcount = 0;
		return(1);
	}


	curpulse = alths / encoder.altsighs;

	if( curpulse >= ( encoder.altpulse - encoder.alttolerance )){
		if(curpulse <= ( encoder.altpulse + encoder.alttolerance ) ){
			encoder.calths = alths;
			encoder.alt_errcount = 0;
			return(1);
		}
	}


	encoder.alt_errcount = encoder.alt_errcount + 1.0;

	/*
	  So, here we have a incorrect alt-setting, the encoderposition is not the same
	  as the motorposition
	*/
	
	if( encoder.alt_errcount > ( encoder.altsighs + encoder.alttolerance ) ){
	
		/*we have lost position for two encoder-signalchanges*/
		/*Thats to much, we set position to current encoderposition*/
		
		encoder.correctalths = encoder.altpulse * encoder.altsighs;
		encoder.alt_errcount = 0;
			
		if( prog.debug ){
			fprintf(stderr, "ALT-position lost for complete range of halfsteps per encoderpulse.\n");
			fprintf(stderr, "We set position to current encodervalue\n");
		}
		return(0);
	}


	/*
	It seems, we have only lost steps,
	so we use the last correct value
	*/
		
	encoder.correctalths = encoder.calths;
		
	return(0);
}











/*only called, after an encoder- signalchange*/
void _encoder_set_azvalues( double azhs, double step ) {

	if(encoder.azleft){
		encoder.azpulse = encoder.azpulse - step;
		if(encoder.azpulse < 0.0 ){
			encoder.azpulse = encoder.azpulse360 + encoder.azpulse;
		}
			
	}
	else {
		encoder.azpulse = encoder.azpulse + step;
		if(encoder.azpulse >= encoder.azpulse360 ){
			encoder.azpulse = encoder.azpulse - encoder.azpulse360;
		}
	}

	if( prog.debug ) {
		if( encoder.debugazcount < 22 ){
			encoder.debug_azhs[encoder.debugazcount] = azhs;
			encoder.debugazcount++;
		}		
	}
}


/*only called, after an encoder- signalchange*/
void _encoder_set_altvalues( double alths, double step ) {

	if(encoder.altdown){
		encoder.altpulse = encoder.altpulse - step;
	}
	else {
		encoder.altpulse = encoder.altpulse + step;
	}

	if( prog.debug ) {
		if( encoder.debugaltcount < 22 ){
			encoder.debug_alths[encoder.debugaltcount] = alths;
			encoder.debugaltcount++;
		}
	}

}









/*
  this is called every time the encodersignals are read,
  or a previous comparison is gone bad.
  
  it calculates current pulses, and if the pulse is
  changed, it compares directly current positions azhs, alths
  with this.
  If comparison invalid, it sets encoder.correctazhs, encoder.correctalths
  to right values, and returns with 0.
  Calling function then must set the actual position to encoder.correctazhs,
  encoder.correctalths.
  If all ok, it returns 1, and nothing must be done.

  if one of azhs, or alths is < 0, the comparison for this will not be done.
  Note: comparison will only be done, if signals really changed.
  
  This is not called by serial-interface. Serial interface sets signalchanges
  internally, but uses functions above.
*/

int encoders_calculate( unsigned char val, double azhs, double alths )
{
	int flag = 1;
	unsigned char az,alt,_az,_alt;
	
	if(encoder.type == NOENCODER ){
		return(flag);
	}

		
	if(val == encoder.val){
		
		if( ( encoder.az_errcount > 0.0 ) && ( azhs >= 0.0) ){
			return(encoders_compare_az(azhs));
		}

		if( ( encoder.alt_errcount > 0.0 ) && ( alths >= 0.0 ) ){
			return(encoders_compare_alt(alths));
		}
		
		if(azhs >= 0.0){
			encoder.cazhs  = azhs;
		}
		if(alths >= 0.0){
			encoder.calths = alths;
		}		
		return(flag);
	}


	az   = val;
	az   = az >> 2;
	az   = az & 3;
	alt  = val;
	alt  = alt & 3; 

	_az  = encoder.val;
	_az  = _az >> 2;
	_az  = _az & 3;
	_alt = encoder.val;
	_alt = _alt & 3; 

	encoder.val = val;

	if( az != _az){
		
		encoder._azflag = 1;
		encoder.azleft  = 0;
		
		if(az == 2){
			if(_az == 3){
				encoder.azleft = 1;
			}		
		}
		else if(az == 3){
			if(_az == 1){
				encoder.azleft = 1;
			}		
		}
		else if(az == 1){
			if(_az == 0){
				encoder.azleft = 1;
			}						
		}
		else if(az == 0){
			if(_az == 2){
				encoder.azleft = 1;
			}							
		}

		_encoder_set_azvalues( azhs, 1.0 );
	}

	
	if(alt != _alt){

		encoder._altflag = 1;
		encoder.altdown  = 0;
		
		if(alt == 2){
			if(_alt == 3){
				encoder.altdown = 1;
			}		
		}
		else if(alt == 3){
			if(_alt == 1){
				encoder.altdown = 1;
			}		
		}
		else if(alt == 1){
			if(_alt == 0){
				encoder.altdown = 1;
			}						
		}
		else if(alt == 0){
			if(_alt == 2){
				encoder.altdown = 1;
			}							
		}

		_encoder_set_altvalues( alths, 1.0 );
	}	

	/*
		bit 3,2 = az
		bit 1,0 = alt
	*/



	if( ( azhs >= 0.0 ) && ( encoder._azflag || ( encoder.az_errcount > 0.0 ) ) ){
		if(! encoders_compare_az(azhs)){
			flag = 0;
		}
		encoder._azflag = 0;
	}

	if( ( alths >= 0.0 ) && ( encoder._altflag || ( encoder.alt_errcount > 0.0 ) ) ){
		if(! encoders_compare_alt(alths)){
			flag = 0;
		}
		encoder._altflag = 0;
	}
	
	return(flag);
}