// Open Stuff:
// Implementing:
//   - Calibrate Temp measurement

#include <U8g2lib.h>
#include <FastLED.h>
#include <TM1637Display.h>
#include <Keypad.h>
#include <ResponsiveAnalogRead.h>
#include <PinChangeInterrupt.h>

#ifndef NOP
#define NOP __asm__("nop\n\t")
#endif

#define NUM_LEDS 4
#define DATA_PIN 11
#define led 12
#define BEEP_PIN 17

#define cut0 A0
#define cut1 A1
#define cut2 A2

uint8_t i;
CRGBArray<NUM_LEDS> leds;

// Module connection pins (Digital Pins)
#define CLK 10
#define DIO 9

TM1637Display ndis(CLK, DIO);

char keys[4][3]={
  {'1','2','3'},
  {'4','5','6'},
  {'7','8','9'},
  {'*','0','#'},
};

byte rowPins[4] = {2,3,4,5};
byte colPins[3] = {6,7,8};

Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, 4, 3 );
U8G2_SSD1306_128X64_NONAME_2_HW_I2C display(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);

#define max_timerindex 11
uint16_t timerval[max_timerindex] = {30, 60, 90, 120, 180, 240, 300, 360, 420, 480, 600};
uint8_t timerindex = 4;

#define max_penaltyindex 11
uint8_t penaltyval[max_penaltyindex] = {0, 5, 10, 15, 20, 30, 40, 50, 60, 90, 120};
uint8_t penaltyindex = 0;

uint16_t timer;
uint16_t tdisplay;
unsigned long startmillis;
bool initmillis = false;
volatile uint16_t tint_h = 500;
volatile uint16_t tint_f = 1000;

uint16_t defusecode;
uint8_t defdigit;
uint16_t definput;
bool defchecked;

#define GOOD 1
#define BAD 2
#define VERYBAD 3

volatile uint8_t cutopt[] {GOOD, BAD, VERYBAD};

volatile bool disarmed = false;
volatile bool tint_fast = false;
volatile bool detonate = false;

float bgresult, tpresult, batresult;

ResponsiveAnalogRead bgresp(A6, true, 0.05);
ResponsiveAnalogRead tpresp(A6, true, 0.05);
ResponsiveAnalogRead batresp(A6, true, 0.05);

const uint8_t SEG_CLEAR[] = { 0x00, 0x00, 0x00, 0x00 };

const uint8_t SEG_OFF[] = {
  SEG_G,           // -
  SEG_G,           // -
  SEG_G,           // -
  SEG_G            // -
};

int analogReadBG(){
  uint8_t low, high;
  ADMUX = 0;
  // For 168/328 boards
  // REFS1 REFS0          --> 0 1, AVcc internal ref. -Selects AVcc external reference
  // MUX3 MUX2 MUX1 MUX0  --> 1110 1.1V (VBG)         -Selects channel 14, bandgap voltage, to measure
  ADMUX = (0 << REFS1) | (1 << REFS0) | (0 << ADLAR) | (1 << MUX3) | (1 << MUX2) | (1 << MUX1) | (0 << MUX0);

  delay(2);
  bitSet(ADCSRA, ADSC);
  while (bit_is_set(ADCSRA, ADSC));
  low  = ADCL;
  high = ADCH;
  return (high << 8) | low;
}

int analogReadTP(){
  uint8_t low, high;
  ADMUX = 0;
  // For 168/328 boards
  // REFS1 REFS0          --> 1 1, Internal 1.1V Voltage Reference -Selects bandgap voltage
  // MUX3 MUX2 MUX1 MUX0  --> 1000 Internal Temperature Sensor     -Selects channel 8, temperature sensor, to measure
  ADMUX = (1 << REFS1) | (1 << REFS0) | (0 << ADLAR) | (1 << MUX3) | (0 << MUX2) | (0 << MUX1) | (0 << MUX0);

  delay(7);
  bitSet(ADCSRA, ADSC);
  while (bit_is_set(ADCSRA, ADSC));
  low  = ADCL;
  high = ADCH;
  return ((high << 8) | low) - 330;
}

void initlcd()
{
  display.setFont(u8g2_font_profont12_tr);
  display.setFontRefHeightExtendedText();
  display.setDrawColor(2);
  display.setFontPosTop();
  display.setFontDirection(0);
}

