// DEBUG Const
#define DEBUG true  //set to true for debug output, false for no debug output
#define DEBUG_SERIAL \
  if (DEBUG) Serial

// Add HW target (eventually boards installed)

// Add External Library
// Adafruit Neopixel
#include <Adafruit_NeoPixel.h>
// Display SSD1306
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Add Internal Library
#include "BTN.h"
#include "Splash.h"

// Pin assignment
#define PIN_WS2812B 21
#define PIN_BTN1 13
#define PIN_BTN2 19
#define PIN_SCL1 26
#define PIN_SDA1 25
#define PIN_SCL2 18
#define PIN_SDA2 5
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)

// HW Constants
#define NUM_PIXELS 12     // Please EVEN number the number of LEDs (pixels) on WS2812B LED strip
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

// SW Constants
#define ROPE_LENGTH 100.0 
#define SINGLE_MOVE 3.0

// HW Global variables
Adafruit_NeoPixel ws2812b(NUM_PIXELS, PIN_WS2812B, NEO_GRB + NEO_KHZ800);
MyBtn btn1(PIN_BTN1);
MyBtn btn2(PIN_BTN2);
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);
Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Cone, OLED_RESET);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Ctwo, OLED_RESET);

// SW Global variables
enum gameStateEnum {INIT_TUG_OF_WAR, START_TUG_OF_WAR, GAME_TUG_OF_WAR, WINNER_TUG_OF_WAR, FINISH_TUG_OF_WAR, INIT_LEDFALL, START_LEDFALL, GAME_LEDFALL, WINNER_LEDFALL, FINISH_LEDFALL};
enum gameStateEnum gameState = INIT_LEDFALL;
enum gameStateEnum gameStateOld = START_TUG_OF_WAR;
bool first = false;

uint32_t rgbPlay1 = 0;    // Color of player1
uint32_t rgbPlay2 = 0;    // Color of player2
bool readyPlay1 = false;  // Player1 ready to play
bool readyPlay2 = false;  // Player2 ready to play

float posCenterRope = ROPE_LENGTH/2;  // Position of the middle of the rope
bool winPlay1, winPlay2 = false;      // true when the relative player won

bool temptPlay1, temptPlay2 = false;
uint16_t scorePlay1, scorePlay2 = 0;

void displayText(uint8_t player, char* text, int n = 0) {
  char line[10];
  int16_t  x1, y1;
  uint16_t w, h;

  snprintf(line, 10, text, n);

  if (player == 1) {
    display1.clearDisplay();
    display1.setTextSize(2);
    display1.setTextColor(WHITE); // Draw white text
    display1.getTextBounds(line, 0, 0, &x1, &y1, &w, &h); //calc width of new string
    display1.setCursor((SCREEN_WIDTH - w) / 2, (SCREEN_HEIGHT - h) / 2);
    display1.print(line);
    display1.display();
  }
  else {
    display2.clearDisplay();
    display2.setTextSize(2);
    display2.setTextColor(WHITE); // Draw white text
    display2.getTextBounds(line, 0, 0, &x1, &y1, &w, &h); //calc width of new string
    display2.setCursor((SCREEN_WIDTH - w) / 2, (SCREEN_HEIGHT - h) / 2);
    display2.print(line);
    display2.display();  
  }
}

