#include "config.h"
#include "extern.h"
#include "libq.h"
#include "conv.h"
#include "CalibrationConstants.h"
#include "types.h"
#include "CRSAL.h"
#include "can.h"
#include "xc.h"

/*Each Command has a unique command ID number, a function name, and a specification on the argument size, type and scaling*/
void SetWindCurrent(unsigned char *RcvdBytes); //CmdID = 31
void SetScaleFactor(unsigned char *RcvdBytes); //CmdID = 21
void SetRetractFactor(unsigned char *RcvdBytes); //CmdID = 22
void SetTargetVoltage(unsigned char *RcvdBytes); //CmdID = 23
void SetMaxBattChargeCurrent(unsigned char *RcvdBytes); //CmdID = 24
void SetBattSwitch(unsigned char *RcvdBytes); //CmdID = 25
void SetGainScheduleGain(unsigned char *RcvdBytes); //CmdID = 27
void SetRPM_StdDevTarget(unsigned char *RcvdBytes); //CmdID = 28
void SetMaxBattDrawCurrent(unsigned char *RcvdBytes); //CmdID = 29
void SetBiasWindCurrent(unsigned char *RcvdBytes); //CmdID = 34
void SetCANPacketRate(unsigned char *RcvdBytes); //CmdID = 35
void SetPermissiveMode(unsigned char *RcvdBytes); //CmdID = 36

//void Reset(unsigned char *RcvdBytes); //CmdID = 255


// There should be NUMBER_OF_CMDS_TO_INTERPRET elements in CmdParams initialized here, see Config.h and extern.h
// These can be arranged in order of most commonly used since the CAN interrupt searches for the correct id top to bottom.
ReceivedCmdParams CmdParams[NUMBER_OF_CMDS_TO_INTERPRET] = {
    {31, SetWindCurrent},
    {21, SetScaleFactor},
    {22, SetRetractFactor},
    {23, SetTargetVoltage},
    {24, SetMaxBattChargeCurrent},
    {25, SetBattSwitch},
    {27, SetGainScheduleGain},
    {28, SetRPM_StdDevTarget},
    {29, SetMaxBattDrawCurrent},
    {34, SetBiasWindCurrent},
    {35, SetCANPacketRate},
    {36, SetPermissiveMode}//,
   // {255, Reset}
};

