#include "SoftRcPulseOut.h"
/*
 Update 01/03/2013: add support for DigiSpark (http://digistump.com): automatic Timer selection (RC Navy: p.loussouarn.free.fr)
 
 English: by RC Navy (2012)
 =======
 <SoftRcPulseOut>: a library mainly based on the <SoftwareServo> library, but with a better pulse generation to limit jitter.
 It supports the same methods as <SoftwareServo>.
 It also support Pulse Width order given in microseconds. The current Pulse Width can also be read in microseconds.
 The refresh method can admit an optionnal argument (force). If SoftRcPulseOut::refresh(1) is called, the refresh is forced even if 20 ms are not elapsed.
 http://p.loussouarn.free.fr

 Francais: par RC Navy (2012)
 ========
 <SoftRcPulseOut>: une librairie majoritairement basee sur la librairie <SoftwareServo>, mais avec une meilleure generation des impulsions pour limiter la gigue.
 Elle supporte les memes methodes que <SoftwareServo>.
 Elle supporte egalement une consigne de largeur d'impulsion passee en microseconde. La largeur de l'impulsion courante peut egalement etre lue en microseconde.
 La methode refresh peut admettre un parametre optionnel (force). Si SoftRcPulseOut::resfresh(1) est appelee, le refresh est force meme si 20 ms ne se sont pas ecoulee.
 http://p.loussouarn.free.fr
*/

/* Automatic Timer selection (at compilation time) */
#ifndef TIMER_TO_USE_FOR_MILLIS //This symbol is not defined arduino standard core and is defined in core_build_options.h in DigiStump version
#define SOFT_RC_PULSE_OUT_TCNT TCNT0 //For arduino standard core of UNO/MEGA, etc
#else
#if (TIMER_TO_USE_FOR_MILLIS==1)
#define SOFT_RC_PULSE_OUT_TCNT TCNT1 //For example for ATtiny85
#else
#define SOFT_RC_PULSE_OUT_TCNT TCNT0 //For example for ATtiny84
#endif
#endif

SoftRcPulseOut *SoftRcPulseOut::first;

#define NO_ANGLE (0xff)

SoftRcPulseOut::SoftRcPulseOut() : pin(0),angle(NO_ANGLE),pulse0(0),min16(34),max16(150),next(0)
{}

void SoftRcPulseOut::setMinimumPulse(uint16_t t)
{
    min16 = t/16;
}

void SoftRcPulseOut::setMaximumPulse(uint16_t t)
{
    max16 = t/16;
}

uint8_t SoftRcPulseOut::attach(int pinArg)
{
    pin = pinArg;
    angle = NO_ANGLE;
    pulse0 = 0;
    next = first;
    first = this;
    digitalWrite(pin,0);
    pinMode(pin,OUTPUT);
    return 1;
}

void SoftRcPulseOut::detach()
{
    for ( SoftRcPulseOut **p = &first; *p != 0; p = &((*p)->next) ) {
	if ( *p == this) {
	    *p = this->next;
	    this->next = 0;
	    return;
	}
    }
}

void SoftRcPulseOut::write(int angleArg)
{
    if ( angleArg < 0) angleArg = 0;
    if ( angleArg > 180) angleArg = 180;
    angle = angleArg;
    // bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
    // That 64L on the end is the TCNT0 prescaler, it will need to change if the clock's prescaler changes,
    // but then there will likely be an overflow problem, so it will have to be handled by a human.
#ifdef TIMER0_TICK_EVERY_X_CYCLES
    pulse0 = (min16*16L*clockCyclesPerMicrosecond() + (max16-min16)*(16L*clockCyclesPerMicrosecond())*angle/180L)/TIMER0_TICK_EVERY_X_CYCLES;
#else
    pulse0 = (min16*16L*clockCyclesPerMicrosecond() + (max16-min16)*(16L*clockCyclesPerMicrosecond())*angle/180L)/64L;
#endif
}

