#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

// packet structure for InvenSense teapot demo
// 0x12 - START
// 0x13 - END
// 0x7D - escape character
//uint8_t teapotPacket[14] = { 0x12, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', 0x13 };
const int DEFAULT_OUTGOING_PACKET_SIZE = 30;
uint8_t dataPacket[DEFAULT_OUTGOING_PACKET_SIZE * 2];

bool requestToSend = false;

////// Reading Incoming SerialPort Related Varables /////////
const int DEFAULT_INCOMING_PACKET_SIZE = 12;

bool previouslyFoundSTART = false;
bool previouslyFoundESCAPE = false;
bool previouslyFoundEND = false;

int incomingPacketByteIndex = 0;
uint8_t incomingPacketArrayBytes[DEFAULT_INCOMING_PACKET_SIZE];

#define INCOMING_PACKET_START_INDEX 0
#define INCOMING_PACKET_END_INDEX (DEFAULT_INCOMING_PACKET_SIZE-1)
#define INCOMING_PACKET_COMMAND_INDEX 1
//////////////////////////////////////////////////////////////

const unsigned long BAUD_RATE = 250000;//115200;

int status_D5;
int status_D6;
int status_D7;
int status_D8;
int status_D9;
int status_D10;
int status_D11;
int status_D12;
int status_D13;

int status_D22;

//int a0_value;

void setup() {
    // initialize serial communication
    Serial.begin(BAUD_RATE);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately
    while (Serial.available() && Serial.read());
    delay(1000);

    // configure LED for output
    pinMode(15, OUTPUT);
    //pinMode(16, OUTPUT);
    //pinMode(17, OUTPUT);
    //pinMode(LED_PIN, OUTPUT);

    pinMode(5, INPUT_PULLUP);
    pinMode(6, INPUT_PULLUP);
    pinMode(7, INPUT_PULLUP);
    pinMode(8, INPUT_PULLUP);
    pinMode(9, INPUT_PULLUP);
    pinMode(10, INPUT_PULLUP);
    pinMode(11, INPUT_PULLUP);
    pinMode(12, INPUT_PULLUP);
    pinMode(13, INPUT_PULLUP);

    pinMode(22, INPUT_PULLUP);

    //pinMode(A0, INPUT);
}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {

    FillDataPacket();
    delay(10);
}

void serialEvent() {
    ////////////////////////////////////////////////////////////////////////////////////
    while (Serial.available()) {
        int b = Serial.read();

        //check the START of a packet
        if (b == 0x12) {
            //clear the packet
            for (int i = 0; i < DEFAULT_INCOMING_PACKET_SIZE; i++) incomingPacketArrayBytes[i] = 0;

            incomingPacketArrayBytes[incomingPacketByteIndex++] = b;
            previouslyFoundSTART = true;
        }//check the ESCAPE flag
        else if (b == 0x7D && previouslyFoundSTART) {
            if (previouslyFoundESCAPE == false) {
                previouslyFoundESCAPE = true;
            }
            else {
                incomingPacketArrayBytes[incomingPacketByteIndex++] = b;
                previouslyFoundESCAPE = false;
            }
        }//check the END of a packet
        else if (b == 0x13 && previouslyFoundSTART)
        {
            if (previouslyFoundESCAPE == false) {
                incomingPacketArrayBytes[incomingPacketByteIndex++] = b;

                previouslyFoundEND = true;
                previouslyFoundSTART = false;
                //break the while loop, proceed to process the packet below
                break;
            }
            else {
                previouslyFoundESCAPE = false;
                incomingPacketArrayBytes[incomingPacketByteIndex++] = b;
            }
        }//the DATA block
        else
        {
            if (previouslyFoundESCAPE == false) {
                incomingPacketArrayBytes[incomingPacketByteIndex++] = b;
            }
            else {
                previouslyFoundESCAPE = false;
                incomingPacketArrayBytes[incomingPacketByteIndex++] = b;
            }
        }
    }
    ////////////////////////////////////////////////////////////////////////////////////

    //Processing the packet information
    if (previouslyFoundEND && incomingPacketByteIndex == DEFAULT_INCOMING_PACKET_SIZE) {
        previouslyFoundEND = false;
        incomingPacketByteIndex = 0;

        //Request to send the MPU6050 data
        if (incomingPacketArrayBytes[INCOMING_PACKET_COMMAND_INDEX] == 0x01) {

            SendPacket();

            if (incomingPacketArrayBytes[2] == 0x03) {
                if (incomingPacketArrayBytes[3] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);
                /*if (incomingPacketArrayBytes[3] & 0x02)  digitalWrite(11, HIGH); else digitalWrite(11, LOW);
                if (incomingPacketArrayBytes[3] & 0x04)  digitalWrite(12, HIGH); else digitalWrite(12, LOW);
                if (incomingPacketArrayBytes[3] & 0x08)  digitalWrite(13, HIGH); else digitalWrite(13, LOW);*/
                /*if (incomingPacketArrayBytes[4] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);
                if (incomingPacketArrayBytes[5] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);
                if (incomingPacketArrayBytes[6] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);
                if (incomingPacketArrayBytes[7] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);
                if (incomingPacketArrayBytes[8] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);
                if (incomingPacketArrayBytes[9] & 0x01) digitalWrite(15, HIGH); else digitalWrite(15, LOW);*/
            }

        }
        else if (incomingPacketArrayBytes[INCOMING_PACKET_COMMAND_INDEX] == 0x02) {


        }
    }
    else {
        //Something wrong, the packet is incomplete, discard it (ignore it).
        previouslyFoundSTART = false;
        previouslyFoundESCAPE = false;
        previouslyFoundEND = false;
        incomingPacketByteIndex = 0;
    }
}

