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


/* This function determines the absolute minimum and maximum allowable winding current targets
 * For the most part this is based on "MAX_WINDCURRENTLIMIT" defined in config.h in Amps and 
 * converted to cnts here.  At high speeds, the maximum allowable motor current is gradually
 * reduced to match the minimum allowable at a maximum speed (i.e. 6000RPM).  This has two
 * effects, it limits how fast the system can run in motor mode by limiting the current, and
 * simultaneously ensures that in generator mode the pressures raise to open the pressure relief valve.
 * Currently this is set up to be symmetric about RPM==0, but this may need to be changed to limit
 * the oil pressures in the retract direction since there is no pressure relief valve in that direction.
 * Inputs:  Requested Winding Current and RPM
 * Output:  Limited Winding Current if Request is outside allowable envelope.
 *  */

#define LO_TABLE_LENGTH 4
#define N_SPEC        {0, 500, 5000, 6000}
#define MAX_CURRENT   { MAX_WINDCURRENTLIMIT,  MAX_WINDCURRENTLIMIT,  MAX_WINDCURRENTLIMIT, -MAX_WINDCURRENTLIMIT} //Maximum Current in Amps versus positive RPM
#define MIN_CURRENT   {-MAX_WINDCURRENTLIMIT, -MAX_WINDCURRENTLIMIT, -MAX_WINDCURRENTLIMIT, -MAX_WINDCURRENTLIMIT} //Minimum Current in Amps versus positive RPM

fractional CurrentMinMax(fractional f_N, fractional locTargetCurrent) /* Returns the target current modified to respect min/max limitations. */ {
    const float N_Spec[LO_TABLE_LENGTH] = N_SPEC; /*RPM in integers, not scaled. */
    const float MinCurrent_Spec[LO_TABLE_LENGTH] = MIN_CURRENT;
    const float MaxCurrent_Spec[LO_TABLE_LENGTH] = MAX_CURRENT;

    static fractional N_cnts[LO_TABLE_LENGTH];
    static fractional MinCurrent_cnts[LO_TABLE_LENGTH];
    static fractional MaxCurrent_cnts[LO_TABLE_LENGTH];

    static long MinCurrentSlope[LO_TABLE_LENGTH];
    static long MaxCurrentSlope[LO_TABLE_LENGTH];
 
    fractional LimitedTargetCurrent;

    static int first_time = 1;
    int i;
    fractional N;
    fractional Imax, Imin, Itmp;


    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);
            MinCurrent_cnts[i] = ConvertEngUnitsToFract(MinCurrent_Spec[i], IWind_cnt, IWind_eng);
            MaxCurrent_cnts[i] = ConvertEngUnitsToFract(MaxCurrent_Spec[i], IWind_cnt, IWind_eng);
        }
        for (i = 0; i < LO_TABLE_LENGTH-1; i++) {
            MinCurrentSlope[i] = ((long) ((long) MinCurrent_cnts[i + 1] - MinCurrent_cnts[i]) << 15) / (N_cnts[i + 1] - N_cnts[i]); //This is promoted to a long and the slope is calcuated as IWind_cnts per 1/32767 rpm counts.
            MaxCurrentSlope[i] = ((long) ((long) MaxCurrent_cnts[i + 1] - MaxCurrent_cnts[i]) << 15) / (N_cnts[i + 1] - N_cnts[i]); //This is promoted to a long and the slope is calcuated as IWind_cnts per 1/32767 rpm counts.
        }
        MinCurrentSlope[LO_TABLE_LENGTH-1] = 0L;
        MaxCurrentSlope[LO_TABLE_LENGTH-1] = 0L;

        first_time = 0;
    }

    N = _Q15abs(f_N);

    if (N >= N_cnts[LO_TABLE_LENGTH - 1])
        {
        Imin = MinCurrent_cnts[LO_TABLE_LENGTH - 1]; //Set result to last entry in table if outside RPM range
        Imax = MaxCurrent_cnts[LO_TABLE_LENGTH - 1]; //Set result to last entry in table if outside RPM range
        }
    else
        for (i = 0; i < LO_TABLE_LENGTH - 1; i++) //0,1,2
        {
            if ((N >= N_cnts[i]) && (N < N_cnts[i + 1])) //Solution is in this segment
            {
                Imin = MinCurrent_cnts[i] + (fractional) ((MinCurrentSlope[i]*(N - N_cnts[i])) >> 15);
                Imax = MaxCurrent_cnts[i] + (fractional) ((MaxCurrentSlope[i]*(N - N_cnts[i])) >> 15); //TODO  Need to consider saturation here.
                break;
            }
        }

    if (f_N < 0) //Imin and Imax are defined for clockwise rotation, so invert if counterclockwise rotation
    {
        Itmp = Imax;
        Imax = _Q15neg(Imin);
        Imin = _Q15neg(Itmp);
    }

    LimitedTargetCurrent = locTargetCurrent;
    if (locTargetCurrent >= Imax)
        LimitedTargetCurrent = Imax;
    if (locTargetCurrent <= Imin)
        LimitedTargetCurrent = Imin;

    return LimitedTargetCurrent;
}

#ifdef DEBUG

void TestCurrentMinMax() {

    fractional Ibig, Ismall;
    fractional f_N = 0;
    char s[256];

    Ibig = ConvertEngUnitsToFract(MAX_WINDCURRENTLIMIT, IWind_cnt, IWind_eng);  //It's a mystery why this computes differently here than in CurrentMinMax
    Ismall = ConvertEngUnitsToFract(-MAX_WINDCURRENTLIMIT, IWind_cnt, IWind_eng);
    sprintf(s,"Ibig/Ismall = %d  %d \r\n",Ibig,Ismall);
    putsU1(s);
    Ibig = 32767;
    Ismall = -32768;

        for (f_N = -32768; f_N <= 32767 - 64; f_N += 64) {
    
            sprintf(s, "%d        %d   %d        %d    %d    %d   \r\n",
                    f_N,Ibig,Ismall,
                    CurrentMinMax(f_N, Ibig),
                    CurrentMinMax(f_N, 0),
                    CurrentMinMax(f_N, Ismall)
                    );
            putsU1(s);
        }
    
//    for (f_N = 0; f_N <= 32767 - 64; f_N += 64) {
//
//        sprintf(s, "%f      %f    %f    %f   \r\n",
//                ConvertFractToEngUnits(f_N, RPMs_cnt, RPMs_eng),
//                ConvertFractToEngUnits(CurrentMinMax(f_N, Ibig), IWind_cnt, IWind_eng),
//                ConvertFractToEngUnits(CurrentMinMax(f_N, 0), IWind_cnt, IWind_eng),
//                ConvertFractToEngUnits(CurrentMinMax(f_N, Ismall), IWind_cnt, IWind_eng)
//                );
//        putsU1(s);
//    }

}
#endif