void SoftRcPulseOut::write_us(int PulseWidth_us)
{
	SoftRcPulseOut::write(map(PulseWidth_us,min16*16,max16*16,0,180));
}

uint8_t SoftRcPulseOut::read()
{
    return angle;
}

uint8_t SoftRcPulseOut::read_us()
{
    return map(angle,0,180,min16*16,max16*16);
}

uint8_t SoftRcPulseOut::attached()
{
    for ( SoftRcPulseOut *p = first; p != 0; p = p->next ) {
	if ( p == this) return 1;
    }
    return 0;
}

uint8_t SoftRcPulseOut::refresh(bool force /* = false */)
{
    uint8_t RefreshDone=0;
    uint8_t count = 0, i = 0;
    uint16_t base = 0;
    SoftRcPulseOut *p;
    static unsigned long lastRefresh = 0;
    unsigned long m = millis();
	if(!force)
	{
		// if we haven't wrapped millis, and 20ms have not passed, then don't do anything
		if ( m >= lastRefresh && m < lastRefresh + 20) return(RefreshDone);
	}
	RefreshDone=1; //Ok: Refresh will be performed
	lastRefresh = m;

    for ( p = first; p != 0; p = p->next ) if ( p->pulse0) count++;
    if ( count == 0) return(RefreshDone);

    // gather all the SoftRcPulseOuts in an array
    SoftRcPulseOut *s[count];
    for ( p = first; p != 0; p = p->next ) if ( p->pulse0) s[i++] = p;

    // bubblesort the SoftRcPulseOuts by pulse time, ascending order
	s[0]->ItMasked=0;
    for(;;)
	{
		uint8_t moved = 0;
		for ( i = 1; i < count; i++)
		{
			s[i]->ItMasked=0;
			if ( s[i]->pulse0 < s[i-1]->pulse0)
			{
				SoftRcPulseOut *t = s[i];
				s[i] = s[i-1];
				s[i-1] = t;
				moved = 1;
			}
		}
		if ( !moved) break;
    }
	for ( i = 1; i < count; i++)
	{
		if ( abs(s[i]->pulse0 - s[i-1]->pulse0)<=5)
		{
			s[i]->ItMasked=1; /* 2 consecutive Pulses are close each other, so do not unmask interrupts between Pulses */
		}
	}
    // turn on all the pins
    // Note the timing error here... when you have many SoftwareServos going, the
    // ones at the front will get a pulse that is a few microseconds too long.
    // Figure about 4uS/SoftRcPulseOut after them. This could be compensated, but I feel
    // it is within the margin of error of software SoftRcPulseOuts that could catch
    // an extra interrupt handler at any time.
	noInterrupts();
    for ( i = 0; i < count; i++) digitalWrite( s[i]->pin, 1);
	interrupts();

    uint8_t start = SOFT_RC_PULSE_OUT_TCNT;
    uint8_t now = start;
    uint8_t last = now;

    // Now wait for each pin's time in turn..
    for ( i = 0; i < count; i++)
	{
		uint16_t go = start + s[i]->pulse0;
		uint16_t it = go - 4; /* 4 Ticks is OK for UNO @ 16MHz */ /* Mask Interruptions just before setting down the pin */

		// loop until we reach or pass 'go' time
		for (;;)
		{
			now = SOFT_RC_PULSE_OUT_TCNT;
			if ( now < last) base += 256;
			last = now;
			if(!s[i]->ItMasked)
			{
				if( base + now > it)
				{
					noInterrupts();
					s[i]->ItMasked=1;
				}
			}
			if ( base + now > go)
			{
				 digitalWrite( s[i]->pin,0);
				 if((i+1)<count)
				 {
					if(!s[i+1]->ItMasked)
					{
						interrupts();
					}
				 }else interrupts();
				 break;
			}
		}
	}
	return(RefreshDone);
}