uint16_t pow10(uint8_t n)
{
  switch(n)
  {
    case 0: return 1; break;
    case 1: return 10; break;
    case 2: return 100; break;
    case 3: return 1000; break;
    default: return 0; break;
  }
}

void explosion()
{
  const uint8_t frame[10][4] = {
    0x00,
    SEG_B | SEG_C,
    SEG_E | SEG_F,
    0x00,

    0x00,
    SEG_B | SEG_C | SEG_G,
    SEG_E | SEG_F | SEG_G,
    0x00,

    0x00,
    0b01111111,
    0b01111111,
    0x00,

    SEG_B | SEG_C |SEG_G,
    0b01111111,
    0b01111111,
    SEG_E | SEG_F | SEG_G,

    0b01111111,
    0b01111111,
    0b01111111,
    0b01111111,

    0b01111111,
    SEG_A | SEG_D | SEG_E | SEG_F,
    SEG_A | SEG_B | SEG_C | SEG_D,
    0b01111111,

    SEG_A | SEG_D | SEG_E | SEG_F,
    0x00,
    0x00,
    SEG_A | SEG_B | SEG_C | SEG_D,
    
    SEG_A | SEG_D | SEG_E | SEG_F,
    0x00,
    0x00,
    SEG_A | SEG_B | SEG_C | SEG_D,

    SEG_CLEAR[0],
    SEG_CLEAR[1],
    SEG_CLEAR[2],
    SEG_CLEAR[3],

    SEG_B | SEG_C | SEG_D | SEG_E | SEG_G,
    SEG_A | SEG_D | SEG_E | SEG_F | SEG_G,
    SEG_A | SEG_B | SEG_C | SEG_E | SEG_F | SEG_G,
    SEG_B | SEG_C | SEG_D | SEG_E | SEG_G
  };
  const uint8_t delwait = 20;
  uint8_t i;

  // Make sure everything is switched off
  digitalWrite(led, LOW);
  digitalWrite(LED_BUILTIN, LOW);
  // But beep through the animation
  digitalWrite(BEEP_PIN, HIGH);

  display.firstPage();
  do {
    initlcd();
    display.setCursor(20, 22);
    display.print("< DETONATING >");
  } while ( display.nextPage() );

  for(i=0; i < 9; i++) {
    switch(i) {
      case 0:
      case 7:
      case 8:
        ndis.setBrightness(0);
        break;
      case 1:
        ndis.setBrightness(1);
        break;
      case 2:
        ndis.setBrightness(3);
        break;
      case 3:
      case 5:
        ndis.setBrightness(5);
        break;
      case 4:
        ndis.setBrightness(7);
        break;
      case 6:
        ndis.setBrightness(2);
        break;
    }
    ndis.setSegments(frame[i]);
    leds[0].setRGB(i, i*2, 0);
    leds[1].setRGB(i*2, i*4, i);
    leds[2].setRGB(i*2, i*4, i);
    leds[3].setRGB(i, i*2, 0);
    FastLED.show();
    delay_init();
    delay_waste(delwait);
  }
  delay_init();
  delay_waste(500);
  ndis.setBrightness(7);
  ndis.setSegments(frame[9]);
  
  digitalWrite(BEEP_PIN, LOW);
  leds[0].setRGB(0, 0, 0);
  leds[1].setRGB(0, 0, 0);
  leds[2].setRGB(0, 0, 0);
  leds[3].setRGB(0, 0, 0);
  FastLED.show();
  display.firstPage();
  do {
    initlcd();
    display.setCursor(26, 22);
    display.print("You are dead");
    display.setCursor(24, 32);
    display.print("Press any key");
    display.drawBox(0, 0, 128, 64);
  } while ( display.nextPage() );

}

void safe()
{
  // Display leftover time
  tdisplay = ((timer / 60) * 100);
  tdisplay += timer % 60;
  ndis.showNumberDecEx(tdisplay, 64, true);

  // Make sure everything is switched off
  digitalWrite(led, LOW);
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(BEEP_PIN, LOW);
  for(uint8_t i=0; i < 4; i++) { leds[i].setRGB(0,0,0); }
  FastLED.show();

  display.firstPage();
  do {
    initlcd();
    display.setCursor(26, 22);
    display.print("< DISARMED >");
    display.setCursor(24, 32);
    display.print("Press any key");
    display.drawBox(0, 0, 128, 64);
  } while ( display.nextPage() );
  delay_init();
  delay_waste(500);
}

