/*
   Firmware para Arduino UNO del proyecto COBRA Entrenador de Tiempo de Reacción
   descrito en:
   https://roble.uno/cobra
   Puedes comprar tu kit en:
   https://roble.uno/product/cobra
   bajo Licencia Creative Commons Atribución-CompartirIgual 4.0 Internacional.
   https://creativecommons.org/licenses/by-sa/4.0/
   12 de junio de 2023
   por Angel Espeso
   ESTUDIO ROBLE
   https://roble.uno/
*/
#include "pins_arduino.h" // Arduino pre-1.0 needs this
#include <Adafruit_NeoPixel.h>
#include <Wire.h>
#include <hd44780.h>  // main hd44780 header
#include <hd44780ioClass/hd44780_I2Cexp.h>  // i2c expander i/o class header
Adafruit_NeoPixel strip = Adafruit_NeoPixel(24, 3, NEO_GRB + NEO_KHZ800);
uint32_t off = strip.Color(0, 0, 0);
uint32_t red = strip.Color(255, 0, 0);
uint32_t redSoft = strip.Color(25, 0, 0);
uint32_t green = strip.Color(0, 255, 0);
uint32_t greenSoft = strip.Color(0, 25, 0);
uint32_t blue = strip.Color(0, 0, 255);
uint32_t blueSoft = strip.Color(0, 0, 60);
uint32_t glow = strip.Color(1, 1, 1);
uint32_t magenta = strip.Color(255, 0, 255);
hd44780_I2Cexp lcd; // declare lcd object: auto locate & config exapander chip
const int LCD_COLS = 20;
const int LCD_ROWS = 4;
#define pinTono 2 //2
int capPin [16];
int prevState [16];
int threshold = 5;
int pads = 7;
int pin[] = {8, 7, 9, 11, 13, 12, 10}; // Índice pixel, da pad
//           0  1  2  3    4   5   6
int neoPix [] = {99, 99, 99, 99, 99, 99, 99, 1, 0, 2, 6, 3, 5, 4}; // Índice pad, da nº de neo Pixel. 99 para pines no ocupados
//                0   1   2   3   4   5  6   7  8  9 10 11, 12,13
int machineState = 0;
int rep;
int reps = 10;
unsigned long start;
unsigned long reactionTime;
unsigned long wait;
unsigned long waitLimit = 2000; // No mayor de 9999 para que no se descuadre el LCD
unsigned long maximun = 0;
unsigned long sum;
unsigned long average;
unsigned long minimun = 10000;
int pixel; // pixel RND
int pixel0; // pixel RND que salio antes
int greenDecision; // Estimulo Rojo (0) o Verde (1)
int greenCount;
int fails;
char myString[10];
int posCursor = 0;
int posCursor0;
uint8_t readCapacitivePin(int pinToMeasure) {
  // Variables used to translate from Arduino to AVR pin naming
  volatile uint8_t* port;
  volatile uint8_t* ddr;
  volatile uint8_t* pin;
  // Here we translate the input pin number from
  //  Arduino pin number to the AVR PORT, PIN, DDR,
  //  and which bit of those registers we care about.
  byte bitmask;
  port = portOutputRegister(digitalPinToPort(pinToMeasure));
  ddr = portModeRegister(digitalPinToPort(pinToMeasure));
  bitmask = digitalPinToBitMask(pinToMeasure);
  pin = portInputRegister(digitalPinToPort(pinToMeasure));
  // Discharge the pin first by setting it low and output
  *port &= ~(bitmask);
  *ddr  |= bitmask;
  delay(1);
  uint8_t SREG_old = SREG; //back up the AVR Status Register
  // Prevent the timer IRQ from disturbing our measurement
  noInterrupts();
  // Make the pin an input with the internal pull-up on
  *ddr &= ~(bitmask);
  *port |= bitmask;
  // Now see how long the pin to get pulled up. This manual unrolling of the loop
  // decreases the number of hardware cycles between each read of the pin,
  // thus increasing sensitivity.
  uint8_t cycles = 17;
  if (*pin & bitmask) {
    cycles =  0;
  }
  else if (*pin & bitmask) {
    cycles =  1;
  }
  else if (*pin & bitmask) {
    cycles =  2;
  }
  else if (*pin & bitmask) {
    cycles =  3;
  }
  else if (*pin & bitmask) {
    cycles =  4;
  }
  else if (*pin & bitmask) {
    cycles =  5;
  }
  else if (*pin & bitmask) {
    cycles =  6;
  }
  else if (*pin & bitmask) {
    cycles =  7;
  }
  else if (*pin & bitmask) {
    cycles =  8;
  }
  else if (*pin & bitmask) {
    cycles =  9;
  }
  else if (*pin & bitmask) {
    cycles = 10;
  }
  else if (*pin & bitmask) {
    cycles = 11;
  }
  else if (*pin & bitmask) {
    cycles = 12;
  }
  else if (*pin & bitmask) {
    cycles = 13;
  }
  else if (*pin & bitmask) {
    cycles = 14;
  }
  else if (*pin & bitmask) {
    cycles = 15;
  }
  else if (*pin & bitmask) {
    cycles = 16;
  }
  // End of timing-critical section; turn interrupts back on if they were on before, or leave them off if they were off before
  SREG = SREG_old;
  // Discharge the pin again by setting it low and output
  //  It's important to leave the pins low if you want to
  //  be able to touch more than 1 sensor at a time - if
  //  the sensor is left pulled high, when you touch
  //  two sensors, your body will transfer the charge between
  //  sensors.
  *port &= ~(bitmask);
  *ddr  |= bitmask;
  return cycles;
}
int pixelRandom() { // pixel RND no repetido
  while (pixel == pixel0) {
    pixel = random(0, pads);
  }
  pixel0 = pixel;
  return pixel;
}
void countDown() {
  strip.fill(off, 0, 7);
  strip.show();
  unsigned long vel = 500;
  lcd.clear();
  lcd.setCursor(5, 1);
  lcd.print("3, ");
  delay(vel);
  lcd.print("2, ");
  delay(vel);
  lcd.print("1...");
  delay(vel);
}
void showLCD() { // Durante el set
  lcd.setCursor(0, 0);
  lcd.print("#");
  lcd.print(rep);
  lcd.print("/");
  lcd.print(reps);
  if (machineState == 3) {
    lcd.setCursor(9, 0);
    lcd.print("F:");
    lcd.print(fails);
  }
  if (reactionTime < waitLimit && greenDecision == 1) {
    dtostrf(reactionTime / 1000.0, 4, 2, myString);
    lcd.setCursor(19, 0);
    lcd.print("s");
    lcd.setCursor(14, 0);
    lcd.print(myString);
  } else {
    lcd.setCursor(14, 0);
    lcd.print("      ");
  }
  lcd.setCursor(0, 1);
  lcd.print("Maximo: ");
  dtostrf(maximun / 1000.0, 4, 2, myString);
  lcd.print(myString);
  lcd.print(" s");
  lcd.setCursor(0, 2);
  lcd.print("Media : ");
  dtostrf(average / 1000.0, 4, 2, myString);
  lcd.print(myString);
  lcd.print(" s");
  lcd.setCursor(0, 3);
  lcd.print("Minimo: ");
  if (minimun == 10000) {
    lcd.print("0.00");
  } else {
    dtostrf(minimun / 1000.0, 4, 2, myString);
    lcd.print(myString);
  }
  lcd.print(" s");
}
void showStats() {
  lcd.clear();
  if (machineState == 3) {    //decision
    lcd.setCursor(0, 0);
    lcd.print("                    ");
    lcd.setCursor(0, 0);
    lcd.print("#");
    lcd.print(reps);
    lcd.print("         ");
    lcd.setCursor(10, 0);
    lcd.print("Fallos: ");
    lcd.print(fails);
  } else {
    lcd.setCursor(0, 0);
    lcd.print("#");
    lcd.print(reps);
  }
  lcd.setCursor(0, 1);
  lcd.print("Maximo: ");
  dtostrf(maximun / 1000.0, 4, 2, myString);
  lcd.print(myString);
  lcd.print(" s");
  lcd.setCursor(0, 2);
  lcd.print("Media : ");
  dtostrf(average / 1000.0, 4, 2, myString);
  lcd.print(myString);
  lcd.print(" s");
  lcd.setCursor(0, 3);
  lcd.print("Minimo: ");
  if (minimun == 10000) {
    lcd.print("0.00");
  } else {
    dtostrf(minimun / 1000.0, 4, 2, myString);
    lcd.print(myString);
  }
  lcd.print(" s");
}
void abortLCD() {
  strip.fill(off, 0, 7);
  strip.show();
  lcd.clear();
  lcd.setCursor(3, 1);
  lcd.print("Serie Abortada");
  lcd.setCursor(3, 2);
  lcd.print("2s sin pulsar.");
  delay(2000);
}
void beep() {
  tone(pinTono, 2000, 100);
}
void beepSad() {
  tone(pinTono, 50, 100);
}
void beepHappy() {
  tone(pinTono, 2500, 100);
}
void putDecisionPixel() {
  strip.fill(off, 0, 7);
  if (random(100) > 60 && rep > 1) { // El primer estimulo verde para evitar que salgan todos rojos y no calculo de la media sea division por 0
    greenDecision = 0; // rojo
  } else {
    greenDecision = 1; // verde
  }
  if ( greenDecision == 0) {
    strip.setPixelColor(pixel, 255, 0, 0);
  } else {
    strip.setPixelColor(pixel, 0, 255, 0);
    greenCount ++;
  }
  strip.show();
}
void takeStats() {
  maximun = max(maximun, reactionTime);
  minimun = min(minimun, reactionTime);
  if (machineState == 3) {
    average = sum / greenCount;
  } else {
    sum = sum + reactionTime;
    average = sum / rep;
  }
}
void navigationPixels() {
  strip.fill(off, 0, 7);
  strip.setPixelColor(0, redSoft);
  strip.setPixelColor(1, blueSoft);
  strip.setPixelColor(2, greenSoft);
  for (int i = 3; i < 8; i++) {
    strip.setPixelColor(i, glow);
  }
  strip.show();
}
void showMenu() {
  lcd.clear();
  lcd.setCursor(5, 0);
  lcd.print("Reaccion");
  lcd.setCursor(5, 1);
  lcd.print("Continuo");
  lcd.setCursor(5, 2);
  lcd.print("Decision");
  lcd.setCursor(5, 3);
  lcd.print("Configuracion");
}
void showCursor() {
  for (int i = 0; i < 4; i++) {
    lcd.setCursor(1, i);
    lcd.print("  ");
  }
  lcd.setCursor(1, posCursor);
  lcd.print("=>");
}
void setup() {
  Serial.begin(115200);
  pinMode(pinTono, OUTPUT);
  pinMode(3, OUTPUT);
  strip.begin();
  strip.fill(redSoft, 0, 7);
  strip.setBrightness(255);
  strip.show();
  randomSeed(analogRead(0));
  int status = lcd.begin(LCD_COLS, LCD_ROWS);
  lcd.noLineWrap();
  lcd.clear();
  lcd.setCursor(0, 0);
  delay(100); //Si no, no dibuja de una tirada la siguiente línea
  lcd.print("====== COBRA =======");
  delay(500);
  lcd.setCursor(0, 2);
  lcd.print("What gets measured,");
  delay(500);
  lcd.setCursor(5, 3);
  lcd.print("gets improved.");
  delay(2000);
}
void loop() {
  while (machineState == 0) { // init MENU
    navigationPixels();
    showMenu();
    posCursor = 0;
    posCursor0 = posCursor;
    showCursor();
    machineState = 8;
  }
  while (machineState == 8) { // MENU
    capPin[0] = readCapacitivePin(pin[0]);
    if (capPin[0] > threshold) {
      strip.setPixelColor(0, red);
      strip.show();
      if (prevState[0] == 0) {
        prevState[0] = 1;
        beep();
        delay(100);
        posCursor--;
      }
    } else {
      if (prevState[0] == 1) {
        prevState[0] = 0;
        strip.setPixelColor(0, redSoft);
        strip.show();
      }
    }
    capPin[1] = readCapacitivePin(pin[1]);
    if (capPin[1] > threshold) {
      strip.setPixelColor(1, blue);
      strip.show();
      if (prevState[1] == 0) {
        prevState[1] = 1;
        beep();
        delay(100);
        machineState = posCursor + 1;
      }
    } else {
      if (prevState[1] == 1) {
        prevState[1] = 0;
        strip.setPixelColor(1, blueSoft);
        strip.show();
      }
    }
    capPin[2] = readCapacitivePin(pin[2]);
    if (capPin[2] > threshold) {
      strip.setPixelColor(2, green);
      strip.show();
      if (prevState[2] == 0) {
        prevState[2] = 1;
        beep();
        delay(100);
        posCursor++;
      }
    } else {
      if (prevState[2] == 1) {
        prevState[2] = 0;
        strip.setPixelColor(2, greenSoft);
        strip.show();
      }
    }
    if (posCursor != posCursor0) {
      if (posCursor == 4) {
        posCursor = 0;
      }
      if (posCursor == -1) {
        posCursor = 3;
      }
      Serial.println(posCursor);
      posCursor0 = posCursor;
      showCursor();
    }
  } // (machineState == 8)
  while (machineState == 1) { // Reacción
    maximun = 0;
    minimun = 10000;
    sum = 0;
    average = 0;
    countDown();
    for (rep = 1; rep <= reps; rep++) {
      showLCD();
      wait = random(300, 1500);
      delay(wait);
      pixel = pixelRandom();
      strip.fill(off, 0, 7);
      strip.setPixelColor(pixel, red);
      strip.show();
      start = millis();
      beep();
      capPin[pixel] = readCapacitivePin(pin[pixel]);
      while (capPin[pixel] < threshold) {
        capPin[pixel] = readCapacitivePin(pin[pixel]);
        if ((millis() - start) > waitLimit) {
          abortLCD();
          rep = reps;
          machineState = 0;
          break;
        }
      }
      if (machineState != 0) {
        reactionTime = millis() - start;
        beepHappy();
        strip.fill(off, 0, 7);
        strip.show();
        takeStats();
        showStats();
      }
    }
    if (machineState != 0) {
      strip.fill(greenSoft, 0, 7);
      strip.show();
      delay(1000);
      strip.fill(redSoft, 0, 7);
      strip.show();
      machineState = 9;
    }
  }
  while (machineState == 2) { // Continuo
    maximun = 0;
    minimun = 10000;
    sum = 0;
    average = 0;
    countDown();
    for (rep = 1; rep <= reps; rep++) {
      showLCD();
      pixel = pixelRandom();
      strip.fill(off, 0, 7);
      strip.setPixelColor(pixel, red);
      strip.show();
      start = millis();
      beep();
      capPin[pixel] = readCapacitivePin(pin[pixel]);
      while (capPin[pixel] < threshold) {
        capPin[pixel] = readCapacitivePin(pin[pixel]);
        if ((millis() - start) > waitLimit) {
          rep = reps;
          machineState = 0;
          break;
        }
      }
      reactionTime = millis() - start;
      strip.fill(off, 0, 7);
      strip.show();
      takeStats();
      showStats();
    }
    if (machineState != 0) {
      beep();
      strip.fill(greenSoft, 0, 7);
      strip.show();
      delay(1000);
      strip.fill(redSoft, 0, 7);
      strip.show();
      machineState = 9;
    }
  }
  while (machineState == 3) { // Decision
    minimun = 10000;
    maximun = 0;
    sum = 0;
    average = 0;
    greenCount = 0;
    fails = 0;
    countDown();
    for (rep = 1; rep <= reps; rep++) {
      showLCD();
      pixel = pixelRandom();
      putDecisionPixel();
      start = millis();
      capPin[pixel] = readCapacitivePin(pin[pixel]);
      while (capPin[pixel] < threshold &&
             (millis() - start) < waitLimit) {
        capPin[pixel] = readCapacitivePin(pin[pixel]);
      }
      reactionTime = millis() - start;
      if (greenDecision == 1) {
        if (reactionTime >= waitLimit) { // Abortamos por espera
          abortLCD();
          rep = reps;
          machineState = 0;
        } else { // Pulsación correcta
          beepHappy();
          sum = sum + reactionTime;
          takeStats();
          showLCD();
        }
      }
      if (greenDecision == 0) {
        if (reactionTime >= waitLimit) { // Espera correcta
          beepHappy();
          showLCD();
        } else { // Pulsación incorrecta
          beepSad();
          fails ++;
        }
      }
    }
    if (machineState != 9) {
      showStats();
    }
    if (machineState != 0) {
      strip.fill(greenSoft, 0, 7);
      strip.show();
      delay(1000);
      strip.fill(redSoft, 0, 7);
      strip.show();
      machineState = 9;
    }
  }
  while (machineState == 4) { // init Configuración
    lcd.clear();
    lcd.setCursor(1, 2);
    lcd.print("Repeticiones: ");
    lcd.print(reps);
    machineState = 5;
  }
  while (machineState == 5) { // Configuración
    capPin[0] = readCapacitivePin(pin[0]);
    if (capPin[0] > threshold) {
      strip.setPixelColor(0, red);
      strip.show();
      if (prevState[0] == 0) {
        prevState[0] = 1;
        beep();
        delay(100);
        if (reps == 99) {
          reps = 1;
        } else {
          reps++;
        }
        lcd.setCursor(15, 2);
        lcd.print(reps);
        lcd.print(" ");
      }
    } else {
      if (prevState[0] == 1) {
        prevState[0] = 0;
        strip.setPixelColor(0, redSoft);
        strip.show();
      }
    }
    capPin[1] = readCapacitivePin(pin[1]);
    if (capPin[1] > threshold) {
      strip.setPixelColor(1, blue);
      strip.show();
      if (prevState[1] == 0) {
        prevState[1] = 1;
        beep();
        delay(100);
        machineState = 0;
      }
    } else {
      if (prevState[1] == 1) {
        prevState[1] = 0;
        strip.setPixelColor(1, blueSoft);
        strip.show();
      }
    }
    capPin[2] = readCapacitivePin(pin[2]);
    if (capPin[2] > threshold) {
      strip.setPixelColor(2, green);
      strip.show();
      if (prevState[2] == 0) {
        prevState[2] = 1;
        beep();
        delay(100);
        if (reps == 1) {
          reps = 99;
        } else {
          reps--;
        }
        lcd.setCursor(15, 2);
        lcd.print(reps);
        lcd.print(" ");
      }
    } else {
      if (prevState[2] == 1) {
        prevState[2] = 0;
        strip.setPixelColor(2, greenSoft);
        strip.show();
      }
    }
  }
  while (machineState == 9) { // Ended
    for (int i = 0; i < 7; i++) {
      capPin[i] = readCapacitivePin(pin[i]);
      if (capPin[i] > threshold) {
        machineState = posCursor + 1;
        break;
      }
    }
  }
}