/*SetWindCurrent:  CmdID = 31
 * Input: takes signed int from CANbus (two bytes).
 * Scaling: CRSF_CURRENT cnts per Amp.
 * Upper Limit (Engineering Units):  MAX_WINDCURRENTLIMIT Amps
 * Lower Limit (Engineering Units):  -MAX_WINDCURRENTLIMIT Amps
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetWindCurrent(unsigned char *RcvdBytes) {
    int local_int;
    fractional local_setpoint;
    if (StatusBitFields.Mode != PREINDEXPULSE_MODE) //Only allow user to set current if Index Pulse ahs been seen.
    {

        local_int = (RcvdBytes[2] << 8) + RcvdBytes[1];

        local_setpoint = ConvertEngUnitsToFract(((float) local_int) / CRSF_CURRENT, Cal.IWind_cnt, Cal.IWind_eng);
        if (Set_CRSALValue(local_setpoint, &UserCommandedCurrent) == 0) //Sets if within limits and returns 0, otherwise does not change UserCommandedCurrent.
        {
            TorqueCmdTimeout_ctr = TorqueCmdTimeout; //Reset Command Timeout Counter
            StatusBitFields.Mode = TORQUE_MODE;
            iTemp3 = UserCommandedCurrent.Value;

        }

    }
}


/*SetScaleFactor:  CmdID = 21
 * Input: takes single unsigned byte as argument from CANbus.
 * Scaling: CRSF_PERCENTAGES cnts per unit factor.
 * Upper Limit (Engineering Units):  MAX_SCALE_FACTOR
 * Lower Limit (Engineering Units):  MIN_SCALE_FACTOR
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetScaleFactor(unsigned char *RcvdBytes) {
    unsigned char local_setpoint;

    local_setpoint = RcvdBytes[1]; //Scale and Retract are transmitted as bytes, promote to int here for storage.  No re-scaling necessary

    Set_CRSALValue(local_setpoint, &scale_factor); //Sets if within limits, otherwise does not change scale_factor.
}


/*SetRetractFactor:  CmdID = 22
 * Input: takes single unsigned byte as argument from CANbus.
 * Scaling: CRSF_PERCENTAGES cnts per unit factor.
 * Upper Limit (Engineering Units):  MAX_RETRACT_FACTOR
 * Lower Limit (Engineering Units):  MIN_RETRACT_FACTOR
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetRetractFactor(unsigned char *RcvdBytes) {
    unsigned char local_setpoint;

    local_setpoint = RcvdBytes[1]; //Scale and Retract are transmitted as bytes, promote to int here for storage.  No re-scaling necessary

    Set_CRSALValue(local_setpoint, &retract_factor); //Sets if within limits, otherwise does not change retract_factor.

}


/*SetTargetVoltage:  CmdID = 23
 * Input: takes unsigned int from CANbus (two bytes).
 * Scaling: CRSF_VOLTAGE cnts per Volt.
 * Upper Limit (Engineering Units):  MAX_TARGETVOLTAGE Volts
 * Lower Limit (Engineering Units):  MIN_TARGETVOLTAGE Volts
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetTargetVoltage(unsigned char *RcvdBytes) {
    unsigned int local_uint;
    int local_setpoint;

    local_uint = ((unsigned int) ((RcvdBytes[2] << 8) + RcvdBytes[1]));
    local_setpoint = ConvertEngUnitsToFract(((float) local_uint) / CRSF_VOLTAGE, Cal.VBus_cnt, Cal.VBus_eng);

    Set_CRSALValue(local_setpoint, &TargetVoltage); //Sets if within limits, otherwise does not change TargetVoltage.

}


/*SetMaxBattChargeCurrent:  CmdID = 24
 * Input: takes unsigned int from CANbus (two bytes).
 * Scaling: CRSF_CURRENT cnts per Amp.
 * Upper Limit (Engineering Units):  MAX_BATTCHARGECURRENTLIMIT Amps
 * Lower Limit (Engineering Units):  MIN_BATTCHARGECURRENTLIMIT Amps
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetMaxBattChargeCurrent(unsigned char *RcvdBytes) {
    unsigned int local_uint;
    int local_setpoint;

    local_uint = ((unsigned int) ((RcvdBytes[2] << 8) + RcvdBytes[1]));
    local_setpoint = ConvertEngUnitsToFract(((float) local_uint) / CRSF_CURRENT, Cal.IBatt_cnt, Cal.IBatt_eng);
    Set_CRSALValue(local_setpoint, &MaxBattChargeCurrent); //Sets if within limits, otherwise does not change MaxBattChargeCurrent.

}


/*SetBattSwitchMode: CmdID = 25
 * Input: takes single from CANbus (two bytes).
 * Scaling: None
 * Valid Values:  0 = Off, 1 = On
 * Out of Range Behavior:  Only sets if argument is 0 or 1
 */
void SetBattSwitch(unsigned char *RcvdBytes)
{
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 1))
        StatusBitFields.BattSwitchRequest = RcvdBytes[1];

}


