#include <dsp.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <libq.h>
#include <limits.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "types.h"
#include "conv.h"
#include "isr.h"
#include "CalibrationConstants.h"
#include "GainScheduling.h"
#include "libq.h"
#include "RPM_Calculation.h"
#include "stdio.h"


fractional CurrentRateLimit(fractional current_cmd) {
    static fractional local_TargetCurrent = 0; //Return Value  Static so it can be used to estimate size of change requested.
    static fractional CurrentRateLimit_ctsperinterpt;
    fractional TargetCurrentDelta;
    static int first_time = 1;

    if (first_time) {
        if (CURRENT_CMD_RATELIMIT == 0)
            CurrentRateLimit_ctsperinterpt = 0; //Indicates no rate limit.
        else {
            CurrentRateLimit_ctsperinterpt = ConvertEngUnitsToFract(((float) CURRENT_CMD_RATELIMIT) / ISR_UPDATE_RATE, Cal.IWind_cnt, Cal.IWind_eng); //Amps/interrupt in cnts.
            if (CurrentRateLimit_ctsperinterpt == 0)
                CurrentRateLimit_ctsperinterpt = 1; //Ensure non-zero rate if CURRENT_CMD_RATELIMIT != 0, sets to slowest possible ramp
        }
        first_time = 0;
    } 
    
        //Rate limit how fast the target current can change. (CurrentRateLimit_ctsperinterpt == 0 indicates no limit specified).
        TargetCurrentDelta = _Q15sub(current_cmd, local_TargetCurrent); //Determine size of requested change in current.  TODO:  Issue here in that TargetQuadratureCUrrent currently can be adjusted to limit current out of the battery, so that will confuse this.

        if ((CurrentRateLimit_ctsperinterpt > 0) && (_Q15abs(TargetCurrentDelta) > CurrentRateLimit_ctsperinterpt)) {
            if (TargetCurrentDelta > 0)
                local_TargetCurrent = _Q15add(local_TargetCurrent, CurrentRateLimit_ctsperinterpt);
            else
                local_TargetCurrent = _Q15sub(local_TargetCurrent, CurrentRateLimit_ctsperinterpt);
        }
        else
            local_TargetCurrent = current_cmd;

    return local_TargetCurrent;

}



/* 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 N_SPEC        {0, 500, 1000, 2000} //For testing
#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], Cal.IWind_cnt, Cal.IWind_eng);
            MaxCurrent_cnts[i] = ConvertEngUnitsToFract(MaxCurrent_Spec[i], Cal.IWind_cnt, Cal.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 Cal.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 Cal.IWind_cnts per 1/32767 rpm counts.
        }
        MinCurrentSlope[LO_TABLE_LENGTH-1] = 0L;
        MaxCurrentSlope[LO_TABLE_LENGTH-1] = 0L;

        first_time = 0;
    }


    SC_Range_Max = SC_RANGE_MAX*CRSF_POSITION;
    SC_Range_Min = SC_RANGE_MIN*CRSF_POSITION;
    StopRange = STOP_RANGE*CRSF_POSITION;
    StopGain = MaxRPMAdjustment / StopRange; //RPMcnts per Range cnts

    int N_adjust = 0;
        
    if ((StatusBitFields.SpringRangeValid == 1) && (StatusBitFields.PermissiveMode == 0)) { //Valid Spring Range and not in permissive mode, adjust RPM for current winding limit computation.

        if ((f_N > 0) && (SC_Range < (SC_Range_Min + StopRange)))
        {
                N_adjust = ((SC_Range_Min + StopRange) - SC_Range) * StopGain;
        }
        if (N_adjust > MaxRPMAdjustment)
           N_adjust = MaxRPMAdjustment;     
        
        if ((f_N < 0) && (SC_Range > (SC_Range_Max - StopRange)))
                N_adjust = ((SC_Range_Max - StopRange) - SC_Range) * StopGain;       
        if(N_adjust < -MaxRPMAdjustment)
            N_adjust = -MaxRPMAdjustment;
        }    
    
    
        N = _Q15abs(f_N+N_adjust);
    
    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, Cal.IWind_cnt, Cal.IWind_eng);  //It's a mystery why this computes differently here than in CurrentMinMax
    Ismall = ConvertEngUnitsToFract(-MAX_WINDCURRENTLIMIT, Cal.IWind_cnt, Cal.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 = -32768; 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), Cal.IWind_cnt, Cal.IWind_eng),
                ConvertFractToEngUnits(CurrentMinMax(f_N, 0), Cal.IWind_cnt, Cal.IWind_eng),
                ConvertFractToEngUnits(CurrentMinMax(f_N, Ismall), Cal.IWind_cnt, Cal.IWind_eng)
                );
        putsU1(s);
    }

}

void TestCurrentMinMaxWithStop() {

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

    Ibig = ConvertEngUnitsToFract(MAX_WINDCURRENTLIMIT, Cal.IWind_cnt, Cal.IWind_eng);  //It's a mystery why this computes differently here than in CurrentMinMax
    Ismall = ConvertEngUnitsToFract(-MAX_WINDCURRENTLIMIT, Cal.IWind_cnt, Cal.IWind_eng);
    //sprintf(s,"Ibig/Ismall = %d  %d \r\n",Ibig,Ismall);
    //putsU1(s);
    Ibig = 32767;
    Ismall = -32768;
    
    float range;
    for (f_N = ConvertEngUnitsToFract(7000, RPMs_cnt, RPMs_eng); f_N >= ConvertEngUnitsToFract(0, RPMs_cnt, RPMs_eng); f_N -= ConvertEngUnitsToFract(100, RPMs_cnt, RPMs_eng)) {
   
    for(range = 82.0;range>-2;range = range-.25)
    {
        SC_Range = range*CRSF_POSITION;
        SC_RangeCtr = SC_RANGE_VALID_TIMEOUT;
        StatusBitFields.SpringRangeValid = 1;  //Indicate SC_Range is valid in mode word.
        StatusBitFields.PermissiveMode = 0;  //Indicate not in permissive mode is valid in mode word.
        sprintf(s, "%f  ",
                ConvertFractToEngUnits(CurrentMinMax(f_N, Ibig), Cal.IWind_cnt, Cal.IWind_eng)
                );
        putsU1(s);
    }
             sprintf(s, "\r\n");  putsU1(s);
    }

}

#endif