void setup() {
  Serial.begin(9600);

  DEBUG_SERIAL.println("SETUP"); 

  ws2812b.begin();
  pinMode(PIN_BTN1, INPUT_PULLUP);
  pinMode(PIN_BTN2, INPUT_PULLUP);

  I2Cone.begin(PIN_SDA1, PIN_SCL1, 100000); 
  I2Ctwo.begin(PIN_SDA2, PIN_SCL2, 100000);  

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display1.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  if(!display2.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  display1.clearDisplay();
  display2.clearDisplay();  

  randomSeed(analogRead(0));
}

void loop() {
  switch (gameState) {
    case INIT_TUG_OF_WAR:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents();
        
        readyPlay1 = readyPlay2 = false;

        display1.clearDisplay();
        display1.drawBitmap(0, 0, epd_bitmap_Splash_TugOfWar, 128, 32, 1);
        display1.display();
        display2.clearDisplay();
        display2.drawBitmap(0, 0, epd_bitmap_Splash_TugOfWar, 128, 32, 1);
        display2.display();        

        DEBUG_SERIAL.println("INIT_TUG_OF_WAR");
      }

      animInitTugOfWar();
      
      if (btn1.wasReleased()) {
        btn1.clearReleased();
        rgbPlay1 = ws2812b.getPixelColor(NUM_PIXELS-1);
        readyPlay1 = true;
        displayText(1, "Ready");
      }

      if (btn2.wasReleased()) {
        btn2.clearReleased();
        rgbPlay2 = ws2812b.getPixelColor(0);
        readyPlay2 = true;
        displayText(2, "Ready");
      }

      if (readyPlay1 && readyPlay2) {
        gameState = START_TUG_OF_WAR;
      }

      if (btn1.wasLongPressed() && btn2.wasLongPressed()) {
        btn1.clearLongPressed();
        btn2.clearLongPressed();
        gameState = INIT_LEDFALL;
      }
      
      break;
    case START_TUG_OF_WAR:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents();

        DEBUG_SERIAL.println("START_TUG_OF_WAR");
      }

      if (animStartTugOfWar()) {
        gameState = GAME_TUG_OF_WAR;
      }

      break;
    case GAME_TUG_OF_WAR:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents(); 
        
        winPlay1 = winPlay2 = false;
        posCenterRope = ROPE_LENGTH/2;

        displayText(1, "GO !");
        displayText(2, "GO !");        

        DEBUG_SERIAL.println("GAME_TUG_OF_WAR");
      }

      if (btn1.wasPressed()) {
        btn1.clearPressed();
        posCenterRope = posCenterRope + SINGLE_MOVE;
        displayText(1, "%d", (int)(ROPE_LENGTH-posCenterRope));
        displayText(2, "%d", (int)posCenterRope);
      }

      if (btn2.wasPressed()) {
        btn2.clearPressed();
        posCenterRope = posCenterRope - SINGLE_MOVE;
        displayText(1, "%d", (int)(ROPE_LENGTH-posCenterRope));
        displayText(2, "%d", (int)posCenterRope);
      }

      DEBUG_SERIAL.print("posCenterRope: ");
      DEBUG_SERIAL.println(posCenterRope);

      if (posCenterRope < 0.0 || posCenterRope > ROPE_LENGTH) {
        winPlay1 = posCenterRope > ROPE_LENGTH;
        winPlay2 = posCenterRope < 0.0;
        gameState = WINNER_TUG_OF_WAR;
      }

      colorRope();
      break;
    case WINNER_TUG_OF_WAR:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents();

        if (winPlay1) {
          displayText(1, "WIN!");
        }
        else {
          displayText(1, "LOSE!");
        }

        if (winPlay2) {
          displayText(2, "WIN!");
        }
        else {
          displayText(2, "LOSE!");
        }        

        DEBUG_SERIAL.println("WINNER_TUG_OF_WAR");
      }

      if (animWinnerTugOfWar()) {
        gameState = FINISH_TUG_OF_WAR;
      }

      break;
    case FINISH_TUG_OF_WAR:
      if (first) {
        first = false;
        ws2812b.fill(winPlay1 ? rgbPlay1 : rgbPlay2);
        ws2812b.show();
        btn1.clearAllEvents();
        btn2.clearAllEvents();

        DEBUG_SERIAL.println("FINISH_TUG_OF_WAR");
      }

      if (btn1.wasPressed() || btn2.wasPressed()) {
        btn1.clearPressed();
        btn2.clearPressed();
        gameState = INIT_TUG_OF_WAR;
      }

      break;
    case INIT_LEDFALL:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents();
        
        readyPlay1 = readyPlay2 = false;

        display1.clearDisplay();
        display1.drawBitmap(0, 0, epd_bitmap_Splash_LedFall, 128, 32, 1);
        display1.display();
        display2.clearDisplay();
        display2.drawBitmap(0, 0, epd_bitmap_Splash_LedFall, 128, 32, 1);
        display2.display();          

        DEBUG_SERIAL.println("INIT_LEDFALL");
      }

      animInitLedFall();
      
      if (btn1.wasReleased()) {
        btn1.clearReleased();
        rgbPlay1 = ws2812b.getPixelColor(NUM_PIXELS-1);
        readyPlay1 = true;
        displayText(1, "Ready");
      }

      if (btn2.wasReleased()) {
        btn2.clearReleased();
        rgbPlay2 = ws2812b.getPixelColor(0);
        readyPlay2 = true;
        displayText(2, "Ready");
      }

      if (readyPlay1 && readyPlay2) {
        gameState = START_LEDFALL;
      }

      if (btn1.wasLongPressed() && btn2.wasLongPressed()) {
        btn1.clearLongPressed();
        btn2.clearLongPressed();
        gameState = INIT_TUG_OF_WAR;
      }    
      break;
    case START_LEDFALL:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents();

        DEBUG_SERIAL.println("START_LEDFALL");
      }

      if (animStartLedFall()) {
        gameState = GAME_LEDFALL;
      }

      break;    
    case GAME_LEDFALL:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents(); 
        
        scorePlay1 = scorePlay2 = 0;
        winPlay1 = winPlay2 = false;

        displayText(1, "GO !");
        displayText(2, "GO !");        

        DEBUG_SERIAL.println("GAME_LEDFALL");
      }

      if (btn1.wasPressed()) {
        btn1.clearPressed();

        if (!temptPlay1) {
          temptPlay1 = true;
          if (ws2812b.getPixelColor(NUM_PIXELS-1) > 0) {
            scorePlay1 = scorePlay1 + 5;
          }
          else {
            if (scorePlay1 > 0) scorePlay1--;
          }

          displayText(1, "%d", (int)(scorePlay1));          

          DEBUG_SERIAL.print("SCORE PLAY1: ");
          DEBUG_SERIAL.println(scorePlay1);  
        }
      }

      if (btn2.wasPressed() && !temptPlay2) {
        btn2.clearPressed();

        if (!temptPlay2) {
          temptPlay2 = true;
          if (ws2812b.getPixelColor(0) > 0) {
            scorePlay2 = scorePlay2 + 5;
          }
          else {
            if (scorePlay2 > 0) scorePlay2--;
          }

          displayText(2, "%d", (int)(scorePlay2)); 

          DEBUG_SERIAL.print("SCORE PLAY2: ");
          DEBUG_SERIAL.println(scorePlay2);
        }     
      }

      if (animGameLedFall()) {
        winPlay1 = scorePlay1 >= scorePlay2;
        winPlay2 = scorePlay2 >= scorePlay1;
        gameState = WINNER_LEDFALL;
      }

      break;
    case WINNER_LEDFALL:
      if (first) {
        first = false;
        clearLeds();
        btn1.clearAllEvents();
        btn2.clearAllEvents();

        if (winPlay1) {
          displayText(1, "WIN!");
        }
        else {
          displayText(1, "LOSE!");
        }

        if (winPlay2) {
          displayText(2, "WIN!");
        }
        else {
          displayText(2, "LOSE!");
        }         

        DEBUG_SERIAL.println("WINNER_LEDFALL");
      }

      if (animWinnerLedFall()) {
        gameState = FINISH_LEDFALL;
      }

      break;     
    case FINISH_LEDFALL:
      if (first) {
        first = false;
        ws2812b.fill(winPlay1 ? rgbPlay1 : rgbPlay2);
        ws2812b.show();
        btn1.clearAllEvents();
        btn2.clearAllEvents();

        DEBUG_SERIAL.println("FINISH_LEDFALL");
      }

      if (btn1.wasPressed() || btn2.wasPressed()) {
        btn1.clearPressed();
        btn2.clearPressed();
        gameState = INIT_LEDFALL;
      }

      break;
    default:
      gameState = INIT_TUG_OF_WAR;
      break;
  }

  // First evaluation for first time in new state
  if (gameStateOld != gameState) {
    first = true;
    gameStateOld = gameState;
  }

  btn1.readBtn();
  btn2.readBtn();
}

