//Pin definitions
int xaccPin = 11;
int yaccPin = 12;

//Global variables
long xacc = 0;
long yacc = 0;

// serial update counters
int count = 0;
int maxCount = 200;

//encoder 0
int encoder0PinA = 2;
int encoder0PinANow = LOW;
int encoder0PinAPrev = LOW;
int encoder0PinB = 3;
int encoder0PinBNow = LOW;
int encoder0PinBPrev = LOW;
int encoder0Pos = 0;
int switchPin0 = 1;
int switch0 = 0;

//encoder 1
int encoder1PinA = 4;
int encoder1PinANow = LOW;
int encoder1PinAPrev = LOW;
int encoder1PinB = 5;
int encoder1PinBNow = LOW;
int encoder1PinBPrev = LOW;
int encoder1Pos = 0;
int switchPin1 = 2;
int switch1 = 0;

//encoder 2
int encoder2PinA = 6;
int encoder2PinANow = LOW;
int encoder2PinAPrev = LOW;
int encoder2PinB = 8;
int encoder2PinBNow = LOW;
int encoder2PinBPrev = LOW;
int encoder2Pos = 0;
int switchPin2 = 3;
int switch2 = 0;

//encoder 3
int encoder3PinA = 9;
int encoder3PinANow = LOW;
int encoder3PinAPrev = LOW;
int encoder3PinB = 10;
int encoder3PinBNow = LOW;
int encoder3PinBPrev = LOW;
int encoder3Pos = 0;
int switchPin3 = 4;
int switch3 = 0;

void setup() { 


  //accelerometer
  pinMode(xaccPin, INPUT);
  pinMode(yaccPin, INPUT);

  //encoder0
  pinMode (encoder0PinA,INPUT);
  digitalWrite(encoder0PinA, HIGH);       // turn on pullup resistor
  pinMode (encoder0PinB,INPUT);
  digitalWrite(encoder0PinB, HIGH);       // turn on pullup resistor

  //encoder1
  pinMode (encoder1PinA,INPUT);
  digitalWrite(encoder1PinA, HIGH);       // turn on pullup resistor
  pinMode (encoder1PinB,INPUT);
  digitalWrite(encoder1PinB, HIGH);       // turn on pullup resistor

  //encoder2
  pinMode (encoder2PinA,INPUT);
  digitalWrite(encoder2PinA, HIGH);       // turn on pullup resistor
  pinMode (encoder2PinB,INPUT);
  digitalWrite(encoder2PinB, HIGH);       // turn on pullup resistor

  //encoder3
  pinMode (encoder3PinA,INPUT);
  digitalWrite(encoder3PinA, HIGH);       // turn on pullup resistor
  pinMode (encoder3PinB,INPUT);
  digitalWrite(encoder3PinB, HIGH);       // turn on pullup resistor


  Serial.begin (115200);
} 

//accelerometer
long readAcceleration(int axe){
  int count = 0;
  long accel = 0;
  int value = 0;

  value = digitalRead(axe);
  while(value == HIGH) { // Loop until pin reads a low
    value = digitalRead(axe);
  }

  while(value == LOW) { // Loop until pin reads a high
    value = digitalRead(axe);
  }

  while(value == HIGH) { // Loop until pin reads a low and count
    value = digitalRead(axe);
    count = count + 1;
  }

  accel = count;

  return accel;
}


void loop() { 

  //encoders
  encoder0PinANow = digitalRead(encoder0PinA);
  encoder0PinBNow = digitalRead(encoder0PinB);

  encoder1PinANow = digitalRead(encoder1PinA);
  encoder1PinBNow = digitalRead(encoder1PinB);

  encoder2PinANow = digitalRead(encoder2PinA);
  encoder2PinBNow = digitalRead(encoder2PinB);

  encoder3PinANow = digitalRead(encoder3PinA);
  encoder3PinBNow = digitalRead(encoder3PinB);

  //encoder0   
  if ((encoder0PinAPrev == LOW) && (encoder0PinANow == HIGH)) {
    if (encoder0PinBNow == LOW) {
      encoder0Pos--;
    } 
    else {
      encoder0Pos++;
    }
  } 

  if ((encoder0PinAPrev == HIGH) && (encoder0PinANow == LOW)) {
    if (encoder0PinBNow == LOW) {
      encoder0Pos++;
    } 
    else {
      encoder0Pos--;
    }
  }    

  //encoder1   
  if ((encoder1PinAPrev == LOW) && (encoder1PinANow == HIGH)) {
    if (encoder1PinBNow == LOW) {
      encoder1Pos--;
    } 
    else {
      encoder1Pos++;
    }
  } 

  if ((encoder1PinAPrev == HIGH) && (encoder1PinANow == LOW)) {
    if (encoder1PinBNow == LOW) {
      encoder1Pos++;
    } 
    else {
      encoder1Pos--;
    }
  }    

  //encoder2   
  if ((encoder2PinAPrev == LOW) && (encoder2PinANow == HIGH)) {
    if (encoder2PinBNow == LOW) {
      encoder2Pos--;
    } 
    else {
      encoder2Pos++;
    }
  } 

  if ((encoder2PinAPrev == HIGH) && (encoder2PinANow == LOW)) {
    if (encoder2PinBNow == LOW) {
      encoder2Pos++;
    } 
    else {
      encoder2Pos--;
    }
  }    

  //encoder3   
  if ((encoder3PinAPrev == LOW) && (encoder3PinANow == HIGH)) {
    if (encoder3PinBNow == LOW) {
      encoder3Pos--;
    } 
    else {
      encoder3Pos++;
    }
  } 

  if ((encoder3PinAPrev == HIGH) && (encoder3PinANow == LOW)) {
    if (encoder3PinBNow == LOW) {
      encoder3Pos++;
    } 
    else {
      encoder3Pos--;
    }
  }    

  encoder0PinAPrev = encoder0PinANow;
  encoder0PinBPrev = encoder0PinBNow;

  encoder1PinAPrev = encoder1PinANow;
  encoder1PinBPrev = encoder1PinBNow;

  encoder2PinAPrev = encoder2PinANow;
  encoder2PinBPrev = encoder2PinBNow;

  encoder3PinAPrev = encoder3PinANow;
  encoder3PinBPrev = encoder3PinBNow;

  //switches

  if (analogRead(switchPin0)> 50){
    switch0 = 1;
  }

  if (analogRead(switchPin1)> 50){
    switch1 = 1;
  }

  if (analogRead(switchPin2)> 50){
    switch2 = 1;
  }

  if (analogRead(switchPin3)> 50){
    switch3 = 1;
  }


  count = count + 1;

  if (count>maxCount){    //update serial
    //accelerometer
    xacc = readAcceleration(xaccPin); //reads and represents acceleration X
    yacc = readAcceleration(yaccPin); //reads and represents acceleration Y

    Serial.print(xacc);
    Serial.print (",");
    Serial.print (yacc);
    Serial.print (",");
    Serial.print (encoder0Pos);
    Serial.print (",");
    Serial.print (switch0);
    Serial.print (",");
    Serial.print (encoder1Pos);
    Serial.print (",");
    Serial.print (switch1);
    Serial.print (",");
    Serial.print (encoder2Pos);
    Serial.print (",");
    Serial.print (switch2);
    Serial.print (",");
    Serial.print (encoder3Pos);
    Serial.print (",");
    Serial.println (switch3);

    count = 0;
    encoder0Pos = 0;
    encoder1Pos = 0;
    encoder2Pos = 0;
    encoder3Pos = 0;
    switch0 = 0;
    switch1 = 0;
    switch2 = 0;
    switch3 = 0;
  }
}