#include <Servo.h>

/*
 * Commandes (exemples) :
 *
 * - RD13 -> lecture de l'entrée digitale 13
 * - RA4 - > lecture de l'entrée  analogique 4
 * - WD13:1 -> mise à 1 de la sortie digitale 13
 * - WA6:125 -> écrit 125 sur la sortie PWM 6 (PWM)
 */

char operation; // operation (R, W, ...)
char mode; // mode (D, A)
int pin_number; // numéro de broche
int digital_value; // valeur digitale
int analog_value; // valeur analogique
int value_to_write; // valeur à écrire
int wait_for_transmission = 5; // délais de réception de donnée 


Servo M0;
Servo M1;
Servo M2;
int pos0 = 90;
int pos1 = 90;
int pos2 = 90;

void set_pin_mode(int pin_number, char mode){
    /*
     * Performs a pinMode() operation depending on the value of the parameter
     * mode :
     * - I: Sets the mode to INPUT
     * - O: Sets the mode to OUTPUT
     * - P: Sets the mode to INPUT_PULLUP
     */

    switch (mode){
        case 'I':
            pinMode(pin_number, INPUT);
            break;
        case 'O':
            pinMode(pin_number, OUTPUT);
            break;
        case 'P':
            pinMode(pin_number, INPUT_PULLUP);
            break;
    }
}

void digital_read(int pin_number){
    /*
     * Performs a digital read on pin_number and returns the value read to serial
     * in this format: D{pin_number}:{value}\n where value can be 0 or 1
     */

    digital_value = digitalRead(pin_number);
    Serial.print('D');
    Serial.print(pin_number);
    Serial.print(':');
    Serial.println(digital_value); // Adds a trailing \n
}

void analog_read(int pin_number){
    /*
     * Performs an analog read on pin_number and returns the value read to serial
     * in this format: A{pin_number}:{value}\n where value ranges from 0 to 1023
     */

    analog_value = analogRead(pin_number);
    Serial.print('A');
    Serial.print(pin_number);
    Serial.print(':');
    Serial.println(analog_value); // Adds a trailing \n
}

void digital_write(int pin_number, int digital_value){
    /*
     * Performs a digital write on pin_number with the digital_value
     * The value must be 1 or 0
     */
	digitalWrite(pin_number, digital_value);
}

void analog_write(int pin_number, int analog_value){
    /*
	 * Performs an analog write on pin_number with the analog_value
	 * The value must be range from 0 to 255
     */
	analogWrite(pin_number, analog_value);
}

void setup() {
    Serial.begin(115200); // Serial Port at 9600 baud
    Serial.setTimeout(10); // Instead of the default 1000ms, in order
                            // to speed up the Serial.parseInt() 

    pinMode(10, OUTPUT);
    pinMode(11, OUTPUT);
    pinMode(12, OUTPUT);
    pinMode(13, OUTPUT);
    pinMode(2, INPUT);
    pinMode(3, INPUT);
    M0.attach(4);
    M1.attach(7);
    M2.attach(8);
    delay(30);
    M0.write(pos0);
    M1.write(pos1);
    M2.write(pos2);   
}

void loop() {
    // Check if characters available in the buffer
    if (Serial.available() > 0) {
        operation = Serial.read();
        delay(wait_for_transmission); // If not delayed, second character is not correctly read
        mode = Serial.read();
        pin_number = Serial.parseInt(); // Waits for an int to be transmitted
        if (Serial.read()==':'){
            value_to_write = Serial.parseInt(); // Collects the value to be written
        }
        switch (operation){
            case 'R': // Read operation, e.g. RD12, RA4
                if (mode == 'D'){ // Digital read
                    digital_read(pin_number);
                } else if (mode == 'A'){ // Analog read
                    analog_read(pin_number);
				} else {
					break; // Unexpected mode
				}
                break;

            case 'W': // Write operation, e.g. WD3:1, WA8:255
                if (mode == 'D'){ // Digital write
                    digital_write(pin_number, value_to_write);
                } else if (mode == 'A'){ // Analog write
                    analog_write(pin_number, value_to_write);
                } else if (mode == 'P'){ // pwm0
                    pos0 = value_to_write;
                    M0.write(pos0);
                } else if (mode == 'Q'){ // pwm1
                    pos1 = value_to_write;
                    M1.write(pos1);
                } else if (mode == 'R'){ // pwm2
                    pos2 = value_to_write;
                    M2.write(pos2);
                } else {
                    break; // Unexpected mode
                }
                break;

            case 'M': // Pin mode, e.g. MI3, MO3, MP3
                set_pin_mode(pin_number, mode); // Mode contains I, O or P (INPUT, OUTPUT or PULLUP_INPUT)
                break;

            default: // Unexpected char
                break;
        }
    }
}