/** @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(){
    // tell the master what type of slave is at which point in the chain
    assign_esmacat_slave_index(&ease_motor_shield, 1);  //<-- The Motor Shield is connected in number 0
    assign_esmacat_slave_index(&ease_touch_lcd_shield, 0);   //<-- The LCD Shield is connected in number 1
}

/**
 * @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()
{ 
    // Touch LCD Shield Initialization
    ease_touch_lcd_shield.set_output_variable_0_OUT_GEN_INT0(0);  // Initialize data into register 0 corresponding to the slave object
    ease_touch_lcd_shield.set_output_variable_1_OUT_GEN_INT1(0); // Initialize data into register 1 corresponding to the slave object
    ease_touch_lcd_shield.set_output_variable_2_OUT_GEN_INT2(0); // Initialize data into register 2 corresponding to the slave object
    ease_touch_lcd_shield.set_output_variable_3_OUT_GEN_INT3(0); // Initialize data into register 3 corresponding to the slave object

    // Motor Shield Initialization
    ease_motor_shield.set_output_variable_0_OUT_GEN_INT0(0); // Initialize data into register 0 corresponding to the slave object
    ease_motor_shield.set_output_variable_1_OUT_GEN_INT1(0); // Initialize data into register 1 corresponding to the slave object
    ease_motor_shield.set_output_variable_2_OUT_GEN_INT2(0); // Initialize data into register 2 corresponding to the slave object
    ease_motor_shield.set_output_variable_3_OUT_GEN_INT3(0); // Initialize data into register 3 corresponding to the slave object

    // Intialisation of the variables
    motor_select = 0;
    servo_postion = 0;
    dc_speed = 0;
    stepper_speed = 0;
    prev_dc = 0;
    prev_servo = 0;
    prev_stepper = 0;
}
/*
   EASE Touch LCD Shield Register Mapping
   |--------------------------------------------------|
   |   Register Number   |           Mapping          |
   |--------------------------------------------------|
   |         0           |         Motor Select       |
   |         1           |         Servo Angle        |
   |         2           |       DC Motor Speed       |
   |         3           |     Stepper motor Speed    |
   |--------------------------------------------------|
*/

/*
   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
    motor_select = ease_touch_lcd_shield.get_input_variable_0_IN_GEN_INT0();   // Read data from register 0 corresponding to the slave object

    ease_motor_shield.set_output_variable_0_OUT_GEN_INT0(motor_select);   // Write data into register 0 corresponding to the slave object

    if (motor_select == 0)   // Servo Motor Selected
    {
        servo_postion = ease_touch_lcd_shield.get_input_variable_1_IN_GEN_INT1();    // Read data from register 1 corresponding to the slave object
        ease_motor_shield.set_output_variable_1_OUT_GEN_INT1(servo_postion);   // Write data into register 1 corresponding to the slave object
        if (servo_postion != prev_servo) cout << "Servo at " << servo_postion << " angle \n";
    }

    else if (motor_select == 1)   // DC Motor Selected
    {
        dc_speed = ease_touch_lcd_shield.get_input_variable_2_IN_GEN_INT2();   // Read data from register 2 corresponding to the slave object
        ease_motor_shield.set_output_variable_2_OUT_GEN_INT2(dc_speed);   // Write data into register 2 corresponding to the slave object
        if (dc_speed != prev_dc) cout << "DC PWM at " << dc_speed << endl;
    }

    else if (motor_select == 2)   // Stepper Motor Selected
    {
        stepper_speed = ease_touch_lcd_shield.get_input_variable_3_IN_GEN_INT3();   // Read data from register 3 corresponding to the slave object
        ease_motor_shield.set_output_variable_3_OUT_GEN_INT3(stepper_speed);   // Write data into register 3 corresponding to the slave object
        if (stepper_speed != prev_stepper) cout << "Stepper speed at " << stepper_speed << " rpm \n";
    }
    prev_servo = servo_postion;
    prev_dc = dc_speed;
    prev_stepper = stepper_speed;
}