#include <Adafruit_SSD1306.h>
#include <avr/eeprom.h>

#include <GyverEncoder.h>

#include <Fonts/FreeMono12pt7b.h>
#include <Fonts/FreeSans12pt7b.h>
#include <Fonts/FreeSerif12pt7b.h>
#include <Fonts/FreeMonoBold12pt7b.h>
#include <Fonts/FreeSansBold12pt7b.h>
#include <Fonts/FreeSerifBold12pt7b.h>
#include <Fonts/FreeMono9pt7b.h>

void* DF = &FreeSans12pt7b;
void* TF = &FreeMono9pt7b;

#define __EMULATE_MODE__
//#define __DROP_EEPROM__

#define TM1_PRSC_VL       32

// ==== PINS config
#define VOLTAGE_1         A0    // 
#define VOLTAGE_2         A2    // 
#define VOLTAGE_3         A1    // 

#define BALANCE_1         11    // PB3
#define BALANCE_2         10    // PB2
#define BALANCE_3         9     // PB1

#define CHARGE_1A         7     // PD7
#define CHARGE_4A         13    // PB5
#define ENC_DT            3     // PD3
#define ENC_CLK           2     // PD2
#define ENC_SW            6     // PD6
#define BTN_RUN           4     // PD4
#define OUT_PIN           12    // PB4
#define EMULATION_TICKS   8     // PB0

void configPins() {
  pinMode(OUT_PIN, OUTPUT);

  pinMode(CHARGE_1A, OUTPUT);
  pinMode(CHARGE_4A, OUTPUT);
  pinMode(BALANCE_1, OUTPUT);
  pinMode(BALANCE_2, OUTPUT);
  pinMode(BALANCE_3, OUTPUT);

  pinMode(ENC_DT, INPUT);
  pinMode(ENC_CLK, INPUT);
  pinMode(ENC_SW, INPUT_PULLUP);
  pinMode(BTN_RUN, INPUT_PULLUP);

#ifdef __EMULATE_MODE__
  pinMode(EMULATION_TICKS, OUTPUT);
#endif
}

#define BTN_SELECT_READ         bitRead(PIND, 6)
#define BTN_RUN_READ            bitRead(PIND, 4)
#define EMULATION_READ          bitRead(PINB, 0)


#define DROP_OUT              PORTB &= ~(1 << 4)
#define SET_OUT               PORTB |= 1 << 4

#define DROP_CHARGE_1A        PORTB &= ~(1 << 5)
#define SET_CHARGE_1A         PORTB |= 1 << 5
#define SET_CHARGE_1A_PIN(b)  if(b)SET_CHARGE_1A;else DROP_CHARGE_1A;
#define DROP_CHARGE_4A        PORTD &= ~(1 << 7)
#define SET_CHARGE_4A         PORTD |= 1 << 7
#define SET_CHARGE_4A_PIN(b)  if(b)SET_CHARGE_4A;else DROP_CHARGE_4A;
#define DROP_BALANCE_1        PORTB &= ~(1 << 3)
#define SET_BALANCE_1         PORTB |= 1 << 3
#define SET_BALANCE_1_PIN(b)  if(b)SET_BALANCE_1;else DROP_BALANCE_1;
#define DROP_BALANCE_2        PORTB &= ~(1 << 2)
#define SET_BALANCE_2         PORTB |= 1 << 2
#define SET_BALANCE_2_PIN(b)  if(b)SET_BALANCE_2;else DROP_BALANCE_2;
#define DROP_BALANCE_3        PORTB &= ~(1 << 1)
#define SET_BALANCE_3         PORTB |= 1 << 1
#define SET_BALANCE_3_PIN(b)  if(b)SET_BALANCE_3;else DROP_BALANCE_3;

#define DROP_EMULATION        PORTB &= ~(1 << 0)
#define SET_EMULATION         PORTB |= 1 << 0

// ==== Screen config
#define SCREEN_WIDTH      128
#define SCREEN_HEIGHT     64
#define SCREEN_ADDRESS    0x3C
#define DISPL_UPD_BOUND   8


