// File: Power.c
//------------------------------------------------------------------------------
// Author: Giovanni de Sanctis
// Email: info@lateral-technologies.com
//------------------------------------------------------------------------------
// Date: Dec 2018
// Pushbutton, onboard LEDs and other power related functions
//------------------------------------------------------------------------------

#include "Power.h"
#include "mcc_generated_files/mcc.h"
#include "LEDs.h"
#include "Servos.h"

#define T_OFF				2			  //Power button hold time in 0.5sec
#define T_TICK				20			  //20ms
#define T_TIMER_ISR			100			  //100ms

//Battery low threshold assuming 2:1 voltage divider (1023*4.8V/2/3.3)
#define ADC_THR(x)			(155u*x/20u)  //1023/3.3V*5*v/2 (5cells,v=cell voltage in (volt*100)

#define THR_BATT_ZERO		ADC_THR(80u)  //minimum realistic voltage
#define THR_BATT_B0			ADC_THR(124u) //0 bars, critical
#define THR_BATT_B1			ADC_THR(126u) //1 bar  > 20%
#define THR_BATT_B2			ADC_THR(128u) //2 bars > 50%
#define THR_BATT_B3			ADC_THR(130u) //3 bars > 80%
#define THR_BATT_B4			ADC_THR(132u) //4 fully charged
#define THR_BATT_MIN		THR_BATT_B1


//NOTE:Timed battery reading has now been disabled.
//  the <battery> variable is updated only if the app requests a battery reading
//  so the red LED will start blinking only if the app reads a low battery level

//Check the battery every 900*0.1s=90s
//#define TIMEOUT_CHECK_BATT  900		  
//#define TIMEOUT_CHECK_BATT0	100		  //Time out for the first battery read

//Power down if not connected for 1200*0.1=120s
#define TIMEOUT_CONNECT		1200

uint32_t redPattern;		//Stores the pattern for the red LED
uint32_t bluePattern;		//Stores the pattern for the blue LED
uint32_t patternPtr;		//current bit pointer;

//uint16_t battTimer;			//Timer counter to refresh the battery reading 
uint16_t connectTimer;		//Timer counter to power down if not connected 

uint16_t battery;			//Last battery reading
uint8_t  battBars;			//battery bars, from 0 to 4

uint8_t  btConnected;		//>0 if BT module is connected
uint8_t  tick;				//enabled every 20ms;
uint8_t  countTick;			//decremented every timer interrupt.
uint8_t  countTimerISR;		//decremented every timer interrupt.
uint8_t  poweringOff;		//1 to request shut down

void ioISR();

//Called after reset to hold the LDO's enable pin high
void powerBootstrap()
{
  IO_POW_SetDigitalOutput();
  IO_POW_SetHigh();

  btConnected=0;				//Assume not connected
  battery=0x3FF;				//Safe value for battery level
  battBars=4;
  
  //battTimer=TIMEOUT_CHECK_BATT0;//Reset battery timer to enable 1st ADC reading
  //connectTimer=TIMEOUT_CONNECT;	//Reset connect timer
  
  poweringOff = 0;				//We're not shutting down yet
  
  //Light up the LED while the power button is pressed
  while(CMP1_GetOutputStatus()==0)
  {
	IO_LED_RED_N_SetLow();
	__delay_ms(500);
  }
  IO_LED_RED_N_SetHigh();
  
  TMR0_SetInterruptHandler(ioISR);
}

void shutDown()
{
  poweringOff=1;
}

uint8_t checkPowerButton()
{
  uint8_t count=0;
  
  if (CMP1_GetOutputStatus()==0 || poweringOff)
  {
	// Disable the Global Interrupts to stop blinking
	INTERRUPT_GlobalInterruptDisable();
	IO_LED_BLUE_N_SetHigh();			//Turns Off blue LED
	
	while(CMP1_GetOutputStatus()==0)
	{
	  if (count<=T_OFF) 
	  {
		IO_LED_RED_N_SetLow();
      }
	  else
	  {
		stopLEDs();
		stopServos();
	  }
	  __delay_ms(250);
	  IO_LED_RED_N_SetHigh();
	  __delay_ms(250);
	  ++count;
	}
	
	//If power button was pressed for more than T_OFF/2 seconds
	// then turn power off
	if (count>T_OFF || poweringOff) IO_POW_SetLow();
	// Re-Enable the Global Interrupts
    INTERRUPT_GlobalInterruptEnable();
  }
  return 0;
}

