/*
 * Copyright (c) 2020, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */


// Include manual start switch

#include "ti_msp_dl_config.h"
#include <stdlib.h>

#define RS 100
#define RES60 12500 // no diff probe connected 
//#define RES60 3571 // RES60 || HV Probe Rin_cm in kohm.  12.5M||5M
#define RES60_DIV 315
#define SamplesSize 2000 // ADC buffer size
#define vRef 3300 // get more precision
#define ADCResolution 4096

// classify various modes of operation
#define SETTLED_MODE 0 // small caps
#define DECAY_MODE 1 // we use prediction for decay
#define DECAY_MODE_LONG 2 // we use prediction for decay
#define CHARGE_MODE 3 // we use prediction for charge
#define CHARGE_MODE_LONG 4 // we use prediction for charge
#define OUT_OF_RANGE_MODE 5 // we use the last adc reading or increase the SSR holding time e.g from 1s to 2s


#define E1 990 // total time for one Rp and Rn is 2xE1 in ms use: 990 600
#define X 50 // settled mode number of samples to average at the end of each half cycle
#define TS (E1/3)
#define S1 0
#define S2 (S1+TS)
#define S3 (S2+TS)

#define E2 (E1+TS-1)
#define E3 (E2+TS)
#define avenum_ (TS) // dummy variable e.g. TS or TS/2 etc. to lower the sram
#define avenum (avenum_-1) // number of prediction to average.  make sure avenum + 2xTS < E1
#define IMULT 1//TS/avenum_ // avenum and IMULT work together.  keep avenum*IMULT < TS
#define totnum 10 // number of data to log

#define adc_sample_index_1 (E1-10) // starting point for vbat averaging
#define adc_sample_index_2 (E1*2-10) // starting point for vbat averaging

#define dVdt_1kV 55 //
#define dVdt2_1kV 20 //
#define VBUS_1kV 3940 //adc code equivalent to 1kV

int16_t dVdt_start[5]= {0};
int16_t dVdt_end[5]= {0};
int16_t dVdt_user; 
int16_t dVdt2;
int16_t dVdt_th = 0;
int16_t dVdt2_th = 0;


uint16_t predictionMode = SETTLED_MODE;

uint32_t vn1 = 0;
uint32_t vn2 = 0;
uint32_t vp1 = 0;
uint32_t vp2 = 0;


uint16_t vBat;
uint16_t vBatMax = 0;
uint32_t rn;
uint32_t rp;
uint16_t rMin = UINT16_MAX;
uint16_t iSO;
volatile uint32_t gRn[totnum] = {0};
volatile uint32_t gRp[totnum] = {0};
uint32_t gRp_mean = 0;
uint32_t gRn_mean = 0;
uint32_t gRp_sum = 0;
uint32_t gRn_sum = 0;
uint16_t vbat_adc_0;
uint16_t vbat_adc_1;

uint32_t vt0vp1[avenum] = {0};
uint32_t vt1vp1[avenum] = {0};
uint32_t vt2vp1[avenum] = {0};
uint16_t vt0vp2[avenum] = {0};
uint16_t vt1vp2[avenum] = {0};
uint16_t vt2vp2[avenum] = {0};

uint32_t vt0vn1[avenum] = {0};
uint32_t vt1vn1[avenum] = {0};
uint32_t vt2vn1[avenum] = {0};

uint32_t vt0vn2[avenum] = {0};
uint32_t vt1vn2[avenum] = {0};
uint32_t vt2vn2[avenum] = {0};

uint32_t vn1x[avenum] = {0};
uint32_t vn2x[avenum] = {0};
uint32_t vp1x[avenum] = {0};
uint32_t vp2x[avenum] = {0};



uint16_t gADC0[SamplesSize] = {0};
uint16_t gADC1[SamplesSize] = {0};

uint32_t tau_us[totnum] = {0}; // in us
uint32_t req_kohm = 0; // in kohm
uint16_t cap_sys_nF[totnum] = {0}; // in nF
uint32_t cap_sys_sum_nF = 0;
uint16_t cap_sys_ave_nF[totnum] = {0};
uint16_t dvdt_dt_time = 50;
uint16_t dVdt_zero[totnum] = {0};

int gCount = 0;
bool gADCChoice = false;
int gCtr0 = 0;
int gCtr1 = 0;
uint16_t i =0;

