/** ServoTask.cpp -- code to run hobby servo motors using Schip's Task scheduler
 **	  Adds the startMove() timed motion capability
 **   Uses modified Arduino Servo Library function calls.
 ** This code is in an unknown domain.
 **/

// get our stuff
#include <SCHint.h>

//#define DEBUG
//#define TRACE

/** initialize the control structure to given position
 **  NOTE that this does NOT actually move the motor to that position
 **    so the first thing the app needs to do is sync the motors.
 **  TODO: is that right?
 **  just assume that we need to wait a while for this to complete
 **  because we don't know where we are starting from
 */
void SCHervo::taskinit( uint8_t pos)
{ 
	// initialize struct
	this->sMove.curposition = pos;
	this->sMove.endposition = 0;
	this->sMove.steps = 0;
	this->sMove.state = SS_START;

	return;
}

void SCHervo::setcurpos( uint8_t pos)
{
	this->sMove.curposition = pos;
}

/** Start a servo moving to specified newpos degree position
 *    over given time number of milliseconds.
 *	  When done either leave the servo energized or
 *		turn it off (if true).
 */
// NOTE: converted to 8x uS/step to smooth out slow moves
void SCHervo::startMove( uint8_t newpos, uint16_t time, uint8_t off )
{
	int16_t travel;		// pulse width microsec change from current to end
	int16_t move_deg;	// degree change from current to end

	// save the new end position degrees we want
	this->sMove.endposition = newpos;
	move_deg = newpos - this->sMove.curposition;

	// save end position in microsec and
	// calculate microsec travel change, with sign
	//  can't use ::readMicroseconds() for current
	//  because we may have shut off the motor...
	this->sMove.end_us = clampNmap( newpos );
	this->sMove.cur_us = clampNmap( this->sMove.curposition );
	travel = this->sMove.end_us - this->sMove.cur_us;

	// calc number of steps and us/step
	//  we can only change the PW every REFRESHPERIOD millisec
	// steps are the number of refresh steps we need,
	//  round up so we always have at least the last step to do
	this->sMove.steps = (time / REFRESHPERIOD);
	if( (time % REFRESHPERIOD) )	++(this->sMove.steps);

/**/
	// TODO: account for motor travel speed... DEGperREFRESH ~= 5
	//   need to fiddle here....
	//  if time is too short, increase number of steps
	if( move_deg < 0 )		move_deg = -move_deg;	 // be positive!
	uint16_t degstep = move_deg / this->sMove.steps; // degrees per step
	if( degstep > DEGperREFRESH )	// trying to move too fast...
	{
		// adjust number of steps we need to wait
		this->sMove.steps += ((degstep / DEGperREFRESH) + 1);
	}
/**/

	// microsecond PW change we need on every step
	// NOTE: both converted to 8X fixed point,
	//  cur_us is the accumulator and step_us is the change/step
	//  the last step will be set to end_us
	//   in ServoUpdateTask via doServoStep()
	// also NOTE: the int16_t cast is required or we get uint (??!)
//	this->sMove.step_us = ( (travel) / (int16_t)this->sMove.steps);
	this->sMove.step_us = ( (travel<<3) / (int16_t)this->sMove.steps);
	this->sMove.cur_us <<= 3;

#ifdef DEBUG
	Serial.print( "move fr:" );
	Serial.print( this->sMove.curposition );
	Serial.print( " to:" );
	Serial.print( newpos );
	Serial.print( "(" );
	Serial.print( this->sMove.end_us );
	Serial.print( "us) tus:" );
	Serial.print( travel );
	Serial.print( " st:" );
	Serial.print( this->sMove.steps );
	Serial.print( " sus:" );
	Serial.println( this->sMove.step_us );
#endif

	// set stop mode flag and put in run state
	this->sMove.state = (off ? SS_ENDOFF : 0) | SS_RUN;

	// now we wait for ServoUpdateTask to take control
	return;
}


/** Stop servo controlled motion and return current degree position
 *   Leave the servo energized or turn it off as specified in startMove().
 *   Lets the ServoTask do the actual stopping...
 */
