#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 "Maxon.h"
#include "types.h"
#include "ManageTreFoilPosition.h"

void HomeMoveStateMachine(void) {
    static unsigned long start_time_ctr;
 //   static unsigned long delay_time_ctr;
 //   static unsigned int HomeMoveNumber;
    CurrentPositionSet = 1;  //Ensures position is read every time, this feature not needed for the HOME_MOVE_STATE_MACHINE

    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).

            if (Status.bits.DoorPositionActual != Status.bits.DoorPositionTarget) //Test And Action
            {
                EnableMaxon();
                Status.bits.State = ST_MAXON_ENABLED;
                break;
            }
            if (Status.bits.MaxonPwrMode == MAXON_HOLDPOSMODE) //Test And Action
            {
                EnableMaxon();
                Status.bits.State = ST_MAXON_ENABLED;
                break;
            }
            if (Status.bits.MaxonPwrMode == MAXON_LOWPWRMODE) //Test And Action
            {
                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
            {
                DisableMaxon(); //Disable for one CAN cycle.  Neccessary to allow two home moves in a row, annoying, need to look for a better solution...
                EnableMaxon();  //Neccessary to allow two home moves in a row, annoying, need to look for a better solution...
                StartHomeMove(Status.bits.DoorPositionActual);
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                Status.bits.State = ST_MOVING_HOME_TOWARDS_CURRENT_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
            {
                DisableMaxon();
                PowerDownMaxon();
                Status.bits.State = ST_MAXON_POWERED_DOWN;
                break;
            }
            break;

        case ST_MOVING_HOME_TOWARDS_CURRENT_POSITION: //3
        {
            if (StatusWord & 0x3000) // Or, Home Move Complete Or Homing Error
            {
                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 (CAN_PacketSend_ctr > (start_time_ctr + HOME_MOVE_TIMEOUT / (1000 / CAN_PACKET_RATE))) //Handle Timeout
            {
                StopMove(); //Stop move if it timed out.
                //if(Status.bits.DoorPositionActual == 0)
                //    SetPosition(OPEN_TREFOIL_ENCODER_VALUE);  //Need to set position in case of timeout
                //else
                //    SetPosition(CLOSED_TREFOIL_ENCODER_VALUE);
                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;
            }

            break;

        }

        case ST_VELOCITY_MOVE_TOWARDS_TARGET_POSITION: //4
        {
            if (((Status.bits.DoorPositionTarget == 1) && (TreFoilPositionActualValue > CLOSED_TREFOIL_ENCODER_VALUE - TREFOIL_TARGET_OFFSET)) || //Getting closed to being closed
                    ((Status.bits.DoorPositionTarget == 0) && (TreFoilPositionActualValue < OPEN_TREFOIL_ENCODER_VALUE + TREFOIL_TARGET_OFFSET))) //Getting closed to being open
            {
                StopMove(); //Stop
                StartHomeMove(Status.bits.DoorPositionTarget);
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                Status.bits.State = ST_MOVING_HOME_TOWARDS_TARGET_POSITION;
                break;
            }

            if ((CAN_PacketSend_ctr > (start_time_ctr + VELOCITY_MOVE_TIMEOUT / (1000 / CAN_PACKET_RATE)))) //Handle Timeout Case
            {
                StopMove(); //Stop
                StartHomeMove(Status.bits.DoorPositionTarget);
                start_time_ctr = CAN_PacketSend_ctr; //CAN_PacketSend_ctr counts up at the CAN packet rate (10Hz).
                Status.bits.State = ST_MOVING_HOME_TOWARDS_TARGET_POSITION;
                break;
            }

            break;

        }

        case ST_MOVING_HOME_TOWARDS_TARGET_POSITION: //5
        {
            if (StatusWord & 0x3000) // Or, Home Move Complete Or Homing Error
            {
                Status.bits.State = ST_DOORS_SUCCESSFULY_MOVED;
                break;
            }
            if (CAN_PacketSend_ctr > (start_time_ctr + HOME_MOVE_TIMEOUT / (1000 / CAN_PACKET_RATE))) //Handle Timeout
            {
                StopMove(); //Stop move if it timed out.
                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;
        }

    }
}