/**************************************************************************************************
*
*  Wiring_SerialLib
*
*   Version:      1.1.0 - February 2009
*   Author:       Christoph Wartmann / chair for caad - ETH Zürich  /  wartmann[at]arch.ethz.ch
*                 Etienne Ribeiro    / tutorial assistant caad      /  eribeiro[at]ethz.ch
*
*   Desc:         This code must be uploaded to your wiring-board if you want to make use of the
*                 WiringLib-Library in Processing or Java. There is a catching option to perform
*                 commands as groups nearly simultanes. The Servo library has been integrated to
*                 control servo motors from your PC or MAC.
*                 However this library is beta software and needs some work to get better
*                 performance.
*
*   Protocoll:    9 bytes, last byte is CR or LF:
*                 0: 'A': Analog, 'D': Digital, 'I': ImpulesLength, 'P': Ping
*                 1: 'I': Input, 'O':Output, 'P': Ping
*                 2: Pin
*                 3: Value 1
*                 4: Value 2
*                 5: empty
*                 6: 'C': Catch
*                 7: 'H': High, 'L': Low (For Impules)
*                 8: LF (10)
*
***************************************************************************************************/




#include <Servo.h>


// Default Pin
int digitalDefaultPin = 48; // default light
int SERIAL_WAIT_MILLIS_BEFORE_BREAK = 100; // Max time to perform a command.


// Blink (for Error tracing)
int blinkPin = 7;
int busyPin = 6;


// catch Commands
int iCatch = 0;
int const countCatch = 10;                    // max catched commands
int catchedCommands[countCatch][8];           // x Commands, 8 Values






// Setup

void setup() {

        // LED (2 * blinken)
        pinMode(digitalDefaultPin, OUTPUT);
        digitalWrite(digitalDefaultPin, HIGH);
        delay(200);
        digitalWrite(digitalDefaultPin, LOW);
        delay(200);
        digitalWrite(digitalDefaultPin, HIGH);


        // Serial at 9600
        Serial.begin(9600);


        //
        pinMode(busyPin, OUTPUT);
        pinMode(blinkPin, OUTPUT);

}




// Loop

void loop() {

        // Read Serial and Execute Command:
        ReadAndExecute();


        // LED
        Blink();

}









// read and execute

void ReadAndExecute() {

        if(Serial.available() > 0) {       // if data is available to read

                //Bytes lesen
                int i = 0;
                int arr[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
                while (true) {
                        int val = doReadSerial();
                        if (val < 0) break;                            //val < 0 -> Serial delayed -> exit
                        if (i < 8) arr[i] = val;                       // store 8 bytes
                        if (i == 8 && (val == 10 || val == 13)) break; // exit when val=10 or val=13
                        if (i == 8) {                   // something went wrong
                                while (true) {                // find end
                                        int val = doReadSerial();
                                        if (val == 10 || val == 13)
                                        return;
                                }
                        }
                        i += 1;
                }

                // Catch:
                boolean doExecute = catchRecievedSerialCommand(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5], arr[6], arr[7]);
                //Execute Commands:
                if(doExecute == true) executeCatchedCommands();


                // no Catch:
                //executeCommand(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5], arr[6], arr[7]);

        }

}







// ______________________________
// Read Serial


int doReadSerial() {

        //pinMode(busyPin, OUTPUT); //Error tracing
        int i = 0;


        // wait for data:
        long startedAt = millis ();
        while(Serial.available() <= 0) {
                //Error tracing
                digitalWrite(busyPin,HIGH);
                if (startedAt + SERIAL_WAIT_MILLIS_BEFORE_BREAK < millis())
                return -1;
                i += 1;
        }
        digitalWrite(busyPin,LOW); //Error tracing


        // read:
        unsigned char charVal = Serial.read();


        // Return
        return (int)charVal;

}







// ______________________________
// Catch

