#include <dsp.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "extern.h"
#include "conv.h"
#include "config.h"
#include "CalibrationConstants.h"


void LowVoltageDisconnect(unsigned int VBatt)
{
    static int first_time = 1;
    static int LVD_off;
    static int LVD_on;
    static unsigned int LVD_off_ctr;
    static unsigned int LVD_on_ctr;


    if(first_time)  //Compute voltages and times in fractional units
      {
      LVD_off = ConvertEngUnitsToFract(LVD_OFF, VBatt_cnt, VBatt_eng);
      LVD_on = ConvertEngUnitsToFract(LVD_ON, VBatt_cnt, VBatt_eng);
      LVD_off_ctr = LVD_OFF_TIME * CAN_PACKET_RATE;
      LVD_on_ctr = LVD_ON_TIME * CAN_PACKET_RATE;
      PS_OUTPUT = 1;  //Turn on at power-up, this is just for convenience when testing in the lab really, in use this controller shouldn't turn on and off, and if it does, it just turns on for a bit before turning off if the voltage is too low.
      Status.bits.PowerSupply = 1;  //Reflect change in status bits
      first_time = 0;
      }


    if((VBatt >= LVD_off) && (VBatt <= LVD_on))  //Reset both counters in hysterysis zone.
    {
     LVD_on_ctr = LVD_ON_TIME * CAN_PACKET_RATE;
     LVD_off_ctr = LVD_OFF_TIME * CAN_PACKET_RATE;
    }

    if(VBatt < LVD_off)
    {
        if(--LVD_off_ctr == 0)
           {
           PS_OUTPUT = 0;       //Voltage has been below off threshold for LVD_OFF_TIME seconds, disconnect power supply and reset on timer
           Status.bits.PowerSupply = 0;  //Reflect change in status bits
           LVD_on_ctr = LVD_ON_TIME * CAN_PACKET_RATE;
           }
    }

    if(VBatt > LVD_on)
    {
        if(--LVD_on_ctr == 0)
           {
           PS_OUTPUT = 1;       //Voltage has been above on threshold for LVD_ON_TIME seconds, connect power supply and reset off timer.
           Status.bits.PowerSupply = 1;  //Reflect change in status bits
           LVD_off_ctr = LVD_OFF_TIME * CAN_PACKET_RATE;
           }
    }


}