bool animInitLedFall() {
  static long timerInitFade = 0;
  const uint8_t TIME_INIT_FADE_ANIM = 10;
  static long timerInit = 0;
  const uint16_t TIME_INIT_ANIM = 1000;  
  static uint16_t hue1 = 0;     // green
  static uint16_t hue2 = 21845; // red
  uint32_t color1, color2;
  const uint8_t PROB_NEW_LED = 40;
  uint32_t colorNewLed = 0;

  if ((millis() - timerInitFade) > TIME_INIT_FADE_ANIM) { 
    if (!readyPlay1) {
      hue1 += random(80, 121);
      color1 = ws2812b.gamma32(ws2812b.ColorHSV(hue1, 255, 255));
      ws2812b.setPixelColor(NUM_PIXELS-1, color1);
    }

    if (!readyPlay2) {
      hue2 += random(80, 121);
      color2 = ws2812b.gamma32(ws2812b.ColorHSV(hue2, 255, 255));
      ws2812b.setPixelColor(0, color2);
    }

    ws2812b.show();
    timerInitFade = millis();
  }

  if ((millis() - timerInit) > TIME_INIT_ANIM) {
    for (int i=NUM_PIXELS-3; i>=NUM_PIXELS/2; i--) {
      ws2812b.setPixelColor(i+1, ws2812b.getPixelColor(i));
    }

    for (int i=2; i<NUM_PIXELS/2; i++) {
      ws2812b.setPixelColor(i-1, ws2812b.getPixelColor(i));
    }

    if (random(1, 101) <= PROB_NEW_LED) {
      colorNewLed = ws2812b.gamma32(ws2812b.ColorHSV(random(1, 65535), 255, 255));
    }
    else {
      colorNewLed = 0;
    }

    ws2812b.setPixelColor(NUM_PIXELS/2, colorNewLed);
    ws2812b.setPixelColor((NUM_PIXELS/2)-1, colorNewLed);

    ws2812b.show();
    timerInit = millis();  
  }   

  return true;
}

