/*processing samples*/

#include <proc/p32mm0064gpl028.h>
#include <sys/attribs.h>
#include "processing.h"
#include "hardware.h"

extern struct results_struct results;
extern int new_samp_ena;
extern int iv_sel;
extern int g_wave_step;
extern uint32_t wav_len;
extern uint32_t wav_periods;

void __ISR (_ADC_VECTOR, IPL4SOFT) ADCHandler (void)
{
    static int32_t i_wave = 0;
    int32_t adc_tmp;

    if((new_samp_ena == 1) && i_wave % (wav_len * wav_periods) == 0)        //after saving last waveform start new signal storing (synchronously)
    {
        i_wave = 0;                                                         //new sampling can start
        new_samp_ena = 0;                                                   //next sampling won't be enabled until this that just starts is saved in data processing function
        iv_sel ^= 0x1;                  //switch to voltage or current (alternatively)
    }
        
    adc_tmp = ADC1BUF0;
    adc_tmp |= (adc_tmp & 0x00008000) ? 0xFFFF0000 : 0x00000000;    //expand to 32bit signed format (see rev b0 errata)

    if(i_wave < wav_len * wav_periods)
        results.wave[(i_wave * g_wave_step) % wav_len] += adc_tmp;     //save sample to buffer
    
    if(i_wave == wav_len * wav_periods - 1)
        results.new_data = 1;                                       //when buffer is filled set the new_data flag
    
    ++i_wave;
    
    IFS0bits.AD1IF = 0x0;                                           //clear interrupt flag from adc
}




