//#include <proc/p32mx440f256h.h>

#include "BatteryMonitor.h"
#include "BoardCfg.h"
#include "timer.h"
#include "ina219.h"
#include "WiFiCtrl.h"
#include "I2C.h"

float mBatteryVoltage;
int mBatteryCurrent;
int mBatterySOC;

float mVoltageMeanSum;
int mVoltageMeanCount;
unsigned int mCurrentMeanSum;
int mCurrentMeanCount;
bool mCurrentModuleOK;

void InitBatteryMonitor()
{
    mBatteryVoltage = 0;
    mBatteryCurrent = 0;
	mBatterySOC = 0;
	mVoltageMeanCount = 0;
	mCurrentMeanCount = 0;
    mCurrentModuleOK = true;
    
    TimerStart(BATTERY_MONITOR_TIMER,100);
    
    
    //experimental stuff!
    mVoltageMeanSum = 0.0;
    mVoltageMeanCount = 0;
    
//    if(ina219Init() == RET_ERROR)
//    {
//        mCurrentModuleOK = false;
//    }
    //ina219SetCalibration_16V_500mA();
   // ina219SetCalibration_16V_200mA();
    
}

void BatteryMonitorTick()
{
    static int NetworkSendCounter;      //Every second (10 counts) we want to send the battery data.
    if(IsTimerExpired(BATTERY_MONITOR_TIMER))
    {
        unsigned int adc;
        double conv, raw;
        
		AD1CHSbits.CH0SA = 1;   //AN1
        AD1CON1bits.SAMP = 0;
        while(AD1CON1bits.DONE == 0);
        adc = ADC1BUF0;
        AD1CON1bits.SAMP = 1;
    //    adc &= 0xFFFE;
        conv = (float)adc / 1023;
        conv *= 3.36;
        raw = conv;
        conv *= 11;
        
        //avoid rollovers in case the LORA network gets disconnected.
        //This could go for a long time but 5000 samples is too much anyways.
        if(mVoltageMeanCount >= 5000)
        {
            mVoltageMeanCount = 0;
            mVoltageMeanSum = conv;
        }
        else
        {
            mVoltageMeanCount++;
            mVoltageMeanSum += conv;
        }
        
        
        
        mBatteryVoltage = conv;
        
        TimerStart(BATTERY_MONITOR_TIMER,100);
        
        if(mCurrentModuleOK == true)
        {
//            mBatteryCurrent = ina219GetCurrent_mA();
//            if(I2CWasLastTransactionOK() == 0 )
//            {
//                mCurrentModuleOK = false;
//            }
        }
		else
		{	
			unsigned int Ref = 0;
			AD1CHSbits.CH0SA = 0;   //AN0
			AD1CON1bits.SAMP = 0;
			while(AD1CON1bits.DONE == 0);
			Ref = ADC1BUF0;
			AD1CON1bits.SAMP = 1;
			
			
			
			
			AD1CHSbits.CH0SA = 2;   //AN2
			AD1CON1bits.SAMP = 0;
			while(AD1CON1bits.DONE == 0);
			adc = ADC1BUF0;
			AD1CON1bits.SAMP = 1;
			//    adc &= 0xFFFE;
			
			adc -= Ref;
			
			conv = (double)adc * 1.0;
			conv /= 1023;
			conv *= 3.3;	//Volts
			conv /= 0.05;	//Amps (50mV/A)
			
			raw = conv;
						
			//avoid rollovers in case the LORA network gets disconnected.
			//This could go for a long time but 5000 samples is too much anyways.
			if(mCurrentMeanCount >= 500)
			{
				mCurrentMeanCount = 1;
				mCurrentMeanSum = adc;
			}
			else
			{
				mCurrentMeanCount++;
				mCurrentMeanSum += adc;
			}
			
			
			
			mBatteryCurrent = adc;
			//mBatteryCurrent = mCurrentMeanSum / mCurrentMeanCount;
		}
        
    }
}

float GetBatteryVoltage(int Reset)
{
	if(Reset == 1)
	{
		mBatteryVoltage = (mVoltageMeanSum/mVoltageMeanCount);
		mVoltageMeanSum = 0.0;
		mVoltageMeanCount = 0;
	}
	
    
    return mBatteryVoltage;
    
}

int GetSolarPanelCurrent()
{
	//mBatteryCurrent = (mCurrentMeanSum/mCurrentMeanCount);
	//mVoltageMeanCount = 0;
	mCurrentMeanSum = 0.0;
	
	return mBatteryCurrent; 
}
float GetConvertedSolarPanelCurrent()
{
	float Cur = (mBatteryCurrent - 2) * (3.3/1023);
	return Cur;
}

int GetBatterySOC()
{
    return mBatterySOC;
}

int SendNetworkBatteryData()
{
//    int VoltageMilliVolts = (int)(mBatteryVoltage * 1000);
//    int BattCurrent = mBatteryCurrent;
//    
//    char BatData[10];
//    BatData[0] = (char)(VoltageMilliVolts & 0x000000FF);    //Battery Voltage 1
//    VoltageMilliVolts >>= 8;
//    BatData[1] = (char)(VoltageMilliVolts & 0x000000FF);    //Battery Voltage 2
//    BatData[2] = (char)(BattCurrent & 0x000000FF);    //Solar panel Current 1
//    BattCurrent >>= 8;
//    BatData[3] = (char)(BattCurrent & 0x000000FF);    //Solar panel Current 2
//
//    SendNetworkData(BatData,4);
    
	printf("Battery voltage: %f\n",mBatteryVoltage);
    
}

bool GetCurrentModuleOK()
{
    return mCurrentModuleOK;
}