#include "config.h"

#include "xc.h"
#include "extern.h"
#include "Control.h"
#include "conv.h"
#include <libq.h>

#define LO_TABLE_LENGTH 6

fractional PowerTarget(void) /* Returns target power in power counts (not Watts) as a function of RPM and scale factor*/ { /* Handling of negative values of RPMs and over/underflow needs to be handled*/
    const long N_LO[LO_TABLE_LENGTH] = {0, 300, 670, 1330, 4500, 6790};
    const long Pcnts_LO[LO_TABLE_LENGTH] = {0, 140, 621, 2455, 16883, 32767}; /*Power versus speed curve in Power counts (not Watts)  32767cnts = 11880W*/
    int N;
    int i;
    fractional P;

    N = abs((int) (0.5 * f_RPMs + 0.5)); /*RPMs is a float*/

    for (i = 1; i < LO_TABLE_LENGTH; i++) {
        if ((N >= N_LO[i - 1]) && (N < N_LO[i])) {
            P = (fractional) Pcnts_LO[i - 1]+((N - N_LO[i - 1])*(Pcnts_LO[i] - Pcnts_LO[i - 1])) / (N_LO[i] - N_LO[i - 1]);
            break;
        }
    }

    return P >> 2;
}

#ifdef BATT_CHARGER_MODE  //Only compile this code if it's called for in the config.h file

//This routine is called by the DMA isr to update the PWM duty cycles based upon the voltage and current targets, and the recently measured voltage and currents

void BattChargerMode_Control(fractional VBus, fractional IBridge, fractional ILoad) {
    fractional Vtarg;
    fractional LoadDumpErrorSignal; //Error signal that results from the voltage command being thresholded.
    fractional BridgeDC,LoadDC;
    fractional Pcmd, Pcmd_last;
    fractional Icmd, Icmd_last;
    float DC_fp;


#ifdef BRIDGE_CURRENT_MEASUREMENT_IS_AFTER_LOAD  // If this is defined, the load resistor is connected before the current measurement.
    // In this case the load current is estimated from the voltage, duty cycle, and resistance and the Bridge current measurement is adjusted accordingly
    DC_fp = ((float) P1DC4) / 4096;
    ILoad = ConvertEngUnitsToFract(ConvertFractToEngUnits(VBus, VBus_cnt, VBus_eng) * DC_fp / R_LOAD, IBridge_cnt, IBridge_eng); //This should be done in fixed point,
    IBridge = _Q15add(IBridge, ILoad);
#endif    // Otherwise theBridge current and Load current are measured seperately as "IBridge" and "ILoad", and passed in as such so nothig is required.


    Pmeas = (fractional) (((long) VBus * (long) IBridge) >> 15);
    //Ptarg = PowerTarget();
    Ptarg = ConvertEngUnitsToFract(0.5 * _Q15abs(f_RPMs)*SpecificPower, Power_cnt, Power_eng); //Done in floating point here for convience, should be changed to fixed point...


    Pcmd_last = Power_PID.controlOutput; //Save last output for use in saturation case
    Power_PID.controlReference = Ptarg;
    Power_PID.measuredOutput = Pmeas;

    PID(&Power_PID); //Call the PID controller, result will be in Power_PID.controlOutput as a Q15
    //It would appear that interrupts that occur during the calling of the PID assembly routine tend
    //to cause an address error in PID.s after it gets back.  I suspect this is because the CORCON register
    //is getting changed, but I'm not sure.  The current solution is to not let interrupts occur while PID is running,
    //either by disabling them, or having no other interrupts present at a higher priority.
    Pcmd = Power_PID.controlOutput; //VBus_PID.controlOutput will always be between -32768 and +32767

    Vtarg = Pcmd; //The power PID outputs a command expressed in voltage, since the base assumption is that raising the voltage raises the power drawn.
    //Apply thresholds and compute
    if (Vtarg > f_MaxTargetVoltage) //e.g. > 330V
    {
        Vtarg = f_MaxTargetVoltage;
        //Power_PID.controlOutput = f_MaxTargetVoltage;
    } else if (Vtarg < f_MinTargetVoltage) //e.g. < 250V
    {
        Vtarg = f_MinTargetVoltage;
        Power_PID.controlOutput = f_MinTargetVoltage;
    }

    f_TargetVoltage = Vtarg;  //For reporting

    BridgeDC = VoltageControl(VBus, Vtarg); //This function closes the voltage loop based upon measurement and voltage target.

    P1DC1 = (int) (BridgeDC >> 3); //These duty cycles are such that 0->100% is 0->2*P1TPER.
    P1DC2 = (int) (BridgeDC >> 3); //These duty cycles are such that 0->100% is 0->2*P1TPER.
    P1DC3 = (int) (BridgeDC >> 3); //These duty cycles are such that 0->100% is 0->2*P1TPER.


        LoadDumpErrorSignal = _Q15sub(Pcmd,Vtarg);
        iTemp1 = LoadDumpErrorSignal;

        if(LoadDumpErrorSignal > 0)
        {
            LoadDC = LoadDumpErrorSignal;
        }
        else
        LoadDC = MinLOAD_DC;

    LoadDC = LoadDC >> 3;


    P1DC4 = (int) (LoadDC); //These duty cycles are such that 0->100% is 0->2*P1TPER.
}

#endif