void data_acq(struct config_struct config, struct cal_struct *cal)
{
    int32_t i_copy = 0;
    int32_t i_dft = 0;
    static struct comp_struct comp;
    double tmp_denom1;
    double tmp_denom2;
    double tmp_volt_real;
    double tmp_volt_imag;
    double tmp_curr_real;
    double tmp_curr_imag;
static       int asdf=0;

    //-------------------------------
    if(iv_sel)
        for(i_copy = 0; i_copy < wav_len; ++i_copy)
            results.voltage_saved[i_copy] = (double)results.wave[i_copy];         //save complete, averaged voltage waveform for processing, accumulated value left without normalization to reduce execution time
    else
        for(i_copy = 0; i_copy < wav_len; ++i_copy)
            results.current_saved[i_copy] = (double)results.wave[i_copy];         //save complete, averaged current waveform for processing, accumulated value left without normalization to reduce execution time

    for(i_copy = 0; i_copy < wav_len; ++i_copy)
        results.wave[i_copy] = 0;                           //clean the array
    
    results.new_data = 0;                                   //processing of results is started
    new_samp_ena = 0x1;                                     //enable new sampling - this ends critical section needed for copying data (started in ISR)
    //-------------------------------

    /*DFT algorithm*/
    results.volt_real_raw = 0;
    results.volt_imag_raw = 0;
    results.curr_real_raw = 0;
    results.curr_imag_raw = 0;
    for(i_dft = 0; i_dft < wav_len; ++i_dft)
    {
	results.volt_real_raw = results.volt_real_raw + results.voltage_saved[i_dft] * cos_l(-2 * PI * 1 * i_dft / wav_len);
	results.volt_imag_raw = results.volt_imag_raw + results.voltage_saved[i_dft] * sin_l(-2 * PI * 1 * i_dft / wav_len);
	results.curr_real_raw = results.curr_real_raw + results.current_saved[i_dft] * cos_l(-2 * PI * 1 * i_dft / wav_len);
	results.curr_imag_raw = results.curr_imag_raw + results.current_saved[i_dft] * sin_l(-2 * PI * 1 * i_dft / wav_len);
    }
    //Voltage and current Re and Im are calculated from raw averaged data from ADC. Thing to do are:
    //1. scale by gain values introduced by selectable gain amplifiers
    //2. scale by calibration data
    //Doing this AFTER DFT saves execution time
    //No need to normalize by dividing by number of averaged periods and by number of samples in DFT here
    //All the currents and voltages are in bits, no need to scale to volts and ampers
    tmp_volt_real = results.volt_real_raw / (V_GAIN(config.v_gain) / V_CAL(config.v_gain));
    tmp_volt_imag = results.volt_imag_raw / (V_GAIN(config.v_gain) / V_CAL(config.v_gain));
    tmp_curr_real = results.curr_real_raw / (I_GAIN(config.i_gain) / I_CAL(config.i_gain)) / (V_GAIN(config.v_i_gain) / V_CAL(config.v_i_gain));
    tmp_curr_imag = results.curr_imag_raw / (I_GAIN(config.i_gain) / I_CAL(config.i_gain)) / (V_GAIN(config.v_i_gain) / V_CAL(config.v_i_gain));
    //rotate by phase calibration coefficient to get final results
    results.volt_real = cos(V_PH_CAL(config.v_gain) * FREQ_PH_CORR(config.fq_sel)) * tmp_volt_real - sin(V_PH_CAL(config.v_gain) * FREQ_PH_CORR(config.fq_sel)) * tmp_volt_imag;
    results.volt_imag = sin(V_PH_CAL(config.v_gain) * FREQ_PH_CORR(config.fq_sel)) * tmp_volt_real + cos(V_PH_CAL(config.v_gain) * FREQ_PH_CORR(config.fq_sel)) * tmp_volt_imag;
    results.curr_real = cos((I_PH_CAL(config.i_gain) + V_PH_CAL(config.v_i_gain)) * FREQ_PH_CORR(config.fq_sel)) * tmp_curr_real - sin((I_PH_CAL(config.i_gain) + V_PH_CAL(config.v_i_gain)) * FREQ_PH_CORR(config.fq_sel)) * tmp_curr_imag;
    results.curr_imag = sin((I_PH_CAL(config.i_gain) + V_PH_CAL(config.v_i_gain)) * FREQ_PH_CORR(config.fq_sel)) * tmp_curr_real + cos((I_PH_CAL(config.i_gain) + V_PH_CAL(config.v_i_gain)) * FREQ_PH_CORR(config.fq_sel)) * tmp_curr_imag;
    
    /*impedance calculation - raw result, without compensation*/
    results.z_raw_real = (results.volt_real * results.curr_real + results.volt_imag * results.curr_imag) / (results.curr_real * results.curr_real + results.curr_imag * results.curr_imag);
    results.z_raw_imag = (results.volt_imag * results.curr_real - results.volt_real * results.curr_imag) / (results.curr_real * results.curr_real + results.curr_imag * results.curr_imag);
    results.y_raw_real = (results.volt_real * results.curr_real + results.volt_imag * results.curr_imag) / (results.volt_real * results.volt_real + results.volt_imag * results.volt_imag);
    results.y_raw_imag = (results.volt_real * results.curr_imag - results.volt_imag * results.curr_real) / (results.volt_real * results.volt_real + results.volt_imag * results.volt_imag);
    
    /*compensation - save stray admittance and residulal impedance*/
    if(config.comp_open)
    {
        comp.y_stray_real = results.y_raw_real;
        comp.y_stray_imag = results.y_raw_imag;
        config.comp_open = 0;
    }
    if(config.comp_short)
    {
        comp.z_resid_real = results.z_raw_real;
        comp.z_resid_imag = results.z_raw_imag;
        config.comp_short = 0;
    }
    
    /*calculate results, considering compensation*/

    tmp_denom1 = (1 - results.z_raw_real * comp.y_stray_real - comp.z_resid_real * comp.y_stray_real + results.z_raw_imag * comp.y_stray_imag - comp.z_resid_real * comp.y_stray_imag) * (1 - results.z_raw_real * comp.y_stray_real - comp.z_resid_real * comp.y_stray_real + results.z_raw_imag * comp.y_stray_imag - comp.z_resid_real * comp.y_stray_imag) + (-results.z_raw_imag * comp.y_stray_real + comp.z_resid_imag * comp.y_stray_real - results.z_raw_real * comp.y_stray_imag + comp.z_resid_real * comp.y_stray_imag) * (-results.z_raw_imag * comp.y_stray_real + comp.z_resid_imag * comp.y_stray_real - results.z_raw_real * comp.y_stray_imag + comp.z_resid_real * comp.y_stray_imag);
    results.z_real = ((results.z_raw_real - comp.z_resid_real) * (1 - results.z_raw_real * comp.y_stray_real - comp.z_resid_real * comp.y_stray_real + results.z_raw_imag * comp.y_stray_imag - comp.z_resid_imag * comp.y_stray_imag) + (results.z_raw_imag - comp.z_resid_imag) * (-results.z_raw_imag * comp.y_stray_real + comp.z_resid_real * comp.y_stray_real - results.z_raw_real * comp.y_stray_imag + comp.z_resid_real * comp.y_stray_imag)) / tmp_denom1;
    results.z_imag = ((results.z_raw_imag - comp.z_resid_imag) * (1 - results.z_raw_real * comp.y_stray_real - comp.z_resid_real * comp.y_stray_real + results.z_raw_imag * comp.y_stray_imag - comp.z_resid_imag * comp.y_stray_imag) - (results.z_raw_real - comp.z_resid_real) * (-results.z_raw_imag * comp.y_stray_real + comp.z_resid_real * comp.y_stray_real - results.z_raw_real * comp.y_stray_imag + comp.z_resid_real * comp.y_stray_imag)) / tmp_denom1;

    tmp_denom2 = (results.z_raw_real - comp.z_resid_real) * (results.z_raw_real - comp.z_resid_real) + (results.z_raw_imag - comp.z_resid_imag) * (results.z_raw_imag - comp.z_resid_imag);
    results.y_real = ((1 - results.z_raw_real * comp.y_stray_real - comp.z_resid_real * comp.y_stray_real + results.z_raw_imag * comp.y_stray_imag - comp.z_resid_imag * comp.y_stray_imag) * (results.z_raw_real - comp.z_resid_real) + (-results.z_raw_imag * comp.y_stray_real + comp.z_resid_imag * comp.y_stray_real - results.z_raw_real * comp.y_stray_imag + comp.z_resid_real * comp.y_stray_imag) * (results.z_raw_imag - comp.z_resid_imag)) / tmp_denom2;
    results.y_imag = ((-results.z_raw_imag * comp.y_stray_real + comp.z_resid_imag * comp.y_stray_real - results.z_raw_real * comp.y_stray_imag + comp.z_resid_real * comp.y_stray_imag) * (results.z_raw_real - comp.z_resid_real) - (1 - results.z_raw_real * comp.y_stray_real - comp.z_resid_real * comp.y_stray_real + results.z_raw_imag * comp.y_stray_imag - comp.z_resid_imag * comp.y_stray_imag) * (results.z_raw_imag - comp.z_resid_imag)) / tmp_denom2;
    
    results.z_mod = sqrt(results.z_real * results.z_real + results.z_imag * results.z_imag);
    results.z_arg = atan2(results.z_imag, results.z_real);
    results.y_mod = 1 / results.z_mod;
    results.y_arg = -results.z_arg;
   
    ++asdf;
    if(asdf >= 29){
        asdf = 0;
    }
}