bool animGameLedFall() {
  const uint16_t TIME_GAME_ANIM = 60*1000;
  static long timerGame = 0;

  const uint16_t TIME_NEW_LED_START_ANIM = 1000;
  const uint16_t TIME_NEW_LED_END_ANIM = 100;
  static long timerNewLed = 0;
  static uint16_t timeNewLedAnim = 1000;
  const uint8_t PROB_NEW_LED = 40;  

  static long timerBlinkPlay1 = 0;
  static long timerBlinkPlay2 = 0;
  static uint8_t statusBlinkPlay1 = 0;
  static uint8_t statusBlinkPlay2 = 0;
  static uint32_t oldColorPlay1 = 0;
  static uint32_t oldColorPlay2 = 0;    
  const uint16_t TIME_BLINK_ANIM = 50;

  if (timerGame == 0) timerGame = millis();
  if ((millis() - timerGame) > TIME_GAME_ANIM) {
    timerGame = 0;
    return true;
  } 

  if (temptPlay1) {
    switch (statusBlinkPlay1) {
      case 0: // OFF
        oldColorPlay1 = ws2812b.getPixelColor(NUM_PIXELS-1);
        timerBlinkPlay1 = millis();
        ws2812b.setPixelColor(NUM_PIXELS-1, 0);
        ws2812b.show();
        statusBlinkPlay1++;
        break;
      case 1: // ON
        if ((millis() - timerBlinkPlay1) > TIME_BLINK_ANIM) {
          timerBlinkPlay1 = millis();
          ws2812b.setPixelColor(NUM_PIXELS-1, oldColorPlay1);
          ws2812b.show();
          statusBlinkPlay1++;
        }
        break;
      case 2: // OFF
        if ((millis() - timerBlinkPlay1) > TIME_BLINK_ANIM) {
          timerBlinkPlay1 = millis();
          ws2812b.setPixelColor(NUM_PIXELS-1, 0);
          ws2812b.show();
          statusBlinkPlay1++;
        }
        break;      
      case 3: // Finish
        // Do nothing only wait
      default:
        statusBlinkPlay1 = 0;
    }
  }
  else {
    statusBlinkPlay1 = 0;
  }

  if (temptPlay2) {
    switch (statusBlinkPlay2) {
      case 0: // OFF
        oldColorPlay2 = ws2812b.getPixelColor(0);
        timerBlinkPlay2 = millis();
        ws2812b.setPixelColor(0, 0);
        ws2812b.show();
        statusBlinkPlay2++;
        break;
      case 1: // ON
        if ((millis() - timerBlinkPlay2) > TIME_BLINK_ANIM) {
          timerBlinkPlay2 = millis();
          ws2812b.setPixelColor(0, oldColorPlay2);
          ws2812b.show();
          statusBlinkPlay2++;
        }
        break;
      case 2: // OFF
        if ((millis() - timerBlinkPlay2) > TIME_BLINK_ANIM) {
          timerBlinkPlay2 = millis();
          ws2812b.setPixelColor(0, 0);
          ws2812b.show();
          statusBlinkPlay2++;
        }
        break;      
      case 3: // Finish
        // Do nothing only wait
      default:
        statusBlinkPlay2 = 0;
    }
  }
  else {
    statusBlinkPlay2 = 0;
  }  

  if ((millis() - timerNewLed) > timeNewLedAnim) {
    // Scrolling led toward play1
    temptPlay1 = false;
    for (int i=NUM_PIXELS-2; i>=NUM_PIXELS/2; i--) {
      ws2812b.setPixelColor(i+1, ws2812b.getPixelColor(i));
    }
    
    // Scrolling led toward play2
    temptPlay2 = false;
    for (int i=1; i<NUM_PIXELS/2; i++) {
      ws2812b.setPixelColor(i-1, ws2812b.getPixelColor(i));
    }

    // New led in the center
    if (random(1, 101) <= PROB_NEW_LED) {
      ws2812b.setPixelColor(NUM_PIXELS/2, rgbPlay1);
      ws2812b.setPixelColor((NUM_PIXELS/2)-1, rgbPlay2);
    }
    else {
      ws2812b.setPixelColor(NUM_PIXELS/2, 0);
      ws2812b.setPixelColor((NUM_PIXELS/2)-1, 0);      
    }

    ws2812b.show();

    timeNewLedAnim = TIME_NEW_LED_START_ANIM - ((TIME_NEW_LED_START_ANIM-TIME_NEW_LED_END_ANIM) * ((float)(millis() - timerGame) / (float)TIME_GAME_ANIM));
    DEBUG_SERIAL.print("NEXT IN: ");
    DEBUG_SERIAL.println(timeNewLedAnim);
    timerNewLed = millis();
  }

  return false;
}

