/** @file
 * @brief This file contains the definition of the functions associated with the user-defined
 * application for the Esmacat slave project */
 /*****************************************************************************************
 * INCLUDES
 ****************************************************************************************/
#include "my_app.h"

/*****************************************************************************************
 * FUNCTIONS
 ****************************************************************************************/
/**
 * @brief Identifies the actual Esmacat slave sequence in the EtherCAT communication chain.
 */
void my_app::assign_slave_sequence(){
    assign_esmacat_slave_index(&ease_motor_shield, 0);   //<-- The Motor Shield is connected in number 0
}

/**
 * @brief Configure your Esmacat slave.
 * Link Esmacat slave object with the actual Esmacat slave in the EtherCAT communication chain.
 * Functions beginning with 'configure_slave' must only be executed in this function
 */
void my_app::configure_slaves(){
    // add initialization code here
    // Functions starting with "configure_slave" work only in configure_slaves() function
}

/** @brief Initialization that needs to happen on the first iteration of the loop
 */
void my_app::init()
{
    // Motor Shield Initialization
    ease_motor_shield.set_output_variable_0_OUT_GEN_INT0(0);
    ease_motor_shield.set_output_variable_1_OUT_GEN_INT1(0);
    ease_motor_shield.set_output_variable_2_OUT_GEN_INT2(0);
    ease_motor_shield.set_output_variable_3_OUT_GEN_INT3(0);

    // Variable Initialization
    servo_position = 0;
    dc_pwm = 60;
    stepper_speed = 0;
    motor_select = -1;    

    cout << "Motor Party Program" << endl;
    counter = 0;
}

/*
   EASE Motor Shield Register Mapping
   |--------------------------------------------------|
   |   Register Number   |           Mapping          |
   |--------------------------------------------------|
   |         0           |         Motor Select       |
   |         1           |         Servo Angle        |
   |         2           |       DC Motor Speed       |
   |         3           |     Stepper motor Speed    |
   |--------------------------------------------------|
/*

/**
 * @brief Executes functions at the defined loop rate
 */
void my_app::loop(){

    // add functions below that are to be executed at the loop rate
    ease_motor_shield.set_output_variable_0_OUT_GEN_INT0(motor_select);

    if (counter == 0)
    {
        motor_select = (motor_select + 1) % 3;
        if (motor_select == 0)
        {
            cout << "Controlling Servo Motor now" << endl;
            dc_pwm = 60;
            stepper_speed = 0;
        }
        else if (motor_select == 1)
        {
            cout << "Controlling DC Motor now" << endl;
            servo_position = 0;
            stepper_speed = 0;
        }
        else if (motor_select == 2)
        {
            cout << "Controlling Stepper Motor now" << endl;
            servo_position = 0;
            dc_pwm = 60;
        }
    }

    if (motor_select == 0)
    {
        cout << "Servo at " << servo_position << "degrees \n";
        if (counter > 90)
        {
            servo_position -= 1;
            ease_motor_shield.set_output_variable_1_OUT_GEN_INT1(servo_position % 180);
        }
        else if (counter < 90)
        {
            servo_position += 1;
            ease_motor_shield.set_output_variable_1_OUT_GEN_INT1(servo_position % 180);
        }
    }
    
    if (motor_select == 1)
    {
        cout << "DC Motor pwm value at " << dc_pwm << endl;
        if (counter > 90)
        {
            dc_pwm -= 1;
            ease_motor_shield.set_output_variable_2_OUT_GEN_INT2(dc_pwm % 255);
        }
        else if (counter < 90)
        {
            dc_pwm += 1;
            ease_motor_shield.set_output_variable_2_OUT_GEN_INT2(dc_pwm % 255);
        }
    }

    if (motor_select == 2)
    {
        cout << "Stepper motor running at " << stepper_speed << "rpm \n";
        if (counter > 90)
        {
            stepper_speed -= 1;
            ease_motor_shield.set_output_variable_3_OUT_GEN_INT3(stepper_speed % 255);
        }
        else if (counter < 90)
        {
            stepper_speed += 1;
            ease_motor_shield.set_output_variable_3_OUT_GEN_INT3(stepper_speed % 255);
        }
    }
    counter = (counter + 1 ) % 180;
}