int main(void)
{
    
    SYSCFG_DL_init();

    NVIC_EnableIRQ(ADC12_0_INST_INT_IRQN);
    NVIC_EnableIRQ(ADC12_1_INST_INT_IRQN);

    DL_ADC12_enableConversions(ADC12_0_INST);
    DL_ADC12_enableConversions(ADC12_1_INST);
    DL_TimerA_startCounter(TIMER_0_INST);    
    // set init sw state
    DL_GPIO_setPins(GPIO_GRP_0_PORT, GPIO_GRP_0_SW1_PIN);

    while (1) {

        // toggle switch when at 1000ms
        if (gCtr0 == E1 && gCtr1 == E1){
            //adc and timer still run, we just need to toggle sw

            DL_GPIO_togglePins(GPIO_GRP_1_PORT, GPIO_GRP_1_SW2_PIN); // init OFF, now ON
            DL_GPIO_togglePins(GPIO_GRP_0_PORT, GPIO_GRP_0_SW1_PIN); // initially ON, now toggle OFF
        }
        // toggle again and reset counter
        else if (gCtr0 == 2*E1 && gCtr1 == 2*E1){
            // debug
            DL_GPIO_togglePins(GPIO_GRP_3_PORT, GPIO_GRP_3_GIO_DB_PIN);
            //adc and timer stopped in ISR
            gCtr0 =0;
            gCtr1 =0;
            //reset
            vn1 = 0;
            vn2 = 0;
            vp1 = 0;
            vp2 = 0;


            // debug
            //DL_GPIO_togglePins(GPIO_GRP_3_PORT, GPIO_GRP_3_GIO_DB_PIN);
            //__BKPT();
            

            //do math here
            
                // calc the dV/dt slope in code/10ms
                for (uint16_t i =0; i< 5; i++){                    
                    dVdt_start[i] = gADC1[i+50]-gADC1[i]; // ADC1 is for VDC+ sensing.  adc code with 50ms spacing. do it for 5 samples.  we might have to average later

                    dVdt_end[i] = gADC1[S3+50+i]-gADC1[S3+i]; //
                }

                dVdt2 = abs(dVdt_end[0] - dVdt_start[0]);

                dVdt_user = dVdt_start[0];

                // Vp1 + Vn1
                vbat_adc_0 = gADC0[adc_sample_index_1] + gADC1[adc_sample_index_1];
                // Vp2 + Vn2
                vbat_adc_1 = gADC0[adc_sample_index_2] + gADC1[adc_sample_index_2];

                dVdt2_th = (dVdt2_1kV*vbat_adc_0)/VBUS_1kV;

                if(dVdt2 > dVdt2_th && abs(dVdt_end[0]) < 2 ){
                    dVdt_th = (dVdt_1kV*vbat_adc_0)/VBUS_1kV/2+1;
                }
                else{
                    dVdt_th = (dVdt_1kV*vbat_adc_0)/VBUS_1kV+1;
                }

                if (-dVdt_th < dVdt_user && dVdt_user < 0){
                    predictionMode = DECAY_MODE;// use 10ms between samples
                } else if (0 < dVdt_user && dVdt_user <dVdt_th){
                    predictionMode = CHARGE_MODE; // use 10ms between samples
                } else if (dVdt_user ==0){
                    predictionMode = OUT_OF_RANGE_MODE; // mode 5. no dV/dt means a very large tau that's out of range for this method
                } else{
                    predictionMode = SETTLED_MODE;  // mode 0. dV/dt is high so we don't need to use prediction, just wait a bit of time for settling
                }

                //switch state 1 copy adc data to local array for readability
                // ADC1 is VDC+
                for (i = 0; i < avenum; i++) {
                    vt0vp1[i] = gADC1[S1+i*IMULT];
                    vt1vp1[i] = gADC1[S2+i*IMULT];
                    vt2vp1[i] = gADC1[S3+i*IMULT];

                    vt0vp2[i] = gADC1[E1+i*IMULT];
                    vt1vp2[i] = gADC1[E2+i*IMULT];
                    vt2vp2[i] = gADC1[E3+i*IMULT];

                    vt0vn1[i] = gADC0[S1+i*IMULT];
                    vt1vn1[i] = gADC0[S2+i*IMULT];
                    vt2vn1[i] = gADC0[S3+i*IMULT];

                    vt0vn2[i] = gADC0[E1+i*IMULT];
                    vt1vn2[i] = gADC0[E2+i*IMULT];
                    vt2vn2[i] = gADC0[E3+i*IMULT];
                }




/* use these data points for controlled debug mode
                vt0vp1[0] = 1396;
                vt0vp1[1] = 1394;
                vt0vp1[2] = 1397;
                vt1vp1[0] = 1449;
                vt1vp1[1] = 1446;
                vt1vp1[2] = 1450;
                vt2vp1[0] = 1485;
                vt2vp1[1] = 1482;
                vt2vp1[2] = 1485;

                vt0vp2[0] = 1540;
                vt0vp2[1] = 1543;
                vt0vp2[2] = 1539;
                vt1vp2[0] = 1485;
                vt1vp2[1] = 1494;
                vt1vp2[2] = 1490;
                vt2vp2[0] = 1456;
                vt2vp2[1] = 1460;
                vt2vp2[2] = 1456;

                vbat_adc_0 = 3159;
                vbat_adc_1 = 3159;


                //__BKPT();
*/
                //predictionMode = CHARGE_MODE;

                switch (predictionMode){
                    case DECAY_MODE:
                        for (i = 0; i < avenum; i++) {

                            vn1x[i] = (- vt1vp1[i]*vt1vp1[i] + vt0vp1[i]*vt2vp1[i])/(vt0vp1[i] - 2*vt1vp1[i] + vt2vp1[i]); // decay
                            vn2x[i] =  vt0vp2[i]+(-2*vt0vp2[i]*vt1vp2[i]+vt1vp2[i]*vt1vp2[i]+vt0vp2[i]*vt0vp2[i])/(-vt0vp2[i] + 2*vt1vp2[i] - vt2vp2[i]); //charge

                            vn1x[i] = vt0vn1[i]+(-2*vt0vn1[i]*vt1vn1[i]+vt1vn1[i]*vt1vn1[i]+vt0vn1[i]*vt0vn1[i])/(-vt0vn1[i] + 2*vt1vn1[i] - vt2vn1[i]); //charge
                            vn2x[i] = (- vt1vn2[i]*vt1vn2[i] + vt0vn2[i]*vt2vn2[i])/(vt0vn2[i] - 2*vt1vn2[i] + vt2vn2[i]); // decay

                            vp1 += vp1x[i];
                            vp2 += vp2x[i];
                            vn1 += vn1x[i];
                            vn2 += vn2x[i];
                            //__BKPT();
                        }
                        // calc average
                        vp1 = vp1/avenum;
                        vp2 = vp2/avenum;
                        vn1 = vn1/avenum;
                        vn2 = vn2/avenum;
                        break;

                    case CHARGE_MODE:
                        for (i = 0; i < avenum; i++) {
                            vp1x[i] =  vt0vp1[i]+(-2*vt0vp1[i]*vt1vp1[i]+vt1vp1[i]*vt1vp1[i]+vt0vp1[i]*vt0vp1[i])/(-vt0vp1[i] + 2*vt1vp1[i] - vt2vp1[i]); //charge
                            vp2x[i] = (- vt1vp2[i]*vt1vp2[i] + vt0vp2[i]*vt2vp2[i])/(vt0vp2[i] - 2*vt1vp2[i] + vt2vp2[i]); // decay

                            vn1x[i] = (- vt1vn1[i]*vt1vn1[i] + vt0vn1[i]*vt2vn1[i])/(vt0vn1[i] - 2*vt1vn1[i] + vt2vn1[i]); // decay
                            vn2x[i] = vt0vn2[i]+(-2*vt0vn2[i]*vt1vn2[i]+vt1vn2[i]*vt1vn2[i]+vt0vn2[i]*vt0vn2[i])/(-vt0vn2[i] + 2*vt1vn2[i] - vt2vn2[i]); //charge

                            vp1 += vp1x[i];
                            vp2 += vp2x[i];
                            vn1 += vn1x[i];
                            vn2 += vn2x[i];
                            //__BKPT();
                        }
                        // calc average
                        //__BKPT();
                        vp1 = vp1/avenum;
                        vp2 = vp2/avenum;
                        vn1 = vn1/avenum;
                        vn2 = vn2/avenum;

                        break;

                    case OUT_OF_RANGE_MODE:
                        //__BKPT();
                        for (i = 0; i < X; i++) {
                            vp1 += gADC1[E1-X+i];
                            vp2 += gADC1[2*E1-X+i];
                            vn1 += gADC0[E1-X+i];
                            vn2 += gADC0[2*E1-X+i];
                        }
                        vp1 = vp1/X;
                        vp2 = vp2/X;
                        vn1 = vn1/X;
                        vn2 = vn2/X;
                        // error bit = 1
                        break;
                    case SETTLED_MODE:
                        for (i = 0; i < X; i++) {
                            vp1 += gADC1[E1-X+i];
                            vp2 += gADC1[2*E1-X+i];
                            vn1 += gADC0[E1-X+i];
                            vn2 += gADC0[2*E1-X+i];
                        }
                        vp1 = vp1/X;
                        vp2 = vp2/X;
                        vn1 = vn1/X;
                        vn2 = vn2/X;
                        // error bit = 1
                        break;
                    default:
                        // error bit = 2
                        break;
                }



                //__BKPT();

                // equations for 6:5
                //rn = -(30*RS*vn1*vp2 - 30*RS*vn2*vp1)/(5*vn1*vp2 - 5*vn2*vp1 + vp1*vp2); 
                //rp = -(30*RS*vn1*vp2 - 30*RS*vn2*vp1)/(6*vn1*vp2 - 5*vn2*vp1);

                // example test voltages
                //vn1 = 1262; // SWT: 10 ; 1:1
                //vn2 = 1390; // SWT: 01 ; 5:7

                //vp1 = 1263; // SWT: 10
                //vp2 = 1134; // SWT: 11


                //vn1 = 1599; // SWT: 10 ; 1:1
                //vn2 = 1763; // SWT: 01 ; 5:7

                //vp1 = 1559; // SWT: 10
                //vp2 = 1395; // SWT: 11


                //vn1 = 1433; // SWT: 10 ; 7:5
                //vn2 = 1760; // SWT: 01 ; 5:7

                //vp1 = 1755; // SWT: 10
                //vp2 = 1429; // SWT: 11

                //equations for 7:5 & 1:1
                //rn = -(30*vn1*vp2 - 30*vn2*vp1)*RS/(5*vn1*vp2 - (30*vn2*vp1)/7 + vp1*vp2);
                //rp = -(21*vn1*vp2 - 21*vn2*vp1)*RS/((5*vn1*vn2 + 42*vn1*vp2 - 35*vn2*vp1)/10);

                //equations for 7:5 & 5:7
                // if VBus>775V, adjust fixed point math to avoid overflow
                if(vbat_adc_0 > 3050){

                    rn = -(35*vn1*vp2 - 35*vn2*vp1)*(RS/5)/(7*vn1*vp2 - 5*vn2*vp1 + 2*vp1*vp2);
                    rp = -(35*vn1*vp2 - 35*vn2*vp1)*(RS/5)/(2*vn1*vn2 + 7*vn1*vp2 - 5*vn2*vp1);

                    rn = rn*5;
                    rp = rp*5;

                    // recover precision if Riso is < 800khohm.  aka 3 sig fig
                    if(rn<800){
                        rn = -(35*vn1*vp2 - 35*vn2*vp1)*(RS)/(7*vn1*vp2 - 5*vn2*vp1 + 2*vp1*vp2);
                    }
                    if(rp<800){

                        rp = -(35*vn1*vp2 - 35*vn2*vp1)*RS/(2*vn1*vn2 + 7*vn1*vp2 - 5*vn2*vp1);

                    }


                } else{
                    rn = -(35*vn1*vp2 - 35*vn2*vp1)*RS/(7*vn1*vp2 - 5*vn2*vp1 + 2*vp1*vp2);
                    rp = -(35*vn1*vp2 - 35*vn2*vp1)*RS/(2*vn1*vn2 + 7*vn1*vp2 - 5*vn2*vp1);

                }



                // calculate   tau in ms
                // dv/dt where dt is ms. need to see if 1, 10, or 50ms makes a difference in error
                // rn||rp includes RES60 already
                // assume charging condition when SW0 turns on.  should be true at steady state
                req_kohm = rn*rp/(rn+rp);
                req_kohm = req_kohm*7*RS/(req_kohm+7*RS);
                req_kohm = req_kohm*5*RS/(req_kohm+5*RS);

                if(dVdt_start[0]>80 && dVdt_end[0]<2){
                   dvdt_dt_time = 2; //ms
                }
                else if(dVdt_start[0]>50 && dVdt_end[0]<2)
                   dvdt_dt_time = 10; //ms
                else{
                   dvdt_dt_time = 30; //ms
                }

                //debug with this
                //dvdt_dt_time = 10;
                cap_sys_sum_nF = 0;
                for (i = 0; i < totnum; i++) {
                    dVdt_zero[i] = gADC1[dvdt_dt_time+i]-gADC1[S1+i];
                    tau_us[i] = (vp1- gADC1[S1+i+dvdt_dt_time/2])*dvdt_dt_time*1000/(dVdt_zero[i]);
                    cap_sys_nF[i] = tau_us[i]/(req_kohm);
                    cap_sys_sum_nF += cap_sys_nF[i];
                }
                 cap_sys_ave_nF[gCount]  = cap_sys_sum_nF/totnum;
               
                // datalogger
                // for the open circuit case rp ~ inf we don't want to do this math
                if (rp < RES60-500){
                    gRp[gCount] = rp * RES60 /(RES60-rp);
                }
                else{
                    //error case
                    gRp[gCount] = 99999;
                }

                if (rn < RES60-500){
                    gRn[gCount] = rn * RES60 /(RES60-rn);
                }
                else{
                    //error case
                    gRn[gCount] = 99999;
                }

                gRp_sum += gRp[gCount];
                gRn_sum += gRn[gCount];

                gCount += 1;
                if (gCount >= totnum) {
                    gRp_mean = gRp_sum/totnum;
                    gRn_mean = gRn_sum/totnum;
                    gRp_sum = 0;
                    gRn_sum = 0;
                    gCount = 0;
                //   __BKPT();  
                }
               /*
                // adc code * vref / (2^n - 1)
                vn1 = (vn1 * vRef) / (ADCResolution - 1);
                vn2 = (vn2 * vRef) / (ADCResolution - 1);
                vp1 = (vp1 * vRef) / (ADCResolution - 1);
                vp2 = (vp2 * vRef) / (ADCResolution - 1);
                //convert to actual Vbus voltage in Volts
                vBat = (vn2 + vp2)*RES60_DIV / 1000;

                if (rn > rp) {
                    rMin = rp;
                }
                else {
                    rMin = rn;
                }
                
                if (vBat > vBatMax) {
                    vBatMax = vBat; 
                }

                iSO = rMin / vBatMax;
                //__BKPT(); 

                 */
            // debug
            DL_GPIO_togglePins(GPIO_GRP_3_PORT, GPIO_GRP_3_GIO_DB_PIN);
            // start timer again and toggle sw
            DL_TimerA_startCounter(TIMER_0_INST);     //start timer  
            //__WFI(); // timer triggers only at zero event.  i think we need to reset     
            DL_GPIO_togglePins(GPIO_GRP_1_PORT, GPIO_GRP_1_SW2_PIN); // init OFF, now ON
            DL_GPIO_togglePins(GPIO_GRP_0_PORT, GPIO_GRP_0_SW1_PIN); // initially ON, now toggle OFF

        }
        __WFI();        
    }
}

