#include "config.h"
#include "stdio.h"
#include "xc.h"
#include "extern.h"
#include "conv.h"
#include "CalibrationConstants.h"
#include <libq.h>
#include <limits.h>
#include "PowerTarget.h"

/* This function determines the power target based upon speed, there is a base curve in the config.h file that
 is then multiplied by the "Scale_Factor", and "Retract_Factor" to determine a target power.  This resulting
 target power is then compared a minimum that depends upon speed for the purposes of not underloading the generator.
 * This second is important to ensure the pressure relief valve opens before the generator spins too fast. */

//#define LO_TABLE_LENGTH 7
//#define N_SPEC        {0, 450, 600, 1000, 1700, 4400, 6790}  //  %11/14/15 9:30AM
//#define POWER_SPEC    {0,   0,  50,  300,  1000, 4500, 11800} //Power in Watts versus speed curve

//#define LO_TABLE_LENGTH 7
//#define N_SPEC        {0, 200, 600, 1000, 1700, 4400, 6790}  //  %11/14/15 9:30AM
//#define POWER_SPEC    {0,   0,  50,  300,  1000, 4500, 11800} //Power in Watts versus speed curve

#define LO_TABLE_LENGTH 4
#define N_SPEC        {0, 25, 50, 8000}  //  %11/14/15 9:30AM
#define POWER_SPEC    {0, 0, 50, 8000} //Power in Watts versus speed curve

#define MINPOWER_LO_TABLE_LENGTH 4
#define N_MINPOWER_RPM {0,5000,6000,7000} //  Two Points defining minimum power line at high speeds
#define N_MINPOWER  {0,0,10000,14000} // Care must be taken to keep this from saturating the fractional representation of power
#define MAX_POWER 8000  //This is the maximum power that the system will deliver unless N_MINPOWER specifies something higher at high RPM.

fractional PowerTarget(fractional f_N) /* 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 float N_Spec[LO_TABLE_LENGTH] = N_SPEC; /*RPM in integers, not scaled. */
    const float Power_Spec[LO_TABLE_LENGTH] = POWER_SPEC;
    static fractional N_cnts[LO_TABLE_LENGTH];
    static fractional Power_cnts[LO_TABLE_LENGTH];
    static long Slope[LO_TABLE_LENGTH];

    const float N_Spec_MinPower[MINPOWER_LO_TABLE_LENGTH] = N_MINPOWER_RPM; //  Two Points defining minimum power line at high speeds
    const float MinPower_Spec[MINPOWER_LO_TABLE_LENGTH] = N_MINPOWER; //
    static fractional N_MinPower_cnts[MINPOWER_LO_TABLE_LENGTH];
    static fractional MinPower_cnts[MINPOWER_LO_TABLE_LENGTH];
    static long MinPower_Slope[MINPOWER_LO_TABLE_LENGTH];

    static int f_MaxPower;
    static int first_time = 1;
    int i;
    fractional N, P;


    if (first_time) //Convert specified RPM and Watts to fractional counts.
    {
        for (i = 0; i < LO_TABLE_LENGTH; i++) {
            N_cnts[i] = ConvertEngUnitsToFract(N_Spec[i], RPMs_cnt, RPMs_eng);
            Power_cnts[i] = ConvertEngUnitsToFract(Power_Spec[i], Power_cnt, Power_eng);
            if (i > 0)
                Slope[i - 1] = ((long) (Power_cnts[i] - Power_cnts[i - 1]) << 15) / (N_cnts[i] - N_cnts[i - 1]); //This is promoted to a long and the slope is calcuated as power_cnts per 1/32767 rpm counts.        
        }

        for (i = 0; i < MINPOWER_LO_TABLE_LENGTH; i++) {
            N_MinPower_cnts[i] = ConvertEngUnitsToFract(N_Spec_MinPower[i], RPMs_cnt, RPMs_eng);
            MinPower_cnts[i] = ConvertEngUnitsToFract(MinPower_Spec[i], Power_cnt, Power_eng);
            if (i > 0)
                MinPower_Slope[i - 1] = ((long) (MinPower_cnts[i] - MinPower_cnts[i - 1]) << 15) / (N_MinPower_cnts[i] - N_MinPower_cnts[i - 1]); //This is promoted to a long and the slope is calcuated as power_cnts per 1/32767 rpm counts.
        }

        f_MaxPower = ConvertEngUnitsToFract(MAX_POWER, Power_cnt, Power_eng);

        first_time = 0;
    }

    N = _Q15abs(f_N);

    if (N >= N_cnts[LO_TABLE_LENGTH - 1])
        N = N_cnts[LO_TABLE_LENGTH - 1] - 1;  //Set result to max in table if outside range

    for (i = 0; i < LO_TABLE_LENGTH - 1; i++) {
        if ((N >= N_cnts[i]) && (N < N_cnts[i + 1]))
        { //Solution is in this segment
            P = Power_cnts[i] + (fractional) ((Slope[i]*(N - N_cnts[i])) >> 15);
            break;
        }
    }