boolean catchRecievedSerialCommand(int s1, int s2, int s3, int s4, int s5, int s6, int sCatch, int s8) {

        //Catch
        catchedCommands[iCatch][0] = s1;
        catchedCommands[iCatch][1] = s2;
        catchedCommands[iCatch][2] = s3;
        catchedCommands[iCatch][3] = s4;
        catchedCommands[iCatch][4] = s5;
        catchedCommands[iCatch][5] = s6;
        catchedCommands[iCatch][6] = sCatch;
        catchedCommands[iCatch][7] = s8;
        iCatch += 1;


        //Catch empty -> return true
        if(iCatch == countCatch) return true;


        //doCatch -> return false
        if((char)sCatch == 'C') return false;


        //Return (true: Befehle ausführen)
        return true;

}


// Catch Execute:

void executeCatchedCommands() {

        // 0: AnalogDigital
        // 1: InputOutput
        // 2: Pin
        // 3: Value
        // 4: empty
        // 5: empty
        // 6: doCatch
        // 7: Transition


        // Execute all catched commands:
        for (int i = 0;i < iCatch;i++) {
                executeCommand(catchedCommands[i][0], catchedCommands[i][1], catchedCommands[i][2], catchedCommands[i][3], catchedCommands[i][4], catchedCommands[i][5], catchedCommands[i][6], catchedCommands[i][7]);
        }

        iCatch = 0;

}


// Command Execute:

