#include "serial.h"

void execute_function()
{
        int data = Serial.read();
        int param1, param2;
        Serial.println(data);
        
        if(data >= SET_SERVO_POS)  //This means we have parameters
        {
            param1 = Serial.read();
            param2 = Serial.read();
            Serial.println(param1);
            Serial.println(param2);
        }

        //command is stored in first position of the array
        switch(data)
        {
                //parameterless functions
                case LED_ON:
                        led_on(); break;
                case LED_OFF:
                        led_off(); break;

                case LIFT_CANS:
                        liftCans(); break;
                case GRAB_FIRST:
                        grabFirst(); break;
                case SCORE:
                        scoreCans(); break;
                case CLOSEWINGS:
                        closeWings(); break;
                case OPENWINGS:
                        openWings(); break;
                case REVERSE:
                        reverse(); break;

                //two-parameter functions
                case SET_SERVO_POS:
                        set_servo_position(param1, param2);
                        break;

                //functions with return values
                case ISCAN:
                        Serial.write(isCan()); break;
                case ISBUTTON:
                    Serial.println("Trying to return button");
                        Serial.write(isButton()); 
                        
                        break;
                default:
                        Serial.println("CMD UNRECOGNIZED: ");
                        Serial.println(data);
                                                // Command was not recognized
                        break;
        }
}