/*
 * timer2.c
 *
 *  Created on: 2 Apr 2018
 *      Author: Mike
 */

#include "ch.h"  // needs for all ChibiOS programs
#include "hal.h" // hardware abstraction layer header

#include "timer2.h"
#define  MICROSECS_PULSE 10


// with a dwell angle of 45 degrees , 4 cylinders and a maximum RPM of 5000
// freq = 5000/60 * 2 = 166Hz. Because the breaker might bounce , we accept the
// first pulse longer than 1/300 of a second as being a proper closure .
// the TIM2 counter counts in 10uS increments,
#define BREAKER_COUNT_MIN (1E6/(MICROSECS_PULSE * 300))

#define SAMPLE_BUFF_SIZE 256
uint16_t halfRot;
uint16_t nominal  = 0;
uint16_t phase10 = 100; // 10 degrees
volatile uint16_t sampleCount = 0;
uint16_t outSampleCount = 0;
volatile uint16_t sampleBuff[SAMPLE_BUFF_SIZE];
typedef enum { WAIT_GAP, SKIP_BOUNCE, HAVE_SAMPLE } sampleState_t ;
sampleState_t  sampleState = WAIT_GAP;
// difference between samples
volatile uint16_t deltaTime;


uint16_t rpm;

void initTimer2()
{
	rccEnableTIM2(FALSE);
	rccResetTIM2();

	TIM2->PSC = 72*MICROSECS_PULSE;
	TIM2->ARR = 60000;
	TIM2->CR1 = ~TIM_CR1_CKD  & (TIM_CR1_CEN |
			TIM_CR1_ARPE );

	/// pulse width 200 uS
	TIM2->CCR1 = 200/MICROSECS_PULSE;

    TIM2->CCER =  TIM_CCER_CC1E | TIM_CCER_CC1P  ; //enabled and active high

    TIM2->CCMR1 = TIM_CCMR1_OC1M_0 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 |
                           TIM_CCMR1_OC1PE ;


    TIM2->CR2 = TIM_CR2_MMS_1 ; // trigger out is 010 = update


    // change the TIM2 CC2 to TIM3 CC1
	rccEnableTIM3(FALSE);
	rccResetTIM3();
	// TIM3 on the PA6 ... pins : remap code 00
	AFIO->MAPR &= ~ AFIO_MAPR_TIM3_REMAP;

	TIM3->PSC = 72*MICROSECS_PULSE;
	TIM3->ARR = 0xFFFF;


	TIM3->CCMR1 = TIM_CCMR1_CC1S_0 /* | TIM_CCMR1_IC1F_0 | TIM_CCMR1_IC1F_1 | TIM_CCMR1_IC1F_2 */ ;  // filter 16, input

	TIM3->CCER = TIM_CCER_CC1E;

    // link TIM3 ITR1 to TIM2 reload
    // use CCR3
    TIM3->CCMR2 = TIM_CCMR2_CC3S_1 | TIM_CCMR2_CC3S_0 ; //  The


	TIM3->CR1 = ~TIM_CR1_CKD  & (TIM_CR1_CEN | TIM_CR1_ARPE );


    nvicEnableVector(TIM3_IRQn,
                          4);



    TIM3->DIER |= TIM_DIER_CC1IE ;
}


void recalcPhase(void)
{
	nominal = halfRot * (long) (phase10)/ 1800;
}

void adjustRPM(void)
{
	if(rpm < 600)
		rpm = 600;
	if(rpm >  5000)
		rpm = 5000;


	float pulseSec = rpm /30;

halfRot = 1e6 / (pulseSec * MICROSECS_PULSE) ;

  TIM2->ARR = halfRot;
recalcPhase();
}

uint16_t setRPM(uint16_t rpm_ )
{
	if(rpm_ >= 600 && rpm_ < 5000)
	{
	  rpm = rpm_;
	  adjustRPM();
	}
	  return halfRot;
}

uint16_t getRPM(void)
{
	return rpm;
}

uint16_t getDelta(void)
{
	return deltaTime;
}

uint16_t wrapIndex(uint16_t index)
{
	if (index >= SAMPLE_BUFF_SIZE)
		index -= SAMPLE_BUFF_SIZE;
    return index;
}


// allows for wrapping
uint16_t getSampleBuff(uint16_t index)
{
	chSysLock();
	return sampleBuff[wrapIndex(index)];
	chSysUnlock();
}


// waits for ignition pulse , debounces readings,
// returns the pulse time, skips debounce time
uint16_t getNextPulse(void)
{
	static uint16_t lastSampleIndex = 0;
	while(1)
	{
			while (outSampleCount == sampleCount)
				chThdSleep(10);

			uint16_t thisTime  = getSampleBuff(outSampleCount);

			outSampleCount = wrapIndex(outSampleCount + 1);
			while (outSampleCount == sampleCount)
				chThdSleep(10);

			uint16_t nextTime = getSampleBuff(outSampleCount);

			// calculate wrapped time delta : should be > than bounce time to allow
			uint16_t diffTime = nextTime - thisTime;

			if(diffTime > BREAKER_COUNT_MIN)
			{
				lastSampleIndex = outSampleCount;
				return nextTime;
			}

	}
  return 0;
}


void processNextPulse(uint16_t retVal)
{

	static uint16_t lastVal = 0;
	// at this point we should try to phase lock
    deltaTime = retVal - lastVal;



    if(deltaTime > 10000)
    {
    	__asm(" BKPT #0");
    }

    lastVal = retVal;





    float nomRPM = 30E6 / (MICROSECS_PULSE * deltaTime) ;

	rpm = rpm + (nomRPM -rpm)/10;




	uint16_t skew = 32768 - nominal;

	long delta = (retVal+skew) - (nominal+skew);

	if(delta > 10)
		rpm = rpm - 1;
	if(delta -10)
		rpm = rpm + 1;

//	rpm += delta / 256;

	adjustRPM();
    }




// set the timing advance from reference to
void setAdvance(int16_t deg10)
{
    phase10 = deg10;
    recalcPhase();

}

// timer 3 interrupt
void VectorB4(void)
{
	uint16_t stat = TIM3->SR;
	if(stat & TIM_SR_CC1IF)
	{
		TIM3->SR &= ~TIM_SR_CC1IF;
	    uint16_t sample = TIM3->CCR1;
	    sampleBuff[sampleCount++] = sample;
	    if (sampleCount >= SAMPLE_BUFF_SIZE)
	    	sampleCount = 0;
	}
}