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


void BattSwitchControl()  //This is called at about 10kHz.  So this routine will be called 300 times in 30 ms.
{
    static unsigned int SlowSwitchCtr = 0;
    StatusBitFields.EXT_Voltage_Detect = EXT_VOLTAGE_DETECT; //External voltage detect 
    
    if((StatusBitFields.BattSwitchRequest == 0) || (StatusBitFields.EXT_Voltage_Detect  == 0))  //  User or external voltage detect has asked for Batt Switches to be off
    {
            BATTERY_SLOW = 0;   
            StatusBitFields.SlowSwitchSetting = 0;
            BATTERY_SWITCH = 0;
            StatusBitFields.BattSwitchSetting = 0; 
    }
    
    //if((StatusBitFields.EXT_Voltage_Detect  == 1) && (DistanceFromTargetCircle > RE_ENABLE_HYSTERIS) && (StatusBitFields.BattSwitchRequest == 1)  && (StatusBitFields.BattSwitchSetting == 0) && (StatusBitFields.SlowSwitchSetting == 0) && (VoltageModification_Limiter == 0) )  //If it's requested that the battery switch is on, but it isn't, and RPMs are low, initiate turn on procedure.
    if((StatusBitFields.EXT_Voltage_Detect  == 1) && (_Q15abs(RPMs) < 200) && (StatusBitFields.BattSwitchRequest == 1)  && (StatusBitFields.BattSwitchSetting == 0) && (StatusBitFields.SlowSwitchSetting == 0) &&(VoltageModification_Limiter == 0) )  //If it's requested that the battery switch is on, but it isn't, and RPMs are low, initiate turn on procedure.
    {
            BATTERY_SLOW = 1;
            StatusBitFields.SlowSwitchSetting = 1;  
            SlowSwitchCtr = 0;
    }
    
    SlowSwitchCtr++;
    if((StatusBitFields.EXT_Voltage_Detect  == 1) && (StatusBitFields.BattSwitchRequest == 1)  && (StatusBitFields.BattSwitchSetting == 0) && (StatusBitFields.SlowSwitchSetting == 1) && (SlowSwitchCtr == 1000)) //In slow switch turn on sequence
    {
          BATTERY_SWITCH = 1;
          StatusBitFields.BattSwitchSetting = 1;  
    }
    
    if(SlowSwitchCtr >= 4000)
    {
        BATTERY_SLOW = 0;
        StatusBitFields.SlowSwitchSetting = 0;  
        SlowSwitchCtr = 40000;
    }
    
    
}