//Adjust power target based on scale and retract factors.

    // Apply scale and retract factors.
    P = (fractional) (((long) P * scale_factor.Value) / scale_factor.cntsPerCmdRepEngUnits); //Scale factor is scaled such that 40-120cnts = 0.4->1.2
    if (f_N > 0) //Piston is Retracting, apply retract factor
    {
        P = (fractional) (((long) P * retract_factor.Value) / retract_factor.cntsPerCmdRepEngUnits); //retract factor is scaled such that 40-100cnts = 0.4->1.2
     }


#if 0
   //Enforce Maximum
    if(P > f_MaxPower)
        P = f_MaxPower;


    //Enforce minimums, this ensures that the pressure relief valve opens before too high a shaft speed is reached.
     N = _Q15abs(f_N); //Reset local variable N.
     if (N >= N_MinPower_cnts[MINPOWER_LO_TABLE_LENGTH - 1])
     N = N_MinPower_cnts[MINPOWER_LO_TABLE_LENGTH - 1] - 1;  //Set result to max in table if outside range
     for (i = 0; i < MINPOWER_LO_TABLE_LENGTH - 1; i++)
     {
         if ((N >= N_MinPower_cnts[i]) && (N < N_MinPower_cnts[i + 1])) { //Solution is in this segment
             Pmin = MinPower_cnts[i] + (fractional) ((MinPower_Slope[i]*(N - N_MinPower_cnts[i])) >> 15);
             break;
         }
     }
     if (P < Pmin)
        P = Pmin;
#endif
   
    return P;
}

#ifdef DEBUG

#include "UART.h"

void TestPowerTarget() {

    fractional P,f_N;
    char s[256];
    fractional locTargetCurrent;
    sprintf(s,"PowerCnts = (%d %d)     PowerEng = (%f  %f)  \r\n\n\n",Power_cnt[0],Power_cnt[1],Power_eng[0],Power_eng[1]);
    putsU1(s);
    sprintf(s,"TorqueCnts = (%d %d)     TorqueEng = (%f  %f)  \r\n\n\n",Torque_cnt[0],Torque_cnt[1],Torque_eng[0],Torque_eng[1]);
    putsU1(s);

    for(f_N = -32768;f_N <  32767-64;f_N+=64)
    {
    P = PowerTarget(f_N);
    if((P > 0) && (_Q15abs(f_N) > 0))
    {
        locTargetCurrent = _Q15neg((fractional) ((((long) P) << 15)/f_N));

    }
    else
        locTargetCurrent = 0;

    sprintf(s,"%d  %f        %d   %f     %d   %f \r\n",f_N,ConvertFractToEngUnits(f_N, RPMs_cnt, RPMs_eng),
                                        P,ConvertFractToEngUnits(P,Power_cnt, Power_eng),
                                        locTargetCurrent,ConvertFractToEngUnits(locTargetCurrent,IWind_cnt,IWind_eng));
    putsU1(s);
    }
}
#endif