bool animWinnerLedFall() {
  return animWinnerTugOfWar();
}

bool animStartLedFall() {
  static long timerStart = 0;
  const uint16_t TIME_START_ANIM = 1000;
  static uint8_t step = 1;

  if ((millis() - timerStart) > TIME_START_ANIM) {
    ws2812b.clear();
    ws2812b.setPixelColor(step-1, rgbPlay2);
    ws2812b.setPixelColor((NUM_PIXELS-1)-step+1, rgbPlay1);
    ws2812b.show();

    step++;

    displayText(1, "%d", (NUM_PIXELS/2)-step+1);
    displayText(2, "%d", (NUM_PIXELS/2)-step+1);    

    if (step > NUM_PIXELS/2) {
      step = 1;
      return true;
    }

    timerStart = millis();
  }

  return false;
}

bool animInitTugOfWar() {
  static long timerInit = 0;
  const uint8_t TIME_INIT_ANIM = 10;
  static uint16_t hue1 = 0;     // green
  static uint16_t hue2 = 21845; // red
  uint32_t color1, color2;

  if ((millis() - timerInit) > TIME_INIT_ANIM) { 
    if (!readyPlay1) {
      hue1 += random(80, 121);
      color1 = ws2812b.gamma32(ws2812b.ColorHSV(hue1, 255, 255));
      for (int i=NUM_PIXELS/2; i<NUM_PIXELS; i++) {
        ws2812b.setPixelColor(i, color1);
      }      
    }

    if (!readyPlay2) {
      hue2 += random(80, 121);
      color2 = ws2812b.gamma32(ws2812b.ColorHSV(hue2, 255, 255));
      for (int i=0; i<NUM_PIXELS/2; i++) {
        ws2812b.setPixelColor(i, color2);
      }      
    }

    ws2812b.show();
    
    timerInit = millis();
  }

  return true;
}