/*SetGainScheduleGain:  CmdID = 27
 * Input: takes unsigned int from CANbus (two bytes).
 * Scaling: Scale Factor percentage points (per hour per 1000RPM Std Deviation Error.  i.e.  50 will change the scale factor by .5 after an hour of a 1000RPM error in standard deviation measurement versus target.)
 * Upper Limit (Engineering Units):  MAX_SCALE_INTEGRATION_GAIN
 * Lower Limit (Engineering Units):  0
 * Notes:  Value of Zero disables gain schedule feature
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetGainScheduleGain(unsigned char *RcvdBytes) {
    unsigned int local_uint;
    fractional local_setpoint; //Currents specified as positive by users indicate negative current limit values.

    local_uint = ((unsigned int) ((RcvdBytes[2] << 8) + RcvdBytes[1]));
    local_setpoint = (signed int) local_uint;

    if (Set_CRSALValue(local_setpoint, &ScaleIntegrationGain) == 0) //Sets if within limits, otherwise does not change ScaleIntegrationGain.
    {
        if (local_setpoint == 0)
            StatusBitFields.GainScheduleMode = 0;
        else
            StatusBitFields.GainScheduleMode = 1;
    }
}


/*SetRPM_StdDevTarget:  CmdID = 28
 * Input: takes unsigned int from CANbus (two bytes).
 * Scaling: CRSF_RPM cnts per RPM.
 * Upper Limit (Engineering Units):  MAX_RPM_STD_DEV_TARGET RPM
 * Lower Limit (Engineering Units):  MIN_RPM_STD_DEV_TARGET RPM
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetRPM_StdDevTarget(unsigned char *RcvdBytes) {
    unsigned int local_uint;
    fractional local_setpoint;

    local_uint = (RcvdBytes[2] << 8) + RcvdBytes[1];
    local_setpoint = ConvertEngUnitsToFract(((float) local_uint) / CRSF_RPM, RPMs_cnt, RPMs_eng);
    Set_CRSALValue(local_setpoint, &GainScheduleRPMStdDevTarget);
}


/*SetMaxBattDrawCurrent:  CmdID = 29
 * Input: takes unsigned int from CANbus (two bytes).
 * Scaling: CRSF_CURRENT cnts per Amp.
 * Upper Limit (Engineering Units):  MAX_BATTDRAWCURRENTLIMIT Amps
 * Lower Limit (Engineering Units):  MIN_BATTDRAWCURRENTLIMIT Amps
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetMaxBattDrawCurrent(unsigned char *RcvdBytes) {
    unsigned int local_uint;
    int local_setpoint; //Currents specified as positive by users indicate negative current limit values.

    local_uint = ((unsigned int) ((RcvdBytes[2] << 8) + RcvdBytes[1]));
    local_setpoint = ConvertEngUnitsToFract(-((float) local_uint) / CRSF_CURRENT, Cal.IBatt_cnt, Cal.IBatt_eng);
    Set_CRSALValue(local_setpoint, &MaxBattDrawCurrent); //Sets if within limits, otherwise does not change MaxBattDrawCurrent.
}


/*SetBiasWindCurrent:  CmdID = 34
 * Input: takes signed int from CANbus (two bytes).
 * Scaling: CRSF_CURRENT cnts per Amp.
 * Upper Limit (Engineering Units):  MAX_BIASCURRENTLIMIT Amps
 * Lower Limit (Engineering Units):  -MAX_BIASCURRENTLIMIT Amps
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetBiasWindCurrent(unsigned char *RcvdBytes) {
    int local_int;
    fractional local_setpoint;
    local_int = (RcvdBytes[2] << 8) + RcvdBytes[1];

    local_setpoint = ConvertEngUnitsToFract(((float) local_int) / CRSF_CURRENT, Cal.IWind_cnt, Cal.IWind_eng);

    if (Set_CRSALValue(local_setpoint, &BiasCurrent) == 0) //Sets if within limits and returns 0, otherwise does not change BiasCurrent.
    {
        BiasCmdTimeout_ctr = BiasCmdTimeout; //Reset Command Timeout Counter
    }
}


/*SetCanPacketRate:  Cmd ID = 35
 * Input: takes single unsigned  byte from CANbus.
 * Scaling: 1 Count per Hz.
 * Out of Range Behavior:  Set to Min or Max allowable if out of range
 */
void SetCANPacketRate(unsigned char *RcvdBytes) {
    int local_setpoint;

    local_setpoint = RcvdBytes[1]; //Scale and Retract are transmitted as bytes, promote to int here for storage.  No re-scaling necessary
    if (Set_CRSALValue(local_setpoint, &CAN_packet_rate) == 0) //Sets if within limits and returns 0, otherwise does not change BiasCurrent.
    {
        PR1 = SetCANTimer(CAN_packet_rate.Value);
        if (TMR1 > PR1) //Need to reset timer to smaller value do it doesn't wait to roll over before interrupting
            TMR1 = 0;

    }

}

/*SetPermissiveMode: CmdID = 36
 * Input: takes single from CANbus (two bytes).
 * Scaling: None
 * Valid Values:  0 = Off, 1 = On
 * Out of Range Behavior:  Only sets if argument is 0 or 1
 */
void SetPermissiveMode(unsigned char *RcvdBytes)
{
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 1))
        StatusBitFields.PermissiveMode = RcvdBytes[1];

}

/*Reset (No Argument)  CmdID = 255
 */
//void Reset(unsigned char *RcvdBytes) {
//#ifdef ALLOW_RESET_BY_CAN
//    asm("RESET"); //Reset micro-controller.
//#endif
//}