double sin_l(double sin_arg)
{
    int x;
    double pi_to_2pi;
    
    x = (int)(sin_arg / (PI / 2) * SIN_LUT_LEN);
    
    while(x >= 4 * SIN_LUT_LEN)
        x = x - 4 * SIN_LUT_LEN;    //subtract full period (SIN_LUT_LEN is only 1/4 of period)
    while(x < 0)
        x = x + 4 * SIN_LUT_LEN;
    
    if(x > 2 * SIN_LUT_LEN)
    {
        x = x - 2 * SIN_LUT_LEN;
        pi_to_2pi = -1;
    }
    else
    {
        pi_to_2pi = 1;
    }
    
    if(x <= 1 * SIN_LUT_LEN)
    {
        if(     x < 1 * SIN_LUT_LEN / SIN_LUT_PARTS)    //0 to 499
            return sin_lut1[x - 0 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 2 * SIN_LUT_LEN / SIN_LUT_PARTS)    //500 to 999
            return sin_lut2[x - 1 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 3 * SIN_LUT_LEN / SIN_LUT_PARTS)    //1000 to 1499
            return sin_lut3[x - 2 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 4 * SIN_LUT_LEN / SIN_LUT_PARTS)    //1500 to 1999
            return sin_lut4[x - 3 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 5 * SIN_LUT_LEN / SIN_LUT_PARTS)    //2000 to 2499
            return sin_lut5[x - 4 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 6 * SIN_LUT_LEN / SIN_LUT_PARTS)    //2500 to 2999
            return sin_lut6[x - 5 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 7 * SIN_LUT_LEN / SIN_LUT_PARTS)    //3000 to 3499
            return sin_lut7[x - 6 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(x < 8 * SIN_LUT_LEN / SIN_LUT_PARTS)    //3500 to 3999
            return sin_lut8[x - 7 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else                                            //4000 is exact pi/2 - return closest value (from x=3999)
            return sin_lut8[(SIN_LUT_LEN - 1) - 7 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
    }
    else if(x <= 2 * SIN_LUT_LEN)
    {
        if(     2 * SIN_LUT_LEN - x < 1 * SIN_LUT_LEN / SIN_LUT_PARTS)    //0 to 499
            return sin_lut1[(2 * SIN_LUT_LEN - x) - 0 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;   //for sin_arg==pi x==8000 so sin_lut1(8000-8000)==0
        else if(2 * SIN_LUT_LEN - x < 2 * SIN_LUT_LEN / SIN_LUT_PARTS)    //500 to 999
            return sin_lut2[(2 * SIN_LUT_LEN - x) - 1 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(2 * SIN_LUT_LEN - x < 3 * SIN_LUT_LEN / SIN_LUT_PARTS)    //1000 to 1499
            return sin_lut3[(2 * SIN_LUT_LEN - x) - 2 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(2 * SIN_LUT_LEN - x < 4 * SIN_LUT_LEN / SIN_LUT_PARTS)    //1500 to 1999
            return sin_lut4[(2 * SIN_LUT_LEN - x) - 3 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(2 * SIN_LUT_LEN - x < 5 * SIN_LUT_LEN / SIN_LUT_PARTS)    //2000 to 2499
            return sin_lut5[(2 * SIN_LUT_LEN - x) - 4 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(2 * SIN_LUT_LEN - x < 6 * SIN_LUT_LEN / SIN_LUT_PARTS)    //2500 to 2999
            return sin_lut6[(2 * SIN_LUT_LEN - x) - 5 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(2 * SIN_LUT_LEN - x < 7 * SIN_LUT_LEN / SIN_LUT_PARTS)    //3000 to 3499
            return sin_lut7[(2 * SIN_LUT_LEN - x) - 6 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;
        else if(2 * SIN_LUT_LEN - x < 8 * SIN_LUT_LEN / SIN_LUT_PARTS)    //3500 to 3999
            return sin_lut8[(2 * SIN_LUT_LEN - x) - 7 * SIN_LUT_LEN / SIN_LUT_PARTS] * pi_to_2pi;   //x=4001
    }
}

double cos_l(double cos_arg)
{
    return sin_l(cos_arg + PI / 2.0);
}
