#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 "WindingCurrentTarget.h"

/* This function determines the winding current target based upon speed, there is a base curve that
 is then multiplied by the "Scale_Factor", and "Retract_Factor" to determine a target current.  
 * This routine just computes the target for generator mode, further modifications and limits are done elsewhere for 
 * special purposes such as ensuring the pressure relieve valve always opens
 */

#define LO_TABLE_LENGTH 7
#define N_SPEC        {0, 300, 600, 1000, 1700, 4400, 6790}  //
#define TORQUE_SPEC    {0,   0,  .8,  2.9,  5.6, 9.8, 16.6} //Target Torque in N-m versus speed curve.  Matches old boost converter targets that have been deployed..

//#define LO_TABLE_LENGTH 4
//#define N_SPEC        {0, 100, 200, 8000}  //  %11/14/15 9:30AM
//#define TORQUE_SPEC    {0, 0, 15.4, 15.4} //Target Torque in N-m versus speed curve.  For brings torques up at much lower speeds.

fractional WindingCurrentTarget(fractional f_N)   /* Returns target winding current in counts  as a function of RPM and scale factor*/
{
    const float N_Spec[LO_TABLE_LENGTH] = N_SPEC;   /*RPM in integers, not scaled. */
    const float Torque_Spec[LO_TABLE_LENGTH] = TORQUE_SPEC;  /*Torque in N-m, not scaled. */

    static fractional N_cnts[LO_TABLE_LENGTH];
    static fractional WindingCurrent_cnts[LO_TABLE_LENGTH];
    
    static long Slope[LO_TABLE_LENGTH];

    static int first_time = 1;
    int i;
    fractional N, I;


    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);
            WindingCurrent_cnts[i] = ConvertEngUnitsToFract(Torque_Spec[i],Torque_cnt, Torque_eng);  //This is a cheater way to convert right to current winding counts, works because Torque and winding counts are the same, see computation in CalibrationConstants.c
            if (i > 0)
                Slope[i - 1] = ((long) (WindingCurrent_cnts[i] - WindingCurrent_cnts[i - 1]) << 15) / (N_cnts[i] - N_cnts[i - 1]); //This is promoted to a long and the slope is calculated as winding current counts per 1/32767 rpm counts.        
        }

        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
            I = WindingCurrent_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.
    I = (fractional) (((long) I * scale_factor.Value) / CRSF_PERCENTAGES); //Scale factor is scaled such that 40-120cnts = 0.4->1.2
    if (f_N > 0) //Piston is Retracting, apply retract factor and change sign of result
    {
        I = (fractional) (((long) _Q15neg(I) * retract_factor.Value) / CRSF_PERCENTAGES); //retract factor is scaled such that 40-100cnts = 0.4->1.2
     }
  
    return I;  //Returns desired winding current in counts
}

#ifdef DEBUG

#include "UART.h"

void TestWindingCurrentTarget() {

    fractional f_N;
    char s[256];
    fractional locTargetCurrent;

    for(f_N = -32768;f_N <  32767-64;f_N+=64)
    {
    locTargetCurrent = WindingCurrentTarget(f_N);

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