void ADC12_0_INST_IRQHandler(void)
{
    switch (DL_ADC12_getPendingInterrupt(ADC12_0_INST)) {
        case DL_ADC12_IIDX_MEM0_RESULT_LOADED:
            //toggle gpio for debug
            //DL_GPIO_togglePins(GPIO_GRP_3_PORT, GPIO_GRP_3_GIO_DB_PIN);

            gADC0[gCtr0] = DL_ADC12_getMemResult(ADC12_0_INST, DL_ADC12_MEM_IDX_0);        
            gCtr0 += 1;
            if (gCtr0 > 2*E1-1) {
                //DL_ADC12_disableConversions(ADC12_0_INST);
                DL_ADC12_clearInterruptStatus(ADC12_0_INST,(DL_ADC12_INTERRUPT_MEM0_RESULT_LOADED));      
                DL_TimerA_stopCounter(TIMER_0_INST);
            }
            break;
        default:
            break;
    }
}

void ADC12_1_INST_IRQHandler(void)
{
    switch (DL_ADC12_getPendingInterrupt(ADC12_1_INST)) {
        case DL_ADC12_IIDX_MEM0_RESULT_LOADED:
            //toggle gpio for debug
            DL_GPIO_togglePins(GPIO_GRP_3_PORT, GPIO_GRP_3_GIO_DB_PIN);

            gADC1[gCtr1] = DL_ADC12_getMemResult(ADC12_1_INST, DL_ADC12_MEM_IDX_0);
            gCtr1 += 1;
            if (gCtr1 > 2*E1-1) {
                //DL_ADC12_disableConversions(ADC12_1_INST);
                DL_ADC12_clearInterruptStatus(ADC12_1_INST,(DL_ADC12_INTERRUPT_MEM0_RESULT_LOADED));
                DL_TimerA_stopCounter(TIMER_0_INST);
            //    __BKPT();
            }
            break;
        default:
            break;
    }
}