void initiate()
{
  display.firstPage();
  do {
    initlcd();
    display.setCursor(26, 0);
    display.print("Enter Code");
    display.setCursor(26, 10);
    display.print("* Delete");
    display.setCursor(26, 20);
    display.print("# Options");
  } while ( display.nextPage() );
  ndis.setBrightness(7); // (0-7)
  ndis.setSegments(SEG_OFF);
  uint8_t digit = 0;
  uint16_t dinput = 0;
  while(digit < 4) {
    char key = keypad.getKey();
    if(key != NO_KEY) {
      dinput *= 10;
      digit++;
      switch(key) {
        case '1': dinput += 1; break;
        case '2': dinput += 2; break;
        case '3': dinput += 3; break;
        case '4': dinput += 4; break;
        case '5': dinput += 5; break;
        case '6': dinput += 6; break;
        case '7': dinput += 7; break;
        case '8': dinput += 8; break;
        case '9': dinput += 9; break;
        case '0': dinput += 0; break;
        case '#':
          optionsmenu();
          display.firstPage();
          do {
            initlcd();
            display.setCursor(26, 0);
            display.print("Enter Code");
            display.setCursor(26, 10);
            display.print("* Back");
            display.setCursor(26, 20);
            display.print("# Options");
          } while ( display.nextPage() );
          dinput = 0;
          digit = 0;
          break;
        case '*':
          if(digit > 1) {
            dinput /= 100;
            digit--;
            digit--;
          } else {
            digit--;
          }
          break;
      }
      ndis.setSegments(SEG_OFF);
      if(dinput > 0) {
        ndis.showNumberDecEx(dinput, 0, true, digit, 3-(digit-1));
      } else {
        if(digit>0) {
          ndis.showNumberDecEx(0, 0, true, digit, 3-(digit-1));
        } else {
          ndis.setSegments(SEG_OFF);
        }
      }
    }
  }
  tint_h = 500;
  tint_f = 1000;
  tint_fast = false;
  disarmed = false;
  detonate = false;
  definput = 0;
  defdigit = 0;

  switch((dinput + analogRead(A5)) % 6)
  {
    case 0: cutopt[0] = GOOD; cutopt[1] =  BAD; cutopt[2] =  VERYBAD; break;
    case 1: cutopt[0] = GOOD; cutopt[1] =  VERYBAD; cutopt[2] =  BAD; break;
    case 2: cutopt[0] = BAD; cutopt[1] =  VERYBAD; cutopt[2] =  GOOD; break;
    case 3: cutopt[0] = BAD; cutopt[1] =  GOOD; cutopt[2] =  VERYBAD; break;
    case 4: cutopt[0] = VERYBAD; cutopt[1] =  GOOD; cutopt[2] =  BAD; break;
    case 5: cutopt[0] = VERYBAD; cutopt[1] =  BAD; cutopt[2] =  GOOD; break;
  }
  
  timer = timerval[timerindex];
  defusecode = dinput;
  defchecked = false;
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(cut0), detcut0, RISING);
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(cut1), detcut1, RISING);
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(cut2), detcut2, RISING);
  display.firstPage();
  do {
    initlcd();
    display.setCursor(37, 0);
    display.print("< ARMED >");
  } while ( display.nextPage() );
}

void waitkey()
{
  char key = getkey();
}

char getkey()
{
  char key = keypad.getKey();
  while(key == NO_KEY) {
    key = keypad.getKey();
  }
  return key;
}

void delay_init()
{
  startmillis=millis();
  initmillis=true;
}

bool delay_done(uint16_t wastetime)
{
  if(initmillis)
  {
    return !(millis() - startmillis < wastetime);
  } else {
    return true;
  }
}

void delay_waste(uint16_t wastetime)
{
  if(initmillis)
  {
    while(!delay_done(wastetime)) {
      NOP;
    }
    initmillis=false;
  }
}

void delay_waste_cont(uint16_t wastetime)
{
  if(initmillis)
  {
    delay_waste(wastetime);
    initmillis=true;
  }
}

