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



void RunCmdsFromCAN(void) {
    switch (MAXON_CmdFromCAN) {
        case 0:
            ReadMaxonStatus();
            break;
        case 1: //PowerUpMaxon
            PowerUpMaxon();
            Status.bits.MaxonPower = 1;
            MAXON_CmdFromCAN = 0; //Mark as done so it doesn't repeat
            break;
        case 2: //EnableMaxon
            EnableMaxon();
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 3: //DisableMaxon
            DisableMaxon();
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 4: //PowerDownMaxon
            PowerDownMaxon();
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 5: //Perform Home Move
            StartHomeMove(HomeDirection); //Try once
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 6: //Perform Intermediate move based on speed
            StartIntermediateMove(HomeDirection); //Try once
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 7: //Perform Intermediate move based on speed
            SetPosition(100000L); //Try once
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 8: //Perform Intermediate move based on speed
            StopMove(); //Try once
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
        case 9: //Start incremental position Move
            StartIncrementalPositionMove(HomeDirection); //Try once
            MAXON_CmdFromCAN = 0; //Mark as done
            break;
    }

    return;
}

void ReadMaxonStatus(void) {
    long temp_long;
    int temp_int;

    if (Status.bits.MaxonPower) {
        if(CurrentPositionSet) //Only read position after current position has been set at powerup.
          {
          temp_long = ReadMaxonDWord(1, 0x6064, 00);
          if ((Status.bits.Maxon_CommError == 0)  || (Status.bits.Maxon_CommError == 2))
            TreFoilPositionActualValue = temp_long;
          }

        temp_long = ReadMaxonDWord(1, 0x30D1, 01); //Read Motor Torque
        if (Status.bits.Maxon_CommError == 0)
            MotorCurrentActualValue = temp_long;

        temp_int = ReadMaxonWord(1, 0x6041, 00); //Read StatusWord
        if (Status.bits.Maxon_CommError == 0)
            StatusWord = temp_int;


    }
    return;
}

void SetPosition(long Pos) {
    int temp;
    temp = Status.bits.MaxonEnabled;
    if (Status.bits.MaxonEnabled == 1)
    {
        DisableMaxon();
    }
    WriteMaxonObject(1, 0x6060, 0x00, 6); //  Put into homing mode
    WriteMaxonObject(1, 0x30B0, 0x00, Pos); //  Set Home Position
    WriteMaxonObject(1, 0x6098, 0x00, 37); //  Set Homing Method to Actual Position
    WriteMaxonObject(1, 0x6040, 0x00, 0x001f); //  Start Homing Move

    if (temp) //Re-enable if enabled to begin with.
    {
        EnableMaxon();
    }
}

void StopMove() {
    WriteMaxonObject(1, 0x6040, 0x00, 0x010f); //  Halt
    return;
}

void StartIntermediateMove(char Dir) {
    WriteMaxonObject(1, 0x6060, 0x00, 3); //  Put into velocity mode

    WriteMaxonObject(1, 0x607F, 0x00, MAXON_MOTOR_SPEED); //  Set Max Velocity
    WriteMaxonObject(1, 0x6083, 0x00, MAXON_MOTOR_ACCEL); //  Set Acceleration
    WriteMaxonObject(1, 0x6084, 0x00, MAXON_MOTOR_DECEL); //  Set Deceleration
    WriteMaxonObject(1, 0x3001, 0x01, (long) WindingCurrent); //  Set Nominal Current
    WriteMaxonObject(1, 0x3001, 0x02, (long) WindingCurrent); //  Set Max Winding Current

    if (Dir == 0)//Open fully
    {
        WriteMaxonObject(1, 0x60FF, 0x00, -MAXON_MOTOR_SPEED); //  Set Velocity
    } else//Close  fully
    {
        WriteMaxonObject(1, 0x60FF, 0x00, MAXON_MOTOR_SPEED); //  Set Velocity
    }
    WriteMaxonObject(1, 0x6040, 0x00, 0x000f); //  Start Velocity Move
    return;
}

void StartIncrementalPositionMove(long cnts)
{
   
    WriteMaxonObject(1, 0x6060, 0x00, 1); //  Put into position Mode
    WriteMaxonObject(1, 0x607F, 0x00, MAXON_MOTOR_SPEED); //  Set Max Velocity
    WriteMaxonObject(1, 0x6083, 0x00, MAXON_MOTOR_ACCEL); //  Set Acceleration
    WriteMaxonObject(1, 0x6084, 0x00, MAXON_MOTOR_DECEL); //  Set Deceleration
    WriteMaxonObject(1, 0x607A, 0x00, cnts); //  Set Distance

    WriteMaxonObject(1, 0x6040, 0x00, 0x007f); //  Start Incremental Move

    return;
}

void StartHomeMove(char Dir) {
    WriteMaxonObject(1, 0x6060, 0x00, 6); //  Put into homing mode

    WriteMaxonObject(1, 0x6099, 0x01, MAXON_MOTOR_SPEED); //  Set Homing Speeds, in RPM of motor shaft

    WriteMaxonObject(1, 0x6099, 0x02, MAXON_MOTOR_SPEED); //  Set Homing Speeds, in RPM of motor shaft

    WriteMaxonObject(1, 0x609A, 0x00, MAXON_MOTOR_HOME_ACCEL); //  Set Homing Acceleration
    WriteMaxonObject(1, 0x30B1, 0x00, TREFOIL_HOME_OFFSET); //  Set Offset Move Distance
    WriteMaxonObject(1, 0x30B2, 0x00, WindingCurrent); //  Set Current threshold for homing



    if (Dir == 0)//Open fully
    {
        WriteMaxonObject(1, 0x6098, 0x00, -4); //  Set Homing Method (Curr Threshold Neg Speed (Opens)).
        WriteMaxonObject(1, 0x30B0, 0x00, OPEN_TREFOIL_ENCODER_VALUE); //  Set Home Position
    } else//Close  fully
    {
        WriteMaxonObject(1, 0x6098, 0x00, -3); //  Set Homing Method (Curr Threshold Neg Speed (Closes)).
        WriteMaxonObject(1, 0x30B0, 0x00, CLOSED_TREFOIL_ENCODER_VALUE); //  Set Home Position
    }

    // sprintf(s, "Starting Home Move\r\n");    putsU1(s);
    WriteMaxonObject(1, 0x6040, 0x00, 0x001f); //  Start Homing Move

    return;
}

void PowerUpMaxon(void) //Power Up and wait 200ms
{
    MAXON_POWER = 1;
    MAXON_TETHER_POWER = 1;
    return;
}

void EnableMaxon(void) {
    if(Status.bits.MaxonPower)
    {
    WriteMaxonObject(1, 0x6040, 0x00, 0x0006); //  Send "Shutdown"
    WriteMaxonObject(1, 0x6040, 0x00, 0x000f); //  Send "Switch on & Enable operation"
    Status.bits.MaxonEnabled = 1;
    }
    return;
}

void DisableMaxon(void) {
    if(Status.bits.MaxonPower)
    {
    WriteMaxonObject(1, 0x6040, 0x00, 0x0006); //  Send "Shutdown"  //This isn't going through for some reason
    Status.bits.MaxonEnabled = 0;
    }
    return;
}

void PowerDownMaxon(void) //Power Up and wait 200ms
{
    MAXON_TETHER_POWER = 0;
    MAXON_POWER = 0;
    Status.bits.MaxonPower = 0;
    return;
}