/*
 SCHervo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers-
 				As hacked up by schip based on Version 2 from Arduino 1.0.4
 original: Copyright (c) 2009 Michael Margolis.  All right reserved.
 
 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

/* 
 A servo is activated by creating an instance of the SCHervo class passing
 the desired pin to the attach() method.  The servos are pulsed in the
 background using the value most recently written using the write() method
 
 Note that analogWrite of PWM on pins associated with the timer are
 disabled when the first servo is attached.
 Timers are seized as needed in groups of 8 servos.

	NOTE: I (schip) think that there should only be 7 or 8 on a timer....
	If they are all set to maximum time 8*2700us == 216000us and
	we want to maintain a 20ms pulse train.
 
 The class interface is:
 SCHervo - Class for manipulating servo motors connected to Arduino pins.
*/

#include <avr/interrupt.h>

// our internal stuff, plus all the external in SCHervo.h
#include "SCHint.h"

// compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
#define TRIM_DURATION       2                     

// static array of servo_t structures, specifying output pin, ticks, and etc
//  used by interrupt and Task functions which are not C++able
//  indexed by the SCHervo->servoIndex value assigned to each object
servo_t servos[MAX_SERVOS];

// servos[] index of the servo currently being pulsed for each timer
//  sequences through all attached servos (set to -1 during "refresh interval")
static volatile int8_t Channel[_Nbr_16timers ];

// the total number of attached servos, number of initialized elements in servos[]
uint8_t ServoCount = 0;

// convenience macros

// The timers are set to the /8 count mode
//  which makes a "tick" == 2.5 per microsecond (on 20MHz Atmega)
// converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)
// converts from ticks back to microseconds
#define ticksToUs(_ticks) (((unsigned)_ticks * 8)/clockCyclesPerMicrosecond())

// returns the timer controlling this servo
#define SERVO_INDEX_TO_TIMER(_servo_nbr) \
	((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER))

// returns the index of the servo on this timer
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) \
	(_servo_nbr % SERVOS_PER_TIMER)

// macro to access servo index by timer and channel
#define SERVO_INDEX(_timer,_channel) \
	((_timer*SERVOS_PER_TIMER) + _channel)

// macro to access servo class by timer and channel
#define SERVO(_timer,_channel) \
	(servos[SERVO_INDEX(_timer,_channel)])

// maximum and minimum values in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - (this->max << 2))
#define SERVO_MIN() (MIN_PULSE_WIDTH - (this->min << 2))

/************ static functions common to all instances ***********************/

/** Interrupt service routine --
 *    called with pointers to interesting timer registers.
 *
 *  Each timer is set up as a counter which increments every micro-second
 *   by setting TCNTn to 0 at the beginning of a "refresh interval" pulse.
 *   (NOTE: "refresh interval" is the 20ms pulse duration used by servos)
 *  This routine steps through the array of active servos[], sequentially
 *   using the value in Channel[timer] as the array index. The tick-count
 *   for the servo is used as the OCRn setting for the next interrupt-count.
 *   Output pulses are set high for the duration of the tick-count and are
 *   reset on the resulting interrupt. Then the next servo is serviced.
 *   When all servos are serviced the OCRn is set to fire at the end
 *   of the 20ms period, and the Channel[] is set to -1 to indicate such.
 *   The net result is that (schip says) a maximum of 7-8 output pulses
 *   of the maximum pulse length can be generated, each following it's
 *   predecessor, in a 20ms period.
 */
static inline void handle_interrupts(
		timer16_Sequence_t timer,		// timer number for this interrupt
		volatile uint16_t *TCNTn,		// address of timer register
		volatile uint16_t* OCRnA )		// address of counter register
{
	uint8_t sidx;		// our index into servos[]
	servo_t *servop;	// pointer to servos[] element of interest

	// channel set to -1 indicates that 20ms refresh interval has completed,
	//  reset the timer and start the servo pulse train thing over again
	if( Channel[timer] < 0 )
	{
		*TCNTn = 0;
	}
	else
	{
		sidx = SERVO_INDEX(timer,Channel[timer]);
		servop = SERVOP(sidx);

		// shut off output pin associated with the previously timed pulse
		//  if it is a valid servo and is Activated
		if( (sidx < ServoCount) && (servop->Pin.isActive == true) )  
		{
			// set this channel low if activated   
			digitalWrite( servop->Pin.nbr, LOW );
		}
	}

	// now increment to the next channel and set its tick-count
	Channel[timer]++;
	sidx = SERVO_INDEX(timer,Channel[timer]);
	servop = SERVOP(sidx);

	// if we still have a valid servo on this timer...
	if( (sidx < ServoCount) && (Channel[timer] < SERVOS_PER_TIMER) )
	{
		uint16_t ticks = servop->ticks;

		// if tick count is set to 0 we want to just shut off the motor
		// but keep doing whatever we were doing
		if( ticks == 0 )
		{
			// set new count for next interrupt to
			//  current counter value + some default resting time...
			//  but don't turn on the pulse output
			ticks = usToTicks(DEFAULT_PULSE_WIDTH);
		}
		else if( (servop->Pin.isActive == true) ) 
		{
			// if servo is actually active toggle the output high
			digitalWrite( servop->Pin.nbr,HIGH);
		}

		// set new count for next interrupt to
		//  current counter value + this new servo's tick-count
		*OCRnA = *TCNTn + ticks;
	}  
	else
	{ 
		// finished with all servo channels on this timer
		// so wait for the refresh interval to expire before starting over 
		if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) )
		{
			// trigger refresh-interval interrupt at end of 20ms
			*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);  
		}
		else 
		{
			// we are way close to, or beyond, the end of the 20ms period...
			// just allow a few ticks to ensure the next OCR1A not missed
			*OCRnA = *TCNTn + 4;
		}

		// flag that we are starting a new period on next interrupt
		//  this will get incremented at the end of the refresh period
		//  to start again at the first channel
		Channel[timer] = -1;

		// schip addition: for servoMove implementation
		//  post a TASK to deal with updating servos timings
		postTask( 0, ServoUpdateTask, 0 );
	}
}