void optionsmenu() // TODO: Write Menu code, change settings, display values
{
  bool optionswitch = true;
  display.firstPage();
  do {
    initlcd();
    display.setCursor(32, 0);
    display.print("< OPTIONS >");
    if(optionswitch) {
      display.setCursor(38, 10);
      display.print("Set Timer");
    } else {
      display.setCursor(32, 10);
      display.print("Set Penalty");
    }
    display.setCursor(0, 30);
    display.print("2 Up, 8 Down");
    display.setCursor(0, 40);
    display.print("4 or 6 Select option");
    display.setCursor(0, 50);
    display.print("* Cancel, # OK");
  } while ( display.nextPage() );
  char key = '-';
  uint8_t temptimerindex = timerindex;
  uint8_t temppenaltyindex = penaltyindex;
  bool endoption = false;
  while(!endoption)
  {
    if(optionswitch) {
      tdisplay = ((timerval[temptimerindex] / 60) * 100);
      tdisplay += timerval[temptimerindex] % 60;
    } else {
      tdisplay = ((penaltyval[temppenaltyindex] / 60) * 100);
      tdisplay += penaltyval[temppenaltyindex] % 60;
    }
    ndis.showNumberDecEx(tdisplay, 64, true);
    key = getkey();
    switch(key)
    {
      case '2':
        if(optionswitch) {
          if(temptimerindex < (max_timerindex - 1)) temptimerindex++;
        } else {
          if(temppenaltyindex < (max_penaltyindex -1)) temppenaltyindex++;
        }
        break;
      case '8':
        if(optionswitch) {
          if(temptimerindex > 0) temptimerindex--;
        } else {
          if(temppenaltyindex > 0) temppenaltyindex--;
        }
        break;
      case '4':
      case '6':
        optionswitch = !optionswitch;
        display.firstPage();
        do {
          initlcd();
          display.setCursor(32, 0);
          display.print("< OPTIONS >");
          if(optionswitch) {
            display.setCursor(38, 10);
            display.print("Set Timer");
          } else {
            display.setCursor(32, 10);
            display.print("Set Penalty");
          }
          display.setCursor(0, 30);
          display.print("2 Up, 8 Down");
          display.setCursor(0, 40);
          display.print("4 or 6 Select option");
          display.setCursor(0, 50);
          display.print("* Cancel, # OK");
        } while ( display.nextPage() );
        break;
      case '*': endoption = true; break;
      case '#': endoption = true; timerindex = temptimerindex; penaltyindex = temppenaltyindex; break;
    }
  }
}

void checkinputs()
{
  char key = keypad.getKey();
  if(key != NO_KEY && defdigit < 4) {
    definput *= 10;
    defdigit++;
    switch(key) {
      case '1': definput += 1; break;
      case '2': definput += 2; break;
      case '3': definput += 3; break;
      case '4': definput += 4; break;
      case '5': definput += 5; break;
      case '6': definput += 6; break;
      case '7': definput += 7; break;
      case '8': definput += 8; break;
      case '9': definput += 9; break;
      case '0': definput += 0; break;
      case '#':
      case '*':
        if(defdigit > 1) {
          definput /= 100;
          defdigit--;
          defdigit--;
        } else {
          defdigit--;
        }
        break;
    }
    uint8_t i;
    for(i=0; i < 4; i++) { leds[i].setRGB(0,0,0); }
    for(uint8_t i=0; i < defdigit; i++) { leds[3-i].setRGB(0,0,16); }
    FastLED.show();
  }
}

void detcut0(void) {
  switch(cutopt[0])
  {
    case GOOD: disarmed=true; break;
    case BAD: if(!tint_fast) {tint_fast=true; tint_h /= 2; tint_f /= 2;} break;
    case VERYBAD: detonate=true; disarmed=true; break;
  }
}

void detcut1(void) {
  switch(cutopt[1])
  {
    case GOOD: disarmed=true; break;
    case BAD: if(!tint_fast) {tint_fast=true; tint_h /= 2; tint_f /= 2;} break;
    case VERYBAD: detonate=true; disarmed=true; break;
  }
}

void detcut2(void) {
  switch(cutopt[2])
  {
    case GOOD: disarmed=true; break;
    case BAD: if(!tint_fast) {tint_fast=true; tint_h /= 2; tint_f /= 2;} break;
    case VERYBAD: detonate=true; disarmed=true; break;
  }
}

void displayaval()
{
  display.setCursor(0, 40);
  display.print("Temp: ");
  display.print(tpresult);
  display.setCursor(0, 52);
  display.print("Bat: ");
  display.print(batresult);
  display.print(" Vcc: ");
  display.print(bgresult);
}