uint16_t SCHervo::stopMove(void)
{
	int16_t pos;

	// TOSO: use? pos = this->read();
	// curposition is set to where we want to be after this servo cycle ends
	pos = this->sMove.endposition =  this->sMove.curposition;
	// set steps to zero and let the Task do the work
	this->sMove.steps = 0;

	return pos;
}

/** Do one servo move refresh step for the given motor
 **  internal function
 */
void doServoStep( servo_t *mine, servoMove *myMove )
{
	int16_t incr_us;	// amount to increment microsec position

	/**
	// superflous now
	// get the total number of degrees we need to move, +/-
	deg = myMove->endposition - myMove->curposition;

	// if we don't need to move we're done
	if( deg == 0 )
	{
		// just in case...
		myMove->steps = 0;
		return;
	}
	**/

	if( myMove->steps == 0 )	// done!!
		return;

#ifdef DEBUG
	// print before and after microsec settings
	Serial.print( myMove->steps );
	Serial.print( ' ' );
	Serial.print( (myMove->cur_us >> 3) );
	Serial.print( "->" );
#endif

	// TODO: fix the fast motion slowdown by comparing increments
	// if more than one step left just do an increment
	//  if this is the last step then move to final endpos
	// note curpos is will be out of date until motor settles from this
	if( myMove->steps > 1 )
	{
		// NOTE: cur_us and step_us are 8x Fixed Point values
		//  to allow for slower and smoother motion sweeps
		//  so need to divide down to get actual uS setting
		myMove->cur_us += myMove->step_us;
		incr_us = (myMove->cur_us >> 3);
	}
	else
	{
		// last step, just use whatever is left over
		myMove->cur_us = myMove->end_us;	// note NOT 8xed!
		incr_us = myMove->end_us;
	}

#ifdef DEBUG
	Serial.print( incr_us );
	Serial.print( " = " );
#endif

	// decrement the number of steps we have left
	--(myMove->steps);
	
	// set current position and start move servo,
	//  assume we will get there by the time this comes around again
	//  since there is no feedback about when the motor gets there.
	// also note that write() will update curposition to what it thinks it's doing
	mine->myinstance->write( incr_us );
	myMove->curposition = mine->myinstance->read();

#ifdef DEBUG
	// print current (after motor actually moves) degree position
	Serial.print( ' ' );
	Serial.println( myMove->curposition );
#endif
	return;
}

/** posted every 20ms REFRESHPERIOD when a servo pulse series is completed
 *   so we can synchronously update pulse position values
 */
void ServoUpdateTask( uint16_t nada )
{
	uint8_t i;

	// do something for each motor
	for( i=0; i<ServoCount; ++i )
	{
		servo_t *mine = SERVOP(i);
		servoMove *myMove = &(mine->myinstance->sMove);

		// if we have steps, do one
		// if no steps left and state is flagged for stop, do that
		if( (myMove->steps != 0) )
		{
#ifdef TRACE
			Serial.print( "step:m" );
			Serial.print( i );
			Serial.print( "@" );
			Serial.print( myMove->curposition );
			Serial.print( " - " );
			// Serial.println();
#endif // TRACE

			// update to new move positions
			doServoStep( mine, myMove );
		}
		else
		{
			// TODO: when shutting off we need to make sure
			//   the motor has actually traveled the full distance
			//   ---- somehow ---

			// note: both of these actions are redundant after
			// 		 the first pass but why bother checking?
			// do turn-off if we want it so
			if( myMove->state & SS_ENDOFF )
				mine->myinstance->turnOff();

			// clear state
			myMove->state = SS_START;
		}
	}

	return;
}


/** return TRUE when motor is ready for a new move command or
 *         FALSE otherwise
 *
 *   note: only works when motion is controlled by startMove()
 *          not accurate when using lower level write()
 *	TODO: make this work with the 'normal' write() moves too...
 */
uint8_t SCHervo::ready()
{
	return (this->sMove.state & SS_RUN) ? false : true;
}