/*
 * 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 1


// 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 COUNT_FROM_RPM(RPM) ((1E6/(MICROSECS_PULSE * 30 / (RPM ) )))


uint16_t nominal = 0;
uint16_t halfRot;
uint16_t phase10 = 100; // 10 degrees
volatile uint16_t sampleVar;
volatile uint16_t sampleRef;

volatile uint16_t lastSampleRef = 0;

volatile uint8_t refCount = 0;
volatile uint8_t varCount = 0;




volatile uint16_t samplePeriod = 0 ;

static signed phaseSamp = 0;
static uint8_t validPhaseSamp = 0;

int gainControl = 1000 ;


uint16_t rpm;
signed count;
signed delta;



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

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

}

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

uint16_t getRPM(void)
{
	return rpm;
}

signed getDelta(void)
{
	return delta;
}

signed getCount(void)
{
	return  count;
}

void setGain(int gain)
{
	gainControl = gain;
}




void processPhase ( void )
{
    // lpcl
	chSysLock();

	signed pd;
	pd = 0;
static	unsigned sp = 0;
	if(validPhaseSamp)
	{
	 pd =   phaseSamp;
     validPhaseSamp = 0;

     delta = phaseSamp;

     sp = samplePeriod;
	}

     chSysUnlock();


     static int sampleAverage = 0;

     static int phaseAverage  = 0;
     const int freqScale = 40;

     const int phaseScale = 200;

     sampleAverage = sampleAverage + (sp - sampleAverage/freqScale);





     int32_t arr;

   // 	if(lock)

       int intSample = sampleAverage / freqScale;

static unsigned strange = 0;



       int deltaPd= pd/10;

       if(pd < -2000 || pd > 2000)
       {
    	   strange++;
       }

       arr =  intSample + deltaPd;





    if(arr > 65535)
    	arr = 65535;
    if(arr < 1000)
    	arr = 1000;



     count = arr;

    TIM2->ARR = arr -1;
    recalcPhase();



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

	rpm =  nomRPM ;


	adjustRPM();
    }




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

}



// specialist timer setup :
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


    // link TIM3 ITR2 to TIM2 reload
    // use TS = 001 to make TRC from Tim2 TRIGGER
	TIM3->SMCR  &= ~(TIM_SMCR_TS_Msk );
	TIM3->SMCR  |=  TIM_SMCR_TS_0; // select ITR2 as trigger source TRC

    TIM3->CCMR1 |=  TIM_CCMR1_CC2S_1 |  TIM_CCMR1_CC2S_0 ; //  The CC2S bits are 11, use TRC

	TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;

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


    nvicEnableVector(TIM3_IRQn,
                          4);



    TIM3->DIER |= TIM_DIER_CC1IE  | TIM_DIER_CC2IE;
}


// timer 3 interrupt
void VectorB4(void)
{

	if(TIM3->SR & TIM_SR_CC1IF)
	{
		uint16_t sample = TIM3->CCR1;
//		if(sample-lastSampleRef >  1000 /*BREAKER_COUNT_MIN */)
		{

	     	samplePeriod = sample-sampleRef;

	     	sampleRef = sample;

			++refCount;
		}
		lastSampleRef= sample;

	}
	if(TIM3->SR & TIM_SR_CC2IF)
	{
		 sampleVar  = TIM3->CCR2;
		 ++varCount;
	}

    if(refCount == 1 && varCount == 1)
    {
    	if(sampleRef == sampleVar)
    		phaseSamp = 0;
    	else
    	{

    	uint16_t refToVar = sampleRef - sampleVar;
    	uint16_t varToRef = sampleVar - sampleRef;

    	if(refToVar < 32768)
    		phaseSamp = refToVar;
    	else if(varToRef < 32768)
    		phaseSamp = -varToRef;

    	}


    	validPhaseSamp = 1;
        refCount=0;
        varCount=0;

    }

    // frequency error, should deal with by direct period measurement
    if(refCount > 1 || varCount > 1  )
    {
    	refCount = 0;
    	varCount = 0;

    }

}