#include "Adafruit_FreeTouch.h"
#define QTOUCH_OVERSAMPLE 100
#define NUM_BOARD 2
#define THRESH_ONE 90000
#define THRESH_TWO 500

// Servo myservo;

Adafruit_FreeTouch qt[2] = {
  Adafruit_FreeTouch(PIN_A0, OVERSAMPLE_64, RESISTOR_50K, FREQ_MODE_NONE),
  Adafruit_FreeTouch(PIN_A1, OVERSAMPLE_64, RESISTOR_50K, FREQ_MODE_NONE)
};

int red_leds[] = {3, 6, 9, 2, 5, 8, 1, 4, 7};


void setup(){
  qt[0].begin();
  qt[1].begin();
  // myservo.attach(2);
  pinMode(D2, OUTPUT);
  pinMode(D3, OUTPUT);
  pinMode(D4, OUTPUT);
  pinMode(D5, OUTPUT);
  pinMode(D6, OUTPUT);
  pinMode(D7, OUTPUT);
  pinMode(D8, OUTPUT);
  pinMode(D9, OUTPUT);
  pinMode(D10, OUTPUT);
}

void reset(){
  digitalWrite(D2, LOW);
  digitalWrite(D3, LOW);
  digitalWrite(D4, LOW);
  digitalWrite(D5, LOW);
  digitalWrite(D6, LOW);
  digitalWrite(D7, LOW);
  digitalWrite(D8, LOW);
  digitalWrite(D9, LOW);
  digitalWrite(D10, LOW);
}

void red(){
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED3
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED6
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED9
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED2
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED5
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED8
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED1
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED4
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED7
  reset();
}

void blue(){
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED3
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED6
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  delay(500); //LED9
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED2
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED5
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  delay(500); //LED8
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED1
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED4
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  delay(500); //LED7
  reset();
}

void purple(){
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED3
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED6
  reset();
  digitalWrite(D4, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  delay(500); //LED9
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED2
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED5
  reset();
  digitalWrite(D3, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  delay(500); //LED8
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED1
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D9, HIGH);
  digitalWrite(D10, HIGH);
  delay(500); //LED4
  reset();
  digitalWrite(D2, HIGH);
  digitalWrite(D5, HIGH);
  digitalWrite(D6, HIGH);
  digitalWrite(D7, HIGH);
  digitalWrite(D8, HIGH);
  delay(500); //LED7
  reset();
}

void loop() {
  long int qt_value_sum_1 = 0;
  long int qt_value_sum_2 = 0;
  for (int i = 0; i < QTOUCH_OVERSAMPLE; i++) {
    qt_value_sum_1 += qt[0].measure();
    qt_value_sum_2 += qt[1].measure();
  }

  reset();
  if (qt_value_sum_1 > THRESH_ONE && qt_value_sum_2 < THRESH_ONE) {
    red();
  } else if (qt_value_sum_1 < THRESH_ONE && qt_value_sum_2 > THRESH_ONE) {
    blue();
  } else if (abs(qt_value_sum_1 - qt_value_sum_2) < THRESH_TWO) {
    purple();
  }

  Serial.print(qt_value_sum_1);
  Serial.print(", ");
  Serial.println(qt_value_sum_2);
  delay(500);

}