#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
// Interrupt handlers for Arduino 
#if defined(_useTimer1)
ISR(TIMER1_COMPA_vect) 
{ 
  handle_interrupts(_timer1, &TCNT1, &OCR1A);
}
#endif

#if defined(_useTimer3)
ISR(TIMER3_COMPA_vect) 
{ 
  handle_interrupts(_timer3, &TCNT3, &OCR3A); 
}
#endif

#if defined(_useTimer4)
ISR(TIMER4_COMPA_vect) 
{
  handle_interrupts(_timer4, &TCNT4, &OCR4A); 
}
#endif

#if defined(_useTimer5)
ISR(TIMER5_COMPA_vect) 
{
  handle_interrupts(_timer5, &TCNT5, &OCR5A); 
}
#endif

#elif defined WIRING
// Interrupt handlers for Wiring 
#if defined(_useTimer1)
void Timer1Service() 
{ 
  handle_interrupts(_timer1, &TCNT1, &OCR1A); 
}
#endif
#if defined(_useTimer3)
void Timer3Service() 
{ 
  handle_interrupts(_timer3, &TCNT3, &OCR3A); 
}
#endif
#endif

static void initISR(timer16_Sequence_t timer)
{  
#if defined (_useTimer1)
  if(timer == _timer1) {
    TCCR1A = 0;             // normal counting mode 
    TCCR1B = _BV(CS11);     // set prescaler of 8 
    TCNT1 = 0;              // clear the timer count 
#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
    TIFR |= _BV(OCF1A);      // clear any pending interrupts; 
    TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt  
#else
    // here if not ATmega8 or ATmega128
    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts; 
    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 
#endif    
#if defined(WIRING)       
    timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); 
#endif	
  } 
#endif  

#if defined (_useTimer3)
  if(timer == _timer3) {
    TCCR3A = 0;             // normal counting mode 
    TCCR3B = _BV(CS31);     // set prescaler of 8  
    TCNT3 = 0;              // clear the timer count 
#if defined(__AVR_ATmega128__)
    TIFR |= _BV(OCF3A);     // clear any pending interrupts;   
	ETIMSK |= _BV(OCIE3A);  // enable the output compare interrupt     
#else  
    TIFR3 = _BV(OCF3A);     // clear any pending interrupts; 
    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt      
#endif
#if defined(WIRING)    
    timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only	
#endif  
  }
#endif

#if defined (_useTimer4)
  if(timer == _timer4) {
    TCCR4A = 0;             // normal counting mode 
    TCCR4B = _BV(CS41);     // set prescaler of 8  
    TCNT4 = 0;              // clear the timer count 
    TIFR4 = _BV(OCF4A);     // clear any pending interrupts; 
    TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt
  }    
#endif

#if defined (_useTimer5)
  if(timer == _timer5) {
    TCCR5A = 0;             // normal counting mode 
    TCCR5B = _BV(CS51);     // set prescaler of 8  
    TCNT5 = 0;              // clear the timer count 
    TIFR5 = _BV(OCF5A);     // clear any pending interrupts; 
    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt      
  }
#endif
} 

static void finISR(timer16_Sequence_t timer)
{
    //disable use of the given timer
#if defined WIRING   // Wiring
  if(timer == _timer1) {
    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
    TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
    #else 
    TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt   
    #endif
    timerDetach(TIMER1OUTCOMPAREA_INT); 
  }
  else if(timer == _timer3) {     
    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
    TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
    #else
    ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
    #endif
    timerDetach(TIMER3OUTCOMPAREA_INT);
  }
#else
    //For arduino - in future: call here to a currently
	// undefined function to reset the timer
#endif
}

static boolean isTimerActive(timer16_Sequence_t timer)
{
  // returns true if any servo is active on this timer
  for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
    if(SERVO(timer,channel).Pin.isActive == true)
      return true;
  }
  return false;
}


