/** @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_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()
{ 
    // LCD Shield Initialization
    ease_lcd_shield.set_output_variable_0_OUT_GEN_INT0(0);
    ease_lcd_shield.set_output_variable_1_OUT_GEN_INT1(0);
    ease_lcd_shield.set_output_variable_2_OUT_GEN_INT2(0);
    ease_lcd_shield.set_output_variable_3_OUT_GEN_INT3(0);
    ease_lcd_shield.set_output_variable_4_OUT_GEN_INT4(0);

    // 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);

    cout << "Servo Motor Selected \n";

    // Intialisation of the variables
    motor_select = 0;
    servo_postion = 0;
    dc_speed = 0;
    stepper_speed = 0;
    prev_analog_value = 0;
    lcd_analog_input_value = 1023;   // Random value initialization
    button_pressed_flag = FALSE;
}
/*
   EASE LCD Shield Register Mapping
   |--------------------------------------------------|
   |   Register Number   |           Mapping          |
   |--------------------------------------------------|
   |         0           |   LCD analog input value   |
   |         1           |         Servo Angle        |
   |         2           |       DC Motor Speed       |
   |         3           |     Stepper motor Speed    |
   |         4           |         Motor Select       |
   |--------------------------------------------------|
*/

/*
   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
    lcd_analog_input_value = ease_lcd_shield.get_input_variable_0_IN_GEN_INT0();

    // cout << lcd_analog_input_value << " ";
    // <----------------------------------------------------------------------->
    //                            LCD Shield  Code 
    // <----------------------------------------------------------------------->


    if (lcd_analog_input_value >= 50 && lcd_analog_input_value <= 150)   // Up Button Increase Speed
    {
        // cout << lcd_analog_input_value << "\t";
        if (motor_select == 0)
        {
            servo_postion += 1;
            ease_lcd_shield.set_output_variable_1_OUT_GEN_INT1(servo_postion % 180);
            cout << "Servo position Increased \n";
        }

        else if (motor_select == 1)
        {
            dc_speed += 1;
            ease_lcd_shield.set_output_variable_2_OUT_GEN_INT2(dc_speed);
            cout << "DC Motor Speed Increased \n";
        }

        else
        {
            stepper_speed += 1;
            ease_lcd_shield.set_output_variable_3_OUT_GEN_INT3(stepper_speed);
            cout << "Stepper Motor Speed Increased \n";
        }

    }

    if (lcd_analog_input_value >= 150 && lcd_analog_input_value <= 300)   // Down Button Decrease Speed
    {
        // cout << lcd_analog_input_value << "\t";
        if (motor_select == 0)
        {
            servo_postion -= 1;
            ease_lcd_shield.set_output_variable_1_OUT_GEN_INT1(servo_postion % 180);
            cout << "Servo position decreased \n";
        }

        else if (motor_select == 1)
        {
            dc_speed -= 1;
            ease_lcd_shield.set_output_variable_2_OUT_GEN_INT2(dc_speed);
            cout << "DC Motor Speed decreased \n";
        }

        else
        {
            stepper_speed -= 1;
            ease_lcd_shield.set_output_variable_3_OUT_GEN_INT3(stepper_speed);
            cout << "Stepper Motor Speed decreeased \n";
        }
    }


    if (lcd_analog_input_value >= 500 && lcd_analog_input_value <= 750)
    {
        cout << lcd_analog_input_value << "\t";
        motor_select = (motor_select + 1) % 3;
        ease_lcd_shield.set_output_variable_4_OUT_GEN_INT4(motor_select);
        if (motor_select == 0)   cout << "Servo Motor Selected" << endl;
        if (motor_select == 1)   cout << "DC Motor Selected" << endl;
        if (motor_select == 2)   cout << "Stepper Motor Selected" << endl;
    }

    // <----------------------------------------------------------------------->
    //                            Motor Shield  Code 
    // <----------------------------------------------------------------------->
    ease_motor_shield.set_output_variable_0_OUT_GEN_INT0(motor_select);
    ease_motor_shield.set_output_variable_1_OUT_GEN_INT1(servo_postion);
    ease_motor_shield.set_output_variable_2_OUT_GEN_INT2(dc_speed);
    ease_motor_shield.set_output_variable_3_OUT_GEN_INT3(stepper_speed);
    prev_analog_value = lcd_analog_input_value;  
}