#include "config.h"
#include "extern.h"
#include "libq.h"
#include "conv.h"
#include "CalibrationConstants.h"
#include "CmdInterpreter.h"
#include "UART.h"

void SetTreFoilTargetPosition(unsigned char *RcvdBytes); //CmdID = 1   0 <= arg <= 1
void SetMaxonPwrMode(unsigned char *RcvdBytes); //CmdID = 2   0 <= arg <= 2
void SetTreFoilCurrLimit(unsigned char *RcvdBytes); //CmdID = 3   0 <= arg <= 5000
void ResetTreFoilWatchDog(unsigned char *RcvdBytes); //CmdID = 4   Arg unused
void SetTreFoilChargeMode(unsigned char *RcvdBytes); //CmdID = 5   0 <= arg <= 3
void SelectMaxonCmdFcn(unsigned char *RcvdBytes); //CmdID = 6
void SelectStateMachine(unsigned char *RcvdBytes); //CmdID = 7
void SetTreFoilActualPosition(unsigned char *RcvdBytes); //CmdID = 8   0 <= arg <= 1

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


// 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] ={
    {1, SetTreFoilTargetPosition},
    {2, SetMaxonPwrMode},
    {3, SetTreFoilCurrLimit},
    {4, ResetTreFoilWatchDog},
    {5, SetTreFoilChargeMode},
    {6, SelectMaxonCmdFcn},
    {7, SelectStateMachine},
    {8, SetTreFoilActualPosition},
    {255, Reset}
};

void SetTreFoilTargetPosition(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{

//Only let the user intervene if doors are at their targets,this should avoid confusion and take care of power-up and auto power-down cases
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 1) && (Status.bits.DoorPositionActual == Status.bits.DoorPositionTarget)) 
    {
        comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
        Status.bits.DoorPositionTarget = RcvdBytes[1]; //For this packet, Target Trefoil is encoded as a byte.
    }
}

void SetTreFoilActualPosition(unsigned char *RcvdBytes) //8  RcvdBytes points to array of 8 bytes
{
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 1)  && ((Status.bits.State == ST_MAXON_POWERED_DOWN) || (Status.bits.State == ST_MAXON_POWERED_UP_AND_DISABLED) || (Status.bits.State == ST_MAXON_ENABLED)))
    {
        comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
        Status.bits.DoorPositionActual = RcvdBytes[1]; //For this packet, Target Trefoil is encoded as a byte.
    }
}

void SetMaxonPwrMode(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 3))
    {
        comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
        Status.bits.MaxonPwrMode = RcvdBytes[1];
    }
}

void SetTreFoilCurrLimit(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{
    unsigned int local_uint;
    local_uint = ((unsigned int) ((RcvdBytes[2] << 8) + RcvdBytes[1]));

    if ((local_uint >= MIN_CURR) && (local_uint <= MAX_CURR))
    {
        comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
        WindingCurrent = local_uint;
    }
}

void ResetTreFoilWatchDog(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{
    comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
}

void SetTreFoilChargeMode(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 3))
    {
        comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
        Status.bits.ChargeMode = RcvdBytes[1] & 0x03;
    }
}

void SelectMaxonCmdFcn(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{
     comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
    if (RcvdBytes[1] == 0)
        MAXON_CmdFromCAN = 0;
    if (MAXON_CmdFromCAN == 0) //Only request new command if previous command is done or has been canceled.
        switch (RcvdBytes[1]) {
            case 1:
                MAXON_CmdFromCAN = 1;  //PowerUpMaxon
                break;
            case 2:
                MAXON_CmdFromCAN = 2; //EnableMaxon
                break;
            case 3:
                MAXON_CmdFromCAN = 3; //DisableMaxon
                break;
            case 4:
                MAXON_CmdFromCAN = 4; //PowerDownMaxon
                break;
             case 5:
                HomeDirection = RcvdBytes[2];
                MAXON_CmdFromCAN = 5; //StartHomeMove
                break;
             case 6:
                HomeDirection = RcvdBytes[2];
                MAXON_CmdFromCAN = 6; //StartIntermediateMove
                break;
             case 7:
                MAXON_CmdFromCAN = 7; //SetPosition
                break;
             case 8:
                MAXON_CmdFromCAN = 8; //Stop Move
                break;
             case 9:
                HomeDirection = RcvdBytes[2];
                MAXON_CmdFromCAN = 9; //Start incremental position mode
                break;
        }

}

void SelectStateMachine(unsigned char *RcvdBytes) //1  RcvdBytes points to array of 8 bytes
{
    if ((RcvdBytes[1] >= 0) && (RcvdBytes[1] <= 1) && (Status.bits.State == ST_MAXON_POWERED_DOWN))  //Only allow switch when Maxon is powered down
    {
        comms_loss_timeout = COMMSLOSS_TIMEOUT; //Reset seconds since watchdog counter to zero.
        StateMachine = RcvdBytes[1];
    }
}

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