/****************** end of static functions ******************************/
/**************** start the real class methods ***************************/

/** default constructor,
 *  assign a servoIndex number and initialize structs
 *  arbitrarily sets the pulse-width to DEFAULT_PULSE_WIDTH, i.e. 1500
 *   but this probably doesn't match the actual motor
 *   until it's been explicitly set
 **/
SCHervo::SCHervo()
{
  if( ServoCount < MAX_SERVOS)
  {
    this->servoIndex = ServoCount++;   // assign a servo index to this instance
	// store default values  - 12 Aug 2009
	servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);
	servos[this->servoIndex].myinstance = this;
    // initialize default position so servoMove settings agree
	this->taskinit( this->read() );
  }
  else
  {
    this->servoIndex = INVALID_SERVO ;  // too many servos 
  }
}

/** Initialize a servo thingie attached to the specified Arduino pin
 *   with min and max microsecond pulse counts. These will be used as
 *   the 0 and 180 degree values when setting position by degrees.
 **/
uint8_t SCHervo::attach(uint8_t pin, uint16_t min, uint16_t max)
{
  if(this->servoIndex < MAX_SERVOS )
  {
    pinMode( pin, OUTPUT) ;     // set servo pin to output
    servos[this->servoIndex].Pin.nbr = pin;  
    this->setmm( min, max );
    // initialize the timer if it has not already been initialized 
    timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
    if(isTimerActive(timer) == false)
      initISR(timer);    

	// this must be set after the check for isTimerActive
    servos[this->servoIndex].Pin.isActive = true;
  } 

  return this->servoIndex ;
}

/** {re}set min and max pulse widths
 */
void SCHervo::setmm(uint16_t min, uint16_t max)
{
    // note: store the _difference_ between the MIN/MAX defines
    //   and the given limits in this->min,max
    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
    this->min  = (MIN_PULSE_WIDTH - min) >> 2; //resolution of min/max is 4 uS
    this->max  = (MAX_PULSE_WIDTH - max) >> 2;
}

/** Default value initializer
 **/
uint8_t SCHervo::attach(uint8_t pin)
{
  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}

void SCHervo::detach()  
{
  servos[this->servoIndex].Pin.isActive = false;  
  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
  if(isTimerActive(timer) == false) {
    finISR(timer);
  }
}

//  map an angle value into microseconds for setting motor pulse
//  return the microsecond value
uint16_t SCHervo::clampNmap(uint16_t value)
//uint16_t clampNmap(uint16_t value)
{  
  // treat values less than 500 as angles in degrees
  //  (valid values in microseconds are handled as microseconds)
  if( value < MIN_PULSE_WIDTH )
  {
	// clamp angle value and calculate microseconds needed
    if(value < 0) value = 0;
    if(value > 180) value = 180;
    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      
  }

  return( value );
}


// set an output pulse width in degrees or micro-seconds
// clamped to range of MIN_PULSE_WIDTH <=> MAX_PULSE_WIDTH modulo min,max settings
// if value is < MIN_PULSE_WIDTH it's treated as an angle,
//  otherwise as pulse width in microseconds
void SCHervo::write(uint16_t value)
{  
  value = this->clampNmap( value );
  this->writeMicroseconds(value);
}

void SCHervo::writeMicroseconds(uint16_t value)
{
  // calculate and store the values for the given channel
  uint8_t channel = this->servoIndex;
  if( (channel < MAX_SERVOS) )   // ensure channel is valid
  {  
    if( value < SERVO_MIN() )          // ensure pulse width is valid
      value = SERVO_MIN();
    else if( value > SERVO_MAX() )
      value = SERVO_MAX();   
    
	// convert to ticks after compensating for interrupt overhead - 12 Aug 2009
  	value = value - TRIM_DURATION;
    value = usToTicks(value);

    uint8_t oldSREG = SREG;
    cli();
    servos[channel].ticks = value;  
    SREG = oldSREG;

    // note this will be correct _after_ the motor settles
    this->setcurpos( this->read() );
  } 
}

/** Turn off the pulse output to the servo motor so it stops running
 *   but is not detached from the system. Lets motor free run.
 */
void SCHervo::turnOff()
{
    uint8_t oldSREG = SREG;
    cli();
    servos[this->servoIndex].ticks = 0;  
    SREG = oldSREG;   
}

uint16_t SCHervo::read() // return the pulse value as degrees
{
  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);     
}

uint16_t SCHervo::readMicroseconds() // return the pulse value as microseconds
{
  unsigned int pulsewidth;
  if( this->servoIndex != INVALID_SERVO )
    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
  else 
    pulsewidth  = 0;

  return pulsewidth;   
}

/** return true if servo is actually attached ...
 *  doesn't actually check if it's valid though!!?
 */
bool SCHervo::attached()
{
  return servos[this->servoIndex].Pin.isActive ;
}