void FillDataPacket() {

    dataPacket[2] = 0;
    dataPacket[3] = 0;
    dataPacket[4] = 0;
    dataPacket[5] = 0;
    dataPacket[6] = 0;
    dataPacket[7] = 0;
    dataPacket[8] = 0;
    dataPacket[9] = 0;

    dataPacket[10] = 0;

    dataPacket[11] = 0;
    dataPacket[12] = 0;
    dataPacket[13] = 0;
    dataPacket[14] = 0;
    dataPacket[15] = 0;
    dataPacket[16] = 0;

    dataPacket[17] = 0;
    dataPacket[18] = 0;
    dataPacket[19] = 0;
    dataPacket[20] = 0;
    dataPacket[21] = 0;
    dataPacket[22] = 0;

    /////////////////////////////////////////////////////
    uint8_t digitalPinStatus = 0x00;
    uint8_t digitalPinStatus_2 = 0x00;

    status_D5 = digitalRead(5); //bit 0
    status_D6 = digitalRead(6); //bit 1
    status_D7 = digitalRead(7); //bit 2
    status_D8 = digitalRead(8); //bit 3
    status_D9 = digitalRead(9); //bit 4
    status_D10 = digitalRead(10); //bit 5
    status_D11 = digitalRead(11); //bit 6
    status_D12 = digitalRead(12); //bit 7
    status_D13 = digitalRead(13); //bit 1_2

    status_D22 = digitalRead(22); //bit 2_2


    digitalPinStatus = status_D5 == HIGH ? digitalPinStatus & B11111110 : digitalPinStatus | B00000001;
    digitalPinStatus = status_D6 == HIGH ? digitalPinStatus & B11111101 : digitalPinStatus | B00000010;
    digitalPinStatus = status_D7 == HIGH ? digitalPinStatus & B11111011 : digitalPinStatus | B00000100;
    digitalPinStatus = status_D8 == HIGH ? digitalPinStatus & B11110111 : digitalPinStatus | B00001000;
    digitalPinStatus = status_D9 == HIGH ? digitalPinStatus & B11101111 : digitalPinStatus | B00010000;
    digitalPinStatus = status_D10 == HIGH ? digitalPinStatus & B11011111 : digitalPinStatus | B00100000;
    digitalPinStatus = status_D11 == HIGH ? digitalPinStatus & B10111111 : digitalPinStatus | B01000000;
    digitalPinStatus = status_D12 == HIGH ? digitalPinStatus & B01111111 : digitalPinStatus | B10000000;
    digitalPinStatus_2 = status_D13 == HIGH ? digitalPinStatus_2 & B11111110 : digitalPinStatus_2 | B00000001;

    digitalPinStatus_2 = status_D22 == HIGH ? digitalPinStatus_2 & B11111101 : digitalPinStatus_2 | B00000010;

    dataPacket[23] = digitalPinStatus;
    dataPacket[24] = digitalPinStatus_2;
    /////////////////////////////////////////////////////

    /////////////////////////////////////////////////////
    //a0_value = analogRead(A0);
    //dataPacket[24] = (uint8_t)(a0_value >> 8);
    //dataPacket[25] = (uint8_t)(a0_value);
    /////////////////////////////////////////////////////
}

void SendPacket() {
    //////////////////////////////////////////////////////////////////////////////////////////
    //////////////// SET the packet values ////////////////
    //Set the START byte
    dataPacket[0] = 0x12;
    //Set the END byte
    dataPacket[DEFAULT_OUTGOING_PACKET_SIZE - 1] = 0x13;

    ///////// Assign packet data values ///////////////
    dataPacket[1] = DEFAULT_OUTGOING_PACKET_SIZE;
    FillDataPacket();
    ////////////////////////////////////////////////////////


    ///////// Check special character inside the data blocks ///////////////
    //Check the array's data elements if it contains the START, END, or ESCAPE
    int addMoreBytes = 0;

    for (int i = 1; i < DEFAULT_OUTGOING_PACKET_SIZE - 1; i++) {
        if (dataPacket[i] == 0x12 || dataPacket[i] == 0x13 || dataPacket[i] == 0x7D) {
            addMoreBytes++;
        }
    }
    //If the array content contains the special value i.e., START, END, or ESCAPE.
    //It has to be escaped.
    int newPacketSize = DEFAULT_OUTGOING_PACKET_SIZE + addMoreBytes;
    if (addMoreBytes > 0) {
        //Copy dataPacket array values to tmp array
        uint8_t tmpDataPacket[DEFAULT_OUTGOING_PACKET_SIZE];
        for (int i = 0; i < DEFAULT_OUTGOING_PACKET_SIZE; i++) {
            tmpDataPacket[i] = dataPacket[i];
        }

        dataPacket[0] = 0x12; //START
                              //dataPacket[newPacketSize - 1] = 0x13; //END
        dataPacket[1] = newPacketSize; // packet size data block
        int newPacketIndex = 1;
        for (int i = 1; i < DEFAULT_OUTGOING_PACKET_SIZE - 1; i++)
        {
            if (tmpDataPacket[i] == 0x12 || tmpDataPacket[i] == 0x13 || tmpDataPacket[i] == 0x7D) {
                dataPacket[newPacketIndex] = 0x7D;
                dataPacket[newPacketIndex + 1] = tmpDataPacket[i];
                newPacketIndex += 2;
            }
            else {
                dataPacket[newPacketIndex++] = tmpDataPacket[i];
            }
        }
        dataPacket[newPacketIndex] = 0x13;
    }
    //////////////////////////////////////////////////////////////////

    //Send data via Serial port
    Serial.write(dataPacket, newPacketSize);
    Serial.flush();
    //////////////////////////////////////////////////////////////////////////////////////////
}