void setup()
{
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
  leds[0].setRGB(0, 0, 0);
  leds[1].setRGB(0, 0, 0);
  leds[2].setRGB(0, 0, 0);
  leds[3].setRGB(0, 0, 0);
  FastLED.show();
  display.begin();
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(BEEP_PIN, OUTPUT);
  pinMode(led, OUTPUT);
  pinMode(cut0, INPUT_PULLUP);
  pinMode(cut1, INPUT_PULLUP);
  pinMode(cut2, INPUT_PULLUP);

  pinMode(A6, INPUT);
}

void loop()
{
  initiate();
  while((timer > 0) && !disarmed) {
    delay_init();
    // Read all analog inputs, calculate values.
    // This should be a good time because things are going to be noisy afterwards
    bgresp.update(analogReadBG());
    bgresult=1126.4 / bgresp.getValue();
    analogReference(DEFAULT);
    batresp.update(analogRead(A6));
    batresult=(1.1 * batresp.getValue()) / bgresp.getValue();
    tpresp.update(analogReadTP());
    tpresult=tpresp.getValue();
    // Now we have all analog readings, let's start with the noisy stuff

    if((defdigit == 4) && (defusecode == definput)) { disarmed = true; }
    
    if((defdigit == 4) && (!disarmed)) {
      for(uint8_t i=0; i < 4; i++) {
        if( (defusecode / pow10(i)) % 10 == (definput / pow10(i)) % 10) {
          leds[3-i].setRGB(16, 0, 0);
        } else {
          leds[3-i].setRGB(0, 16, 0);
        }
      }
      FastLED.show();
      defchecked = true;
    }
    
    digitalWrite(led, HIGH);
    digitalWrite(LED_BUILTIN, HIGH);
    digitalWrite(BEEP_PIN, HIGH);
    tdisplay = ((timer / 60) * 100);
    tdisplay += timer % 60;
    ndis.showNumberDecEx(tdisplay, 64, true);
    display.firstPage();
    do {
      initlcd();
      display.setCursor(37, 0);
      display.print("< ARMED >");
      displayaval();
    } while ( display.nextPage() );
    while(!delay_done(50) && !disarmed) {
      checkinputs();
    }
    digitalWrite(led, LOW);
    digitalWrite(LED_BUILTIN, LOW);
    digitalWrite(BEEP_PIN, LOW);
    if(timer < 20) {
      while(!delay_done(150) && !disarmed) {
        checkinputs();
      }
      digitalWrite(led, HIGH);
      digitalWrite(LED_BUILTIN, HIGH);
      digitalWrite(BEEP_PIN, HIGH);
      while(!delay_done(250) && !disarmed) {
        checkinputs();
      }
      digitalWrite(led, LOW);
      digitalWrite(LED_BUILTIN, LOW);
      digitalWrite(BEEP_PIN, LOW);
    }
    while(!delay_done(tint_h) && !disarmed) {
      checkinputs();
    }
    if(timer > 20) {
      ndis.showNumberDecEx(tdisplay, 0, true);
      while(!delay_done(tint_f) && !disarmed) {
        checkinputs();
      }
    } else {
      ndis.setSegments(SEG_CLEAR);
      while(!delay_done(tint_f) && !disarmed) {
        checkinputs();
      }
    }
    if(defdigit == 4 && !disarmed && defchecked) {
      defdigit = 0;
      definput = 0;
      for(uint8_t i=0; i < 4; i++) {
        leds[i].setRGB(0, 0, 0);
      }
      FastLED.show();
      // Penalty for wrong try: reduce timer by set penalty (default: 10 seconds)
      if(timer > penaltyval[penaltyindex] + 1 ) { timer -= penaltyval[penaltyindex]; } else { timer = 1; }
      defchecked = false;
    }
    if(!disarmed) {
      timer--;
    }
  }

  detachPinChangeInterrupt(digitalPinToPinChangeInterrupt(cut0));
  detachPinChangeInterrupt(digitalPinToPinChangeInterrupt(cut1));
  detachPinChangeInterrupt(digitalPinToPinChangeInterrupt(cut2));

  if(detonate) {
    explosion();
  } else {
    if(disarmed) {
      safe();
    } else {
      explosion();
    }
  }
  waitkey();
  delay_init();
  delay_waste(200);
}