bool animStartTugOfWar() {
  static long timerStart = 0;
  const uint16_t TIME_START_ANIM = 1000;
  static uint8_t step = 1;

  if ((millis() - timerStart) > TIME_START_ANIM) { 
    ws2812b.setPixelColor((NUM_PIXELS/2)-step, rgbPlay2);
    ws2812b.setPixelColor((NUM_PIXELS/2)+step-1, rgbPlay1);
    ws2812b.show();

    step++;

    displayText(1, "%d", (NUM_PIXELS/2)-step+1);
    displayText(2, "%d", (NUM_PIXELS/2)-step+1);

    if (step > NUM_PIXELS/2) {
      step = 1;
      return true;
    }

    timerStart = millis();
  }

  return false;
}

bool animWinnerTugOfWar() {
  static long timerWinner = 0;
  const uint16_t TIME_WINNER_ANIM = 500; 
  const uint8_t NUM_BLINK = 5;
  static uint32_t color = 0;
  static uint8_t numBlink = 0;
  
  if ((millis() - timerWinner) > TIME_WINNER_ANIM) {
    if (color == 0) {
      color = winPlay1 ? rgbPlay1 : rgbPlay2;
    }
    else {
      color = 0;
      numBlink++;
    }
    ws2812b.fill(color);
    ws2812b.show();

    if (numBlink >= NUM_BLINK) {
      numBlink = 0;
      return true;
    }    

    timerWinner = millis();
  }

  return false;
}

void clearLeds() {
  ws2812b.clear();
  ws2812b.show();
}

void colorRope() {
  uint8_t r_mix, g_mix, b_mix;
  float perc;

  for (int i=0; i<NUM_PIXELS; i++) {
    if (posCenterRope >= ((i+1)*(ROPE_LENGTH/NUM_PIXELS))) {
      ws2812b.setPixelColor(i, rgbPlay2);
    }
    else if (posCenterRope > (i*(ROPE_LENGTH/NUM_PIXELS))) {
      perc = (posCenterRope-((i-1)*(ROPE_LENGTH/NUM_PIXELS)))/(ROPE_LENGTH/NUM_PIXELS);
      r_mix = ((float)((uint8_t)(rgbPlay2 >> 16))*perc) + ((float)((uint8_t)(rgbPlay1 >> 16))*(1-perc));
      g_mix = ((float)((uint8_t)(rgbPlay2 >> 8))*perc) + ((float)((uint8_t)(rgbPlay1 >> 8))*(1-perc));
      b_mix = ((float)((uint8_t)(rgbPlay2))*perc) + ((float)((uint8_t)(rgbPlay1))*(1-perc));
      ws2812b.setPixelColor(i, ws2812b.Color(r_mix, g_mix, b_mix));
    }
    else {
      ws2812b.setPixelColor(i, rgbPlay1);
    }

    ws2812b.show();
  }
}