#include <Adafruit_FreeTouch.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define PIN_LED 15
#define N_TOUCH 6

#define THRESHOLD 500

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define SCREEN_ADDRESS 0x3C // 0x3D or 0x3C depending on brand

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1, 1700000UL, 1700000UL);

// Q touch peripherals
uint8_t touch_pins[N_TOUCH] = {2, 3, 4, 5, 6, 7};
Adafruit_FreeTouch* touch_devices[N_TOUCH];

// touch state
int touch_values[N_TOUCH] = {0, 0, 0, 0, 0, 0};
bool pin_touched_now[N_TOUCH] = {false, false, false, false, false, false};
bool pin_touched_past[N_TOUCH] = {false, false, false, false, false, false};

//coordinates
int x = 28;
int y = 25;

void update_touch() {
  Ptc *ptc = ((Ptc *)PTC);

  for (int i = 0; i < N_TOUCH; i++) {
    // start
    touch_devices[i]->begin();

    // read
    touch_values[i] = touch_devices[i]->measure();

    // reset
    ptc->CTRLA.bit.ENABLE = 0;
    ptc->CTRLA.bit.SWRESET = 1;

    // update the state
    pin_touched_past[i] = pin_touched_now[i];
    pin_touched_now[i] = touch_values[i] > THRESHOLD;
  }
}

void print_touch() {
  char print_buffer[30];
  for (int i=0; i < N_TOUCH; i++) {
    sprintf(print_buffer, "%4d ", touch_values[i]);
    Serial.print(print_buffer);
  }
  Serial.println("");
}

void setup() {
  // initialize Serial port
  Serial.begin(0);

  // give the screen some time to power up
  delay(50);

  // initialize capacitive touch peripherals
  for (int i = 0; i < N_TOUCH; i++) {
    touch_devices[i] = new Adafruit_FreeTouch(touch_pins[i], OVERSAMPLE_1, RESISTOR_100K, FREQ_MODE_NONE);
  }

  // initialize LED
  pinMode(PIN_LED, OUTPUT);

  // initialize display
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.display();

  // text settings
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(x, y);
}

void loop() {
  // update the touch sensors
  update_touch();

  // example pressed button
  if (pin_touched_now[0] && !pin_touched_past[0]) {
    // button 0 was just pressed, do something
    digitalWrite(PIN_LED, HIGH);
    
    //Button 0 draws a pixed in white
    display.drawPixel(x, y, SSD1306_WHITE);
  }

  // example pressed button
  if (pin_touched_now[1] && !pin_touched_past[1]) {
    //Button 1 draws a pixed in black (removes pixel)
    display.drawPixel(x, y, SSD1306_BLACK);
  }

  // example pressed button
  if (pin_touched_now[2] && !pin_touched_past[2]) {
    //Button 2-5 moves the cursor in a direction
    y = y + 1;
    display.setCursor(x, y);
  }

  // example pressed button
  if (pin_touched_now[3] && !pin_touched_past[3]) {
    //Button 2-5 moves the cursor in a direction
    x = x - 1;
    display.setCursor(x, y);
  }

  // example pressed button
  if (pin_touched_now[4] && !pin_touched_past[4]) {
    //Button 2-5 moves the cursor in a direction
    x = x + 1;
    display.setCursor(x, y);
  }

  // example pressed button
  if (pin_touched_now[5] && !pin_touched_past[5]) {
    //Button 2-5 moves the cursor in a direction
    y = y - 1;
    display.setCursor(x, y);
  }

  // example released button
  if (!pin_touched_now[0] && pin_touched_past[0]) {
    // button 0 was just released, do something
    digitalWrite(PIN_LED, LOW);
  }

  // print values to Serial, for debugging
  print_touch();
  display.display();

  // slow down the loop to not print too fast (optional)
  delay(50);
}