#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "conv.h"
#include "CalibrationConstants.h"
#include "libq.h"
#include "types.h"
#include "Maxon.h"
#include "ManageTreFoilPosition.h"


void VelMoveStateMachine(void) {
   static unsigned long start_time_ctr;

    if (Status.bits.MaxonPower) //Read Maxon Status if powered up.
    {
        ReadMaxonStatus();
    }

    switch (Status.bits.State) { //Execute State Machine
        case ST_MAXON_POWERED_DOWN: //0
            if (Status.bits.MaxonPwrMode != MAXON_LOWPWRMODE) //Test And Action
            {
                PowerUpMaxon();
                Status.bits.State = ST_MAXON_POWERED_UP_AND_DISABLED;
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                break;
            }
            if (Status.bits.DoorPositionActual != Status.bits.DoorPositionTarget) //Test And Action
            {
                PowerUpMaxon();
                Status.bits.State = ST_MAXON_POWERED_UP_AND_DISABLED;
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                break;
            }
            break;
        case ST_MAXON_POWERED_UP_AND_DISABLED: //1

            if ((Status.bits.MaxonPower == 0) && (CAN_PacketSend_ctr < (start_time_ctr + DELAY_AFTER_MAXON_POWERUP / (1000 / CAN_PACKET_RATE)))) //Delay if coming in from unpowered state
                break; //Wait for specified time delay for Maxon to come alive.

            Status.bits.MaxonPower = 1; //Indicate Maxon is fully powered up (based on delay having past).

            //Set Position if powering up but not enabling.
            if((Status.bits.State == ST_MAXON_POWERED_UP_AND_DISABLED) && (CurrentPositionSet == 0))
            {
                SetPosition(TreFoilPositionActualValue);  //Set current position before enabling
                CurrentPositionSet = 1;
                break;
            }

            if (Status.bits.DoorPositionActual != Status.bits.DoorPositionTarget) //Test And Action
            {
                SetPosition(TreFoilPositionActualValue);  //Set current position before enabling
                CurrentPositionSet = 1;
                EnableMaxon();
                Status.bits.State = ST_MAXON_ENABLED;
                break;
            }
            if (Status.bits.MaxonPwrMode == MAXON_HOLDPOSMODE) //Test And Action
            {
                SetPosition(TreFoilPositionActualValue);  //Set current position before enabling
                CurrentPositionSet = 1;
                EnableMaxon();
                Status.bits.State = ST_MAXON_ENABLED;
                break;
            }
            if (Status.bits.MaxonPwrMode == MAXON_LOWPWRMODE) //Test And Action
            {
                CurrentPositionSet = 0;  //Don't read position again on power-up until the current position has been set
                PowerDownMaxon();
                Status.bits.State = ST_MAXON_POWERED_DOWN;
                break;
            }
            break;


        case ST_MAXON_ENABLED: //2
            if (Status.bits.DoorPositionActual != Status.bits.DoorPositionTarget) //Test And Action
            {
                StartIntermediateMove(Status.bits.DoorPositionTarget);
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                Status.bits.State = ST_VELOCITY_MOVE_TOWARDS_TARGET_POSITION;
                break;
            }

            if (Status.bits.MaxonPwrMode == MAXON_MONITORPOSMODE) //Test And Action
            {
                DisableMaxon();
                Status.bits.State = ST_MAXON_POWERED_UP_AND_DISABLED;
                break;
            }
            if (Status.bits.MaxonPwrMode == MAXON_LOWPWRMODE) //Test And Action
            {
                CurrentPositionSet = 0;  //Don't read position again on power-up until the current position has been set
                DisableMaxon();
                PowerDownMaxon();
                Status.bits.State = ST_MAXON_POWERED_DOWN;
                break;
            }
            break;

        case ST_VELOCITY_MOVE_TOWARDS_TARGET_POSITION: //4
        {
            if ((CAN_PacketSend_ctr > (start_time_ctr + VELOCITY_MOVE_TIMEOUT / (1000 / CAN_PACKET_RATE)))) //Handle Timeout Case
            {
                StopMove(); //Stop
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                if(Status.bits.DoorPositionTarget == 1)
                  StartIncrementalPositionMove(-TREFOIL_HOME_OFFSET);
                else
                  StartIncrementalPositionMove(TREFOIL_HOME_OFFSET);
                Status.bits.State = ST_PERFORMING_BACKUP_MOVE;
                break;
            }

            break;

        }

        case ST_PERFORMING_BACKUP_MOVE:
        {
            if ((StatusWord & 0x0400) || (CAN_PacketSend_ctr > (start_time_ctr + BACKUP_MOVE_TIMEOUT / (1000 / CAN_PACKET_RATE)))) //Handle timeout or completed move
            {
                StopMove(); //Stop
                Status.bits.State = ST_DOORS_SUCCESSFULY_MOVED;
                break;
            }
            break;
        }

        case ST_DOORS_SUCCESSFULY_MOVED: //6
        {
            Status.bits.DoorPositionActual = Status.bits.DoorPositionTarget;
            Status.bits.State = ST_MAXON_ENABLED; //This will take it back to the enabled state, will disable or power down from there as needed.

            break;
        }
        
    }

}