void executeCommand(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

        if( AD == 'A' && IO == 'O') {              // Analog and Out received
                setAnalogOutput(AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'D' && IO == 'O') {         // Digital and Out received
                setDigitalOutput(AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'A' && IO == 'I') {       // Analog and Input received
                getAnalogInput(AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'D' && IO == 'I') {       // Digital and Input received
                getDigitalInput(AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'I' && IO == 'I') {       // Impules Read
                getImpules(AD, IO, Pin, Value, b5, b6, b7, b8);    //Pin, HIGH/LOW
        }
        else if( AD == 'P' && IO == 'P') {       // Ping
                Ping(AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'S' && IO == 'A') {       // Servo.Attache
                attachServo (AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'S' && IO == 'D') {       // Servo.Detache (Detache all Servos)
                detachAllServos (AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'S' && IO == 'W') {       // Servo.Write val:0-180
                ServoWrite (AD, IO, Pin, Value, b5, b6, b7, b8);
        }
        else if( AD == 'S' && IO == 'R') {       // Servo.Read val:0-180
                ServoRead (AD, IO, Pin, Value, b5, b6, b7, b8);
        }


}








// ______________________________
// Functions:

void getDigitalInput(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

        pinMode(Pin, INPUT);
        int iValue = digitalRead(Pin);
        if(iValue == HIGH)
        iValue = 1;
        else
        iValue = 0;
        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(Pin, BYTE);
        Serial.print(iValue, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(b6, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}

void getAnalogInput(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

        int iValue = analogRead(Pin);
        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(Pin, BYTE);
        Serial.print(iValue / 4, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(b6, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}


void setAnalogOutput(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

        analogWrite(Pin, Value * 4);
        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(Pin, BYTE);
        Serial.print(Value, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(b6, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}


void setDigitalOutput(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

        pinMode(Pin, OUTPUT);


        if(Value == 1)digitalWrite(Pin,HIGH);
        if(Value == 0)digitalWrite(Pin,LOW);

        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(Pin, BYTE);
        Serial.print(Value, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(b6, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}

void getImpules(int AD, int IO, int Pin, int Value, int b5, int option, int b7, int Transition) {

        //Display_Println("getImpules");
        pinMode(busyPin, OUTPUT); //Error tracing
        digitalWrite(busyPin,HIGH);


        int ImpulesLength = 0;
        if((char)Transition == 'L'){
                pinMode(Pin, INPUT);
                ImpulesLength = pulseIn(Pin, HIGH);
                ImpulesLength = pulseIn(Pin, LOW);
        }else if((char)Transition == 'H'){
                pinMode(Pin, INPUT);
                ImpulesLength = pulseIn(Pin, LOW);
                ImpulesLength = pulseIn(Pin, HIGH);
        }
        // Option 'B' Mesure 2 times and return higher value
        if ((char)option == 'B') {
                int tmpImpulesLength = 0;
                if((char)Transition == 'L'){
                        tmpImpulesLength = pulseIn(Pin, HIGH);
                        tmpImpulesLength = pulseIn(Pin, LOW);
                }else if((char)Transition == 'H'){
                        tmpImpulesLength = pulseIn(Pin, LOW);
                        tmpImpulesLength = pulseIn(Pin, HIGH);
                }
                if (tmpImpulesLength > ImpulesLength)
                ImpulesLength = tmpImpulesLength;
        }

        digitalWrite(busyPin,LOW); //Error tracing



        int b1 = floor(ImpulesLength/256);
        int b2 = ImpulesLength - b1 * 256;
        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(Pin, BYTE);
        Serial.print(b1, BYTE);
        Serial.print(b2, BYTE);
        Serial.print(option, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(Transition, BYTE);
        Serial.print(10, BYTE);

}


void setImpules(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

}





// Servos
Servo servos[8]; // Maximal 8 Servos
int servos_pin[8];
int countServos = 0;
void attachServo (int b1, int b2, int pin, int b4, int b5, int id, int b7, int b8) {

        if (id > 7) return; // nur 8 Servos möglich

        servos_pin[id] = pin;
        servos[id].attach(pin);
        countServos += 1;
        Serial.print(b1, BYTE);
        Serial.print(b2, BYTE);
        Serial.print(pin, BYTE);
        Serial.print(b4, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(id, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}

void detachAllServos (int b1, int b2, int pin, int b4, int b5, int id, int b7, int b8) {

        for (int i = 0;i<countServos;i++) {
                servos[i].detach();
        }
        countServos = 0;

        Serial.print(b1, BYTE);
        Serial.print(b2, BYTE);
        Serial.print(pin, BYTE);
        Serial.print(b4, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(id, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}

void ServoWrite(int AD, int IO, int b3, int value, int b5, int id, int b7, int b8) {

        if (id > 7) return; // nur 8 Servos möglich

        servos[id].write(value);
        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(b3, BYTE);
        Serial.print(value, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(id, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}
void ServoRead(int AD, int IO, int b3, int b4, int b5, int id, int b7, int b8) {

        if (id > 7) return; // nur 8 Servos möglich

        int value = servos[id].read();
        Serial.print(AD, BYTE);
        Serial.print(IO, BYTE);
        Serial.print(b3, BYTE);
        Serial.print(value, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(id, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}






//______________________________
//Blink + Ping
long previousTime = 0;
boolean boolLedOn = false;

void Blink() {

        if(boolLedOn == true){
                if((millis() - previousTime) >= 1000){
                        previousTime = millis();
                        boolLedOn = false;
                        pinMode(blinkPin, OUTPUT);
                        digitalWrite(blinkPin, HIGH);		    // turn ON the LED
                }
        }
        if(boolLedOn == false){
                if((millis() - previousTime) >= 100){
                        previousTime = millis();
                        boolLedOn = true;
                        pinMode(blinkPin, OUTPUT);
                        digitalWrite(blinkPin, LOW);		    // turn ON the LED
                }
        }

}



void Ping(int AD, int IO, int Pin, int Value, int b5, int b6, int b7, int b8) {

        Serial.print('P', BYTE);
        Serial.print('P', BYTE);
        Serial.print(0, BYTE);
        Serial.print(7, BYTE);
        Serial.print(b5, BYTE);
        Serial.print(b6, BYTE);
        Serial.print(b7, BYTE);
        Serial.print(b8, BYTE);
        Serial.print(10, BYTE);

}