void setBluePattern(uint32_t pattern)
{
  //patternPtr=0x00000001;
  if (!pattern) LED_BLUE_Off();
  bluePattern=pattern;
}

//Updates the LED patterns; called in Timer1 every 0.1s
void ioISR()
{
  countTick--;
  if (countTick==0) 
  {
	countTick=T_TICK;
	tick=1;
  }
  countTimerISR--;
  if (countTimerISR) return;
  countTimerISR=T_TIMER_ISR;
  uint8_t status=IO_STA_GetValue();
  
  
  if (status^btConnected) 
  {
	btConnected = status;
	if (status) bluePattern=LED_BLUE_CNNCTD;
	else bluePattern=LED_BLUE_ADVERT;
  }
  else 
  {
	if (!btConnected) 
	{
	  connectTimer--;
	  //Timeout connection to BT
	  if (connectTimer==0) poweringOff=1;
	}
	else connectTimer=TIMEOUT_CONNECT;
  }
  
  if (isBattLow())	redPattern=LED_RED_BATT;
  else			  	redPattern=LED_RED_OFF;
  
  patternPtr<<=1;
  if (patternPtr==0) patternPtr=0x00000001;
  if (redPattern)
  {
    if (redPattern & patternPtr) LED_RED_On();
    else LED_RED_Off();
  }
  if (bluePattern)
  {
    if (bluePattern & patternPtr) LED_BLUE_On();
    else LED_BLUE_Off();
  }
  //if (battTimer) --battTimer;
}

//Waits 20mS (for servo timing)
void wait20ms()
{
  tick=0;
  while(tick==0);
}


//Returns the battery voltage in fractions of 2.048V, i.e. 0x00=0V, 0xFF=2V
//the lsb indicates wheather the voltage is above a set threshold THR_BATT
//if lsb=0 then the voltage is below the threshold.
//The battery voltage depends on the voltage divider between battery and ADC in.
uint16_t getBatt()
{
  uint16_t reading;
    
  if (poweringOff) return battery;			//If shutting down then exit  
  //If servos are running then don't read new value (it would be skewed)
  if (isServoOn()) return battery;

  IO_PWM_LEDS_SetHigh();
  IO_PWM_LEDS_SetDigitalOutput();
  
  //Unlock peripheral select
  PPSLOCK=0x55;							//Magic sequence
  PPSLOCK=0xAA;
  PPSLOCK=0x00;							//Reset PPSLOCKED bit
  
  RB7PPS = 0x00;						//RB7->LATB7  (detaches the out from the PWM3 preripheral)
  __delay_us(160);						//Allow parasitic caps to charge
  reading=ADC1_GetConversion(ADC_BATT);	//Get ADC value
  RB7PPS = 0x05;						//RB7->PWM3:PWM3OUT (reattaches PWM3)
  //Re-lock peripheral select
  PPSLOCK=0x55;							//Magic sequence
  PPSLOCK=0xAA;
  PPSLOCK=0x01;							//Set PPSLOCKED bit
  
  //Make sure we read a meaningful voltage before updating
  if (reading>THR_BATT_ZERO) battery=reading;
  
  if (battery>=THR_BATT_B4) battBars=4;
  else if (battery>=THR_BATT_B3) battBars=3;
  else if (battery>=THR_BATT_B2) battBars=2;
  else if (battery>=THR_BATT_B1) battBars=1;
  else
  {
	battBars=0;
	//shutDown();				//Battery critical, shut down to prevent damage
  }
  return battery;
}

//Returns the battery charge remaining, expressed in 'bars', from 0 to 4
uint8_t getBattBars()
{
  getBatt();
  return battBars;
}


//Returns 1 if battery voltage is below threshold
uint8_t isBattLow()
{
  //getBatt();
  return (uint8_t)(battery<THR_BATT_MIN);
}