#include <math.h>
#include "xc.h"
#include "CalibrationConstants.h"
#include "I2C.h"
#include "crc.h"
#include "extern.h" 
#include "config.h"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "UART.h"


const int RPMs_cnt[2] =  {0  , 20000};  //Fractional counts since this doesn't relate to an ADC channel
const  float RPMs_eng[2] =  {0.0 , 8931.839838};  // Interupt timing is set up to create approximately 0.5RPM per bit.
                                                 //encoder difference frequency is (39960375/(4*2*1024*2*10) =  243.8988 Hz = 4.100061624 mS
                                                 //There are 4096*2*2*2 fractional counts per revolution.
                                                 //So if we see 20000 counts during the difference period,
                                                 // that's (20000/(4096*2*2*2))*60/.004100061624  rpm

int Torque_cnt[2];
float Torque_eng[2];  //N-m

int LD_Current_cnt[2];
float LD_Current_eng[2];  //Amps


const CalibrationConstants DefaultCal = {
// Declared with default values that can be over-written with values from EEPROM
    
    .IWind_cnt[0] = 0, .IWind_cnt[1] = 5212, 
    .IWind_eng[0] = 0, .IWind_eng[1] = 9.921,  //Amps
    
    .VBus_cnt[0] = 8728, .VBus_cnt[1] =  21020,
    .VBus_eng[0] = 150.8, .VBus_eng[1] = 352.4,  //Volts

    .IBatt_cnt[0] = 16520, .IBatt_cnt[1] = 19128,
    .IBatt_eng[0] = 0.000, .IBatt_eng[1] = 9.920,   //Amps

    .DiffPress_cnt[0] = 0, .DiffPress_cnt[1] = 14940,
    .DiffPress_eng[0] = 0, .DiffPress_eng[1] = 2.4112,  //AN10 Calibrated in PSI where 5PSI = 10V

    .AuxTorque_cnt[0] =  14480, .AuxTorque_cnt[1] = 16699,
    .AuxTorque_eng[0] = -1.624, .AuxTorque_eng[1] = 1.624,  //Volt
            
    .PhaseA_Offset_cnts = 0, .PhaseB_Offset_cnts = 0,
    
    .LoadDumpResistance = 7.37 //Ohms          
     };


void LoadCalibration()
{
    
   char s[64];
    
   Cal = DefaultCal; 
   
   CalibrationConstants RetrievedCal;
   
   int EEPROM_Reg_Addr = CAL_DATA_EEPROM_ADDR;      
     
   char *data_addr = (char *) &RetrievedCal;
   ReadI2CData(EEPROM_DEVICE_ADDR,EEPROM_Reg_Addr,&data_addr[0],sizeof(CalibrationConstants));

   int CRC = calcCRC((const char*) &(RetrievedCal), sizeof (CalibrationConstants) - 2); //Compute CRC on body of structure and store in last field.
   
   if(RetrievedCal.CRC == CRC)  // Valid calibration read.
   {
       sprintf(s,"Valid CRC\r\n");
        Cal = RetrievedCal;
   }
   else
   {
       sprintf(s,"In-Valid CRC\r\n");
       Cal = DefaultCal;
   }     
   putsU1(s);

}


/*  Compute various scaling between counts and engineering units, based Cal.IWind_cnt/eng and RPMs_cnt/eng */
    /* Important things rely on this being set up, so this function must be called early */
void ComputeScaling(void)
{   
    Torque_cnt[0] = Cal.IWind_cnt[0];
    Torque_cnt[1] = Cal.IWind_cnt[1];
    Torque_eng[0] = Cal.IWind_eng[0]*TORQUE_CONSTANT;
    Torque_eng[1] = Cal.IWind_eng[1]*TORQUE_CONSTANT;
  
    LD_Current_cnt[0] = 0;
    LD_Current_cnt[1] = (Cal.VBus_cnt[1]-Cal.VBus_cnt[0]);
    LD_Current_eng[0] = 0.0;
    LD_Current_eng[1] = (Cal.VBus_eng[1]-Cal.VBus_eng[0])/Cal.LoadDumpResistance;
}

#ifdef DEBUG  //Display the resulting scaling on screen at powerup in debug mode
void PrintScaling(void)
{
    char s[256];
   
     sprintf(s, "Torque cnt/eng    %d     %d     %f     %f\r\n",
            Torque_cnt[0],Torque_cnt[1],
            Torque_eng[0],Torque_eng[1]);
    putsU1(s);
}   
#endif


    
void PrintCalibration(CalibrationConstants C) {
    char s[128];

    sprintf(s, "IWind_cnt[0] = %8d  IWind_cnt[1] = %8d\r\n", C.IWind_cnt[0], C.IWind_cnt[1]);
    putsU1(s);
    sprintf(s, "IWind_eng[0] = %8f  IWind_eng[1] = %8f\r\n", C.IWind_eng[0], C.IWind_eng[1]);
    putsU1(s);
    sprintf(s, "\r\n");
    putsU1(s);
    sprintf(s, "VBus_cnt[0] = %8d  VBus_cnt[1] = %8d\r\n", C.VBus_cnt[0], C.VBus_cnt[1]);
    putsU1(s);
    sprintf(s, "VBus_eng[0] = %8f  VBus_eng[1] = %8f\r\n", C.VBus_eng[0], C.VBus_eng[1]);
    putsU1(s);
    sprintf(s, "\r\n");
    putsU1(s);
    sprintf(s, "IBatt_cnt[0] = %8d  IBatt_cnt[1] = %8d\r\n", C.IBatt_cnt[0], C.IBatt_cnt[1]);
    putsU1(s);
    sprintf(s, "IBatt_eng[0] = %8f  IBatt_eng[1] = %8f\r\n", C.IBatt_eng[0], C.IBatt_eng[1]);
    putsU1(s);
    sprintf(s, "\r\n");
    putsU1(s);
    sprintf(s, "DiffPress_cnt[0] = %8d  DiffPress_cnt[1] = %8d\r\n", C.DiffPress_cnt[0], C.DiffPress_cnt[1]);
    putsU1(s);
    sprintf(s, "DiffPress_eng[0] = %8f  DiffPress_eng[1] = %8f\r\n", C.DiffPress_eng[0], C.DiffPress_eng[1]);
    putsU1(s);
    sprintf(s, "\r\n");
    putsU1(s);
    sprintf(s, "AuxTorque_cnt[0] = %8d  AuxTorque_cnt[1] = %8d\r\n", C.AuxTorque_cnt[0], C.AuxTorque_cnt[1]);
    putsU1(s);
    sprintf(s, "AuxTorque_eng[0] = %8f  AuxTorque_eng[1] = %8f\r\n", C.AuxTorque_eng[0], C.AuxTorque_eng[1]);
    putsU1(s);
    sprintf(s, "\r\n");
    putsU1(s);

    sprintf(s, "PhaseA_Offset_cnts =  %8d\r\n", C.PhaseA_Offset_cnts);
    putsU1(s);
    sprintf(s, "PhaseB_Offset_cnts =  %8d\r\n", C.PhaseB_Offset_cnts);
    putsU1(s);
    sprintf(s, "\r\n");
    putsU1(s);

    sprintf(s, "LoadDumpResistance = %f \r\n", C.LoadDumpResistance);
    putsU1(s);
    sprintf(s, "CRC = 0x%x \r\n", C.CRC);
    putsU1(s);

}