#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"

#define R1  10000.0
#define R2  10000.0
#define R3  4530.0
#define R4  10000.0
#define Ron  60.0
#define Roff 300000000.0
#define VR1 14.8   //Measured on 8/26/2015

unsigned char DecodeBatterySignals(float VR3)
{
    float N_fp;
    float VR2;
    float Rlow_inv,Rext_inv;
    static int first = 1;
    static float K1,K2,K3,K4,K5;
    
    if(first)
    {
      K1 = (R2+R3)/R3;
      K2 = 1.0/(R2+R3);
      K3 = (Ron+R4)*(Roff+R4);
      K4 = 24*(Ron+R4);
      K5 = (Roff-Ron);
      first = 0;
    }

    VR2 = K1*VR3;
    Rlow_inv = (VR1-VR2)/(VR2*R1);
    Rext_inv = Rlow_inv-K2;
    N_fp = (K3*Rext_inv-K4)/K5;

    if(N_fp < 0)  N_fp = 0;
    if(N_fp > 24)  N_fp = 24;
       
    return (unsigned char)(N_fp + 0.5);  //Round result and return as a
}