// ==== EEPROM
#define ADDR_EEPROM_FIRST 52

// ==== Display layout
#define H_LEFT_POS              5
#define H_RIGHT_POS             110
#define V_TOP_POS               1
#define V_MIDDLE_POS            24
#define V_BOTTOM_POS            47
#define H_VOLTAGE_OFFSET        50
#define H_VOLTAGE_REAL_OFFSET   90
#define H_MODE_POS              60
#define V_MODE_POS              53

#define RIGHT_POS               95
#define PHASES_COUNT            5
#define MAX_PHASE_VAL           999
#define DIGIT_WIDTH             12
#define DIGIT_HEIGHT            16


#define VOLTAGE_INDEX 3
#define MODE_INDEX 7
#define VOLTAGE_INDEX_REAL 8

// ==== Logic
#define MODES_COUNT       6

const uint8_t MODES_EDITING_POS[3][2] = {
  {H_LEFT_POS, V_TOP_POS},
  {H_LEFT_POS, V_MIDDLE_POS},
  {H_LEFT_POS, V_BOTTOM_POS}
};
#define POSITIONS_COUNT   3

// const uint8_t VOLTAGE_POS[3][2] = {
//   {H_LEFT_POS, V_TOP_POS},
//   {H_LEFT_POS, V_MIDDLE_POS},
//   {H_LEFT_POS, V_BOTTOM_POS}
// };
#define BATTERIES_COUNT   3

/**
  modes
  0 - base mode - run | select
  1 - run mode - do nothing. automatically turn to 0 mode after the end.
  2 - fast editing first  - edit | save | select | go to 2
  2 - editing first  - edit | save | select | go to 5
  6 - fast editing pause  - edit | save | select | go to 3
*/
volatile int8_t mode = 0;

int16_t* phases = new int16_t[PHASES_COUNT] {
  50, 200, 100, 200, 50
};

volatile int16_t phaseTick = 0;
volatile int8_t phaseNum = 0;

uint8_t phaseNumPrev = 0;
ISR(TIMER1_COMPA_vect) {
  if (mode == 1) {
    if (phaseTick >= phases[phaseNum]) {
      phaseTick = 0;
      phaseNum++;
    }
    phaseTick++;
    if (phaseNum >= PHASES_COUNT) {
      DROP_OUT; // D10 LOW
      resetOutput();
      turnUpdate();
      return;
    }
    if (phaseNum % 2 == 0) {
      DROP_OUT; // D10 LOW
    } else {
      SET_OUT; // D10 HIGH
    }
  }
}

void resetOutput() {
  mode = 0;
  phaseTick = 0;
  phaseNum = 0;
  phaseNumPrev = 0;
}

#ifdef __EMULATE_MODE__
#define FAST_MODE_DELAY_TICKS         60
#define FAST_MODE_ADJUST_VALUE        10
#define FAST_MODE_SPEED_THRESHOLD     3
#endif
#ifndef __EMULATE_MODE__
#define FAST_MODE_DELAY_TICKS         60
#define FAST_MODE_ADJUST_VALUE        10
#define FAST_MODE_SPEED_THRESHOLD     2
#endif

uint8_t adjustValue = 0;
uint8_t fastModeTicks = 0;
uint8_t encSpeed = 0;
uint8_t encSpeedTicks = 0;

#define NEED_TO_SET_BALANCE 2
#define NEED_TO_DROP_BALANCE 0
#define NOTHING_TO_DO_ON_BALANCE 1

boolean balance1 = false;
boolean balance2 = false;
boolean balance3 = false;

uint16_t v1mV = 0;
uint16_t v2mV = 0;
uint16_t v3mV = 0;


#define COUNT_INTEGRATE 40

uint16_t v1mV_st[COUNT_INTEGRATE] = {0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
uint16_t v2mV_st[COUNT_INTEGRATE] = {0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};
uint16_t v3mV_st[COUNT_INTEGRATE] = {0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0};

void putVal(uint16_t* arr, uint16_t val) {
  int i = 0;
  for(; i < (COUNT_INTEGRATE - 1); i++) {
    arr[i] = arr[i+1];
  }
  arr[i] = val;
}

uint16_t av(uint16_t* arr) {
  uint32_t av = arr[0];
  for(int i = 1; i < COUNT_INTEGRATE; i++) {
    av += arr[i];
  }
  return av/COUNT_INTEGRATE;
}

void processCharge() {
  uint16_t b1 = analogRead(VOLTAGE_1);
  uint16_t b2 = analogRead(VOLTAGE_2);
  uint16_t b3 = analogRead(VOLTAGE_3);
  uint16_t v1mV_tmp = voltage1mV(b1, b2, b3);
  uint16_t v2mV_tmp = voltage2mV(b1, b2, b3);
  uint16_t v3mV_tmp = voltage3mV(b1, b2, b3);

  putVal(v1mV_st, v1mV_tmp);
  v1mV =  av(v1mV_st);
  putVal(v2mV_st, v2mV_tmp);
  v2mV =  av(v2mV_st);
  putVal(v3mV_st, v3mV_tmp);
  v3mV =  av(v3mV_st);

  // 2.7 V 2700 mV
  calcBalance(&balance1, v1mV);
  calcBalance(&balance2, v2mV);
  calcBalance(&balance3, v3mV);

  // SET_BALANCE_1_PIN(balance1);
  // SET_BALANCE_2_PIN(balance2);
  // SET_BALANCE_3_PIN(balance3);

  // SET_CHARGE_1A_PIN((v1mV + v2mV + v3mV) < 2580 * 3);
  // SET_CHARGE_4A_PIN((v1mV + v2mV + v3mV) < 2300 * 3);
  turnUpdate();
}

void calcBalance(boolean* currentBalance, uint16_t voltage) {
  if (!(*currentBalance) && voltage > 2600) {
    (*currentBalance) = true;
  }
  if (*currentBalance && voltage < 2500) {
    (*currentBalance) = false;
  }
}

#ifndef __EMULATE_MODE__
double K0 = 2.9296875;
double K1 = 1.9941634241;
double K2 = 3.1392156862;

uint16_t voltage1mV(uint16_t v1, uint16_t v2, uint16_t v3) {
  return v1 * K0;
}
uint16_t voltage2mV(uint16_t v1, uint16_t v2, uint16_t v3) {
  return (v2*K1 - v1) * K0;
}
uint16_t voltage3mV(uint16_t v1, uint16_t v2, uint16_t v3) {
  return (v3*K2 - v2*K1) * K0;
}
#endif

#ifdef __EMULATE_MODE__
uint16_t voltage1mV(uint16_t v1, uint16_t v2, uint16_t v3) {
  return (v1 / 4) + 2450;
}
uint16_t voltage2mV(uint16_t v1, uint16_t v2, uint16_t v3) {
  return (v2 / 4) + 2450;
}
uint16_t voltage3mV(uint16_t v1, uint16_t v2, uint16_t v3) {
  return (v3 / 4) + 2450;
}
#endif

void changeAdjust() {
  if (encSpeed > FAST_MODE_SPEED_THRESHOLD) {
    fastModeTicks = FAST_MODE_DELAY_TICKS;
    adjustValue = FAST_MODE_ADJUST_VALUE;
  } else {
    if (fastModeTicks == 0 ) {
      adjustValue = 1;
    }
  }
}

void decrementFastModeTicks() {
  fastModeTicks -= (fastModeTicks > 0) ? 1 : 0;
}

void onBtnLeftPressed() {
  if (isEditingMode()) {
    changeAdjust();
    uint8_t editingPos = calcEditingIndex() + 1;
    boolean lessMin = phases[editingPos] < adjustValue;
    if (!lessMin) {
      phases[editingPos] -= adjustValue;
    }
  }
}

void onBtnRightPressed() {
  if (isEditingMode()) {
    changeAdjust();
    uint8_t editingPos = calcEditingIndex() + 1;
    boolean biggerMax = (MAX_PHASE_VAL - phases[editingPos]) < adjustValue ;
    if (!biggerMax) {
      phases[editingPos] += adjustValue;
    }
  }
}

boolean isEditingMode() {
  return (mode >=2 && mode <= 4) || mode == 5;
}

void onBtnSelectPressed() {
  if (mode == 1) return;
  mode = (mode += mode == 0 ? 2 : 1) % MODES_COUNT;
  if(mode == 5) {
    SET_CHARGE_1A_PIN(true);
  } else {
    SET_CHARGE_1A_PIN(false);
  }
  if(mode == 0) {
    storeToEEPROM();
  }
}

void onBtnRunPressed() {
  if (mode != 0) {
    return;
  };
  resetOutput();
  phases[0] = phases[1] / 2;
  phases[4] = phases[3] / 2;
  mode = 1;
}

Adafruit_SSD1306 display = Adafruit_SSD1306(SCREEN_WIDTH, SCREEN_HEIGHT);

#ifdef __EMULATE_MODE__
Encoder enc(ENC_DT, ENC_CLK);
#endif
#ifndef __EMULATE_MODE__
Encoder enc(ENC_CLK, ENC_DT);
#endif

uint8_t editingTicks = 0;

void printByN(uint8_t n, boolean inv) {
  if(n >= 0 && n <= 2) {
    // 3 digit number
    uint8_t numberSize = 3;
    display.setTextSize(2);
    uint8_t x = MODES_EDITING_POS[n][0];
    uint8_t y = MODES_EDITING_POS[n][1];
    if(inv) {
      display.fillRect(x - 1, y - 1, DIGIT_WIDTH * numberSize, DIGIT_HEIGHT, WHITE);
      display.setTextColor(BLACK);
    } else {
      display.fillRect(x - 1, y - 1, DIGIT_WIDTH * numberSize, DIGIT_HEIGHT, BLACK);
      display.setTextColor(WHITE);
    }
    display.setCursor(x, y);
    uint16_t currentPhase = phases[n + 1];
    for (int j = 0; j < numberSize; j++) {
      uint8_t phaseAtPos = signAtOf3(currentPhase, j);
      display.print(phaseAtPos);
    }
  }
  if(n == VOLTAGE_INDEX) {
    uint8_t numberSize = 2;
    display.setTextSize(2);
    uint8_t x = MODES_EDITING_POS[0][0] + H_VOLTAGE_OFFSET;
    uint8_t y = MODES_EDITING_POS[0][1]/2 + 1;
    if(inv) {
      display.fillRect(x - 1, y - 1, DIGIT_WIDTH * numberSize, DIGIT_HEIGHT, WHITE);
      display.setTextColor(BLACK);
    } else {
      display.fillRect(x - 1, y - 1, DIGIT_WIDTH * numberSize, DIGIT_HEIGHT, BLACK);
      display.setTextColor(WHITE);
    }
    display.setCursor(x, y);
    display.print("78");
  }
  if(n == VOLTAGE_INDEX_REAL) {
    uint8_t numberSize = 4;
    display.setTextSize(1);
    uint8_t x = MODES_EDITING_POS[0][0] + H_VOLTAGE_REAL_OFFSET;
    uint8_t y = MODES_EDITING_POS[0][1]/2 + 2;
    display.setCursor(x, y);
    display.print(v1mV + v2mV + v3mV);
  }
  if(n >= 4 && n <= 6) {
    uint8_t i = n - 4;
    display.setTextSize(1);
    display.setTextColor(WHITE);

    display.setCursor(
      MODES_EDITING_POS[i][0] + H_VOLTAGE_REAL_OFFSET,
      MODES_EDITING_POS[i][1]/2 + 30 + (i == (BATTERIES_COUNT - 1) ? 1:0)
    );
    
    uint16_t v = 0;
    if(i == 0) v = v1mV;
    if(i == 1) v = v2mV;
    if(i == 2) v = v3mV;

    display.print(v);
  }
  if(n == MODE_INDEX) {
    display.setTextSize(1);
    display.setCursor(H_MODE_POS, V_MODE_POS);
    display.print(mode);
  }
}

void updateDisplay() {
  display.clearDisplay();
  for (int i = 0; i < POSITIONS_COUNT; i++) {
    printByN(i, false);
  }

  printByN(VOLTAGE_INDEX, false);
  printByN(VOLTAGE_INDEX_REAL, false);

  if (isEditingMode()) {
    boolean invertEditingSign = (++editingTicks % 6) > 2;
    if (invertEditingSign) {
      uint8_t editingIndex = calcEditingIndex();
      if(editingIndex >= 0 && editingIndex <= 2) {
        printByN(editingIndex, true);
      }
      if(editingIndex == VOLTAGE_INDEX) {
        printByN(VOLTAGE_INDEX, true);
      }
    }
    turnUpdate();
  }

  for (int i = 0; i < BATTERIES_COUNT; i++) {
    printByN(i + 4, false);
  }

  printByN(MODE_INDEX, false);
}

uint8_t calcEditingIndex() {
  if(mode >= 2 && mode <= 4){
    return mode - 2;
  }
  if(mode == 5){
    return VOLTAGE_INDEX;
  }
}

uint8_t signAtOf3(uint16_t val, uint8_t pos) {
  if (pos == 0) {
    return (val / 10 / 10) % 10;
  }
  if (pos == 1) {
    return (val / 10) % 10;
  }
  return val % 10;
}

uint8_t signAtOf4(uint16_t val, uint8_t pos) {
  if (pos == 0) {
    return (val / 10 / 10 / 10) % 10;
  }
  if (pos == 1) {
    return (val / 10 / 10) % 10;
  }
  if (pos == 2) {
    return (val / 10) % 10;
  }
  return val % 10;
}

boolean btnPressed1Prev = false;
boolean btnPressed2Prev  = false;
boolean btnPressed3Prev  = false;
boolean btnPressed4Prev = false;

volatile int8_t ms = 0;

volatile boolean initDispl = false;
volatile boolean needToDisplay = false;
volatile boolean needToUpdate = false;
volatile boolean updatingDisplay = false;
volatile boolean displaying = false;

void turnUpdate() {
  needToUpdate = true;
}

volatile int value = 0;

ISR(TIMER0_COMPA_vect) {
  if (mode != 1) {
    processCharge();
    decrementFastModeTicks();
    boolean btnPressed3 = BTN_SELECT_READ == 0;
    boolean btnPressed4 = BTN_RUN_READ == 0;

    enc.tick();
    boolean wasTurnRight = enc.isRight();
    boolean wasTurnLeft = enc.isLeft();

    // capture encoder speed
    encSpeed = encSpeedTicks;
    encSpeedTicks = 0;

#ifdef __EMULATE_MODE__
    if (wasTurnLeft) {
      onBtnRightPressed();
      turnUpdate();
    }
    if (wasTurnRight) {
      onBtnLeftPressed();
      turnUpdate();
    }
#endif
#ifndef __EMULATE_MODE__
    if (wasTurnLeft) {
      onBtnLeftPressed();
      turnUpdate();
    }
    if (wasTurnRight) {
      onBtnRightPressed();
      turnUpdate();
    }
#endif

    if (!btnPressed3Prev && btnPressed3) {
      onBtnSelectPressed();
      turnUpdate();
    }
    if (!btnPressed4Prev && btnPressed4) {
      onBtnRunPressed();
      turnUpdate();
    }

    btnPressed3Prev = btnPressed3;
    btnPressed4Prev = btnPressed4;

    if (ms == 0) {
      if (needToUpdate && !displaying) {
        needToUpdate = false;
        updatingDisplay = true;
        updateDisplay();
        updatingDisplay = false;
        needToDisplay = true;
      }
    }
    ms += (ms < DISPL_UPD_BOUND) ? 1 : -DISPL_UPD_BOUND;
  }
}

#ifdef __EMULATE_MODE__
ISR(TIMER2_COMPA_vect) {
  boolean isSet = EMULATION_READ;
  if (isSet) DROP_EMULATION;
  else SET_EMULATION;
}
#endif

int main() {
  setup_c();
  for (;;) {
    loop_c();
  }
  return 0;
}
void isrCLK() {
  enc.tick();
  encSpeedTicks++;
}
void isrDT() {
  enc.tick();
  encSpeedTicks++;
}
void setup_c() {

#ifdef __DROP_EEPROM__
  eeprom_write_word(10, -1);
  eeprom_write_word(12, -1);
  eeprom_write_word(14, -1);
  eeprom_write_word(32, -1);
  eeprom_write_word(34, -1);
  eeprom_write_word(36, -1);
  eeprom_write_word(ADDR_EEPROM_FIRST, -1);
  eeprom_write_word(ADDR_EEPROM_FIRST + 2, -1);
  eeprom_write_word(ADDR_EEPROM_FIRST + 4, -1);
#endif

  configPins();

  // turn on the ADC
  ADCSRA |= (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0) ;

  analogReference(EXTERNAL);

  attachInterrupt(0, isrCLK, CHANGE);
  attachInterrupt(1, isrDT, CHANGE);
  enc.setType(TYPE2);


  noInterrupts();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
#ifndef __EMULATE_MODE__
  OCR1A = TM1_PRSC_VL;
#endif
#ifdef __EMULATE_MODE__
  OCR1A = 1;
#endif
  TCCR1B |= (1 << WGM12);   // CTC
  TCCR1B |= (1 << CS10) | (1 << CS11) | (1 << CS12); // external clock from 5th pin
  TIMSK1 |= (1 << OCIE1A);  // Output Compare Match A Interrupt Enable


  // set compare match register for 62.00396825396825 Hz increments
  TCCR0A = 0;
  TCCR0B = 0;
  TCNT0  = 0;
  OCR0A = 255; // = 16000000 / (1024 * 62.00396825396825) - 1 (must be <256)
  TCCR0B |= (1 << WGM02);
  TCCR0B |= (1 << CS02) | (0 << CS01) | (1 << CS00); // 1024 prescaler
  TIMSK0 |= (1 << OCIE0A);


#ifdef __EMULATE_MODE__
  TCCR2A = 0;
  TCCR2B = 0;
  TCNT2 = 0;
  OCR2A = 60;
  TCCR2A |= (1 << WGM21);
  // prescaler
  // (0 << CS02) | (1 << CS01) | (0 << CS00); // 8
  // (0 << CS02) | (1 << CS01) | (1 << CS00); // 64
  // (1 << CS02) | (0 << CS01) | (0 << CS00); // 256
  // (1 << CS02) | (0 << CS01) | (1 << CS00); // 1024
  TCCR2B |=  (0 << CS02) | (1 << CS01) | (0 << CS00); // 8

  TIMSK2 |= (1 << OCIE2A);
#endif
  interrupts();

  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);

  readFromEEPROM(&(phases[1]));

  initDispl = true;
  needToUpdate = true;
}

void loop_c() {
  if (mode != 1 && needToDisplay && !updatingDisplay) {
    needToDisplay = false;
    displaying = true;
    display.display();
    displaying = false;
  }
}

void storeToEEPROM() {
  uint16_t existedPhases[3];
  readFromEEPROM(existedPhases);

  storeToEEPROMByN(existedPhases, 0);
  storeToEEPROMByN(existedPhases, 1);
  storeToEEPROMByN(existedPhases, 2);

  readFromEEPROM(existedPhases);
}

/**
 * n - 0 , 1, 2
 */
void storeToEEPROMByN(uint16_t* existedPhases, uint8_t n) {
  if (existedPhases[n] != phases[n+1]) {
    eeprom_write_word(ADDR_EEPROM_FIRST +n*2, phases[n+1]);
  }
}

void readFromEEPROM(uint16_t* arr) {
  readFromEEPROMByN(arr, 0);
  readFromEEPROMByN(arr, 1);
  readFromEEPROMByN(arr, 2);
}

/**
 * n - 0 , 1, 2
 */
void readFromEEPROMByN(uint16_t* arr, uint8_t n) {
  uint16_t phase = eeprom_read_word(ADDR_EEPROM_FIRST + n * 2);
  if (phase != 0xffff) {
    arr[n] = phase;
  }
}