#include "Arduino.h"
#include <avr/pgmspace.h>

#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
#define ENCODER_PIN_A 2
#define ENCODER_PIN_B 3

#include <Bounce2.h>

//3D_Cube for Arduino OLED module by Colin Ord, 9/1/2015
//A port of my original JustBasic Cube_3D demo to the Arduino Uno using U8G library.
//Ported to U8G2 by Okubo Heavy Industries
//#define U8X8_USE_ARDUINO_AVR_SW_I2C_OPTIMIZATION

#include <U8g2lib.h>

#include "ui.h"

//The following line will need changing depending on your screen

//U8G2_SSD1306_128X32_UNIVISION_F_SW_I2C u8g2(U8G2_R0, 7, 8); // Amazon 128x32 i2c OLEDs
//U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0);    // Amazon 128x64 i2c OLED
//U8G2_SSD1309_128X64_NONAME0_F_4W_HW_SPI u8g2(U8G2_R0, 10, 9);

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0);

static const unsigned char radio_glyph [] PROGMEM = {
  0x60, 0x00, 
  0x80, 0x01,
  0x00, 0x06, 
  0x00, 0x18, 
  0xFC, 0x3F,  
  0x02, 0x40,  
  0x32, 0x5E, 
  0x7A, 0x40,
  0x7A, 0x5E,  
  0x32, 0x40, 
  0x02, 0x40,
  0xFC, 0x3F
};

Bounce user_sw;
Encoder myEnc(ENCODER_PIN_A, ENCODER_PIN_B);
UIState ui;

void UI_Init(UIState * ui, unsigned long now) {
  ui->config_depth = 0;
  ui->config_mode_enabled = false;
  ui->config_selection_0 = 0;
  ui->config_offset_0 = 0;
  ui->config_selection_1 = 0;
  ui->config_selection_2 = 0;
  ui->display_choice = 0;
  ui->display_error_code = NoMessage;
  ui->error_code = NoMessage;
  ui->last_display_update_ms = now;
  ui->last_encoder_pos = 0;
  ui->last_error_display = now;
  ui->event = NONE;
  ui->last_event = now;
}

void UI_encoder_button_press(UIState * ui, unsigned long now) {
  // simple de-bounce
  if (ui->last_event == BUTTON_PRESS) {
    if ((now - ui->last_event) > 50) {
      ui->event = BUTTON_PRESS;
      ui->last_event = now;
    }
  } else {
    ui->event = BUTTON_PRESS;
    ui->last_event = now;
  }
}

void UI_encoder_position(UIState * ui, unsigned long now, int pos) {
  if (pos > ui->last_encoder_pos + 2) {
    ui->last_encoder_pos = pos;
    ui->event = SCROLL_RIGHT;
    ui->last_event = now;
  } else if (pos < ui->last_encoder_pos - 2) {
    ui->last_encoder_pos = pos;
    ui->event = SCROLL_LEFT;
    ui->last_event = now;
  }
}

UserEvent UI_next_event(UIState * ui, unsigned long now) {
  UserEvent e = ui->event;
  ui->event = NONE;
  return e;
}


ISR (PCINT2_vect) {
  bool pin_state = digitalRead(5);
  // If the button goes high, we rose
  if (pin_state) {
    UI_encoder_button_press(&ui, millis());
  }
}

// Defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

void setup(void){
  user_sw = Bounce();
  user_sw.attach(5, INPUT);
  user_sw.interval(5);

  UI_Init(&ui, millis());

  cli();
  // Enable pin change interrupts on port d
  sbi(PCICR, PCIE2);
  // Set pin change mask for PD5
  sbi(PCMSK2, PCINT21);
  sei();

  u8g2.begin();
  u8g2.clearDisplay();
}

// u8g2_font_profont29_mn large font, only numbers
// u8g2_font_profont15_mr, medium font text and numbers
// u8g2_font_battery19_tn, battery symbols

char batt_ch = 48;
char * batt_s = {0};
bool batt_up = true;

static const unsigned char check_s [] PROGMEM = {
 0xf0,0xf0,0xf8,0xdb,0xdf,0xce,0xcc};

static const unsigned char x[] PROGMEM = {
 0xf3,0xf3,0xde,0xcc,0xde,0xf3,0xf3};

static const unsigned char interrogative [] PROGMEM = {
 0xde,0xf3,0xf3,0xd8,0xcc,0xc0,0xcc,0xcc};

static const unsigned char plug [] PROGMEM = {
 0x20,0xf0,0xe0,0xf1,0xef,0xf7,0xef,0xff,0xe0,0xff,0xe0,0xff,
 0xe0,0xff,0xef,0xff,0xef,0xf7,0xe0,0xf1,0x20,0xf0};

// These are allocated in SRAM since they need reference to global objects
struct DynamicConfig {
  float * value;
  VoltageRead * derived;
};

struct DynamicConfig dynamicConfigs[NUM_CONFIGS + NUM_TUNABLES] = {
  {&(configs.standby_voltage), NULL},
  {&(configs.batt_recovery_ms), NULL},
  {&(configs.batt_float_voltage), NULL},
  {&(configs.batt_high_voltage_cutoff), NULL},
  {&(configs.batt_high_voltage_recovery), NULL},
  {&(configs.batt_high_voltage_trip_ms), NULL},
  {&(configs.batt_low_voltage_cutoff), NULL},
  {&(configs.batt_low_voltage_recovery), NULL},
  {&(configs.batt_low_voltage_trip_ms), NULL},
  {&(configs.overtemp_cutoff), NULL},
  {&(configs.overtemp_recovery), NULL},
  {&(configs.overtemp_recovery_ms), NULL},
  {&(configs.current_factor), NULL},
  {&(configs.current_offset), NULL},
  {&(configs.reference_voltage), NULL},
  {&(configs.voltage_factor), NULL},
  {&(configs.voltage_offset), NULL}
};

const struct StaticConfig staticConfigs[NUM_CONFIGS + NUM_TUNABLES] PROGMEM = {
  {"B1", "Battery", "Standby Voltage", 0, -1, 1},
  {"B2", "Battery", "Recovery ms", 3, 0, 0},
  {"B3", "Battery", "Charging Voltage", 0, -1, 1},
  {"H1", "Battery High", "Voltage Cutoff", 0, -1, 1},
  {"H2", "Battery High", "Voltage Recovery", 0, -1, 1},
  {"H2", "Battery High", "Trip Time ms", 3, 0, 0},
  {"L1", "Battery Low", "Voltage Cutoff", 0, -1, 1},
  {"L2", "Battery Low", "Voltage Recovery", 0, -1, 1},
  {"L3", "Battery Low", "Trip Time ms", 2, 0, 0},
  {"T1", "Temperature", "Cutoff Temp", 1, -1, 1},
  {"T2", "Temperature", "Recovery Temp", 1, -1, 1},
  {"T3", "Temperature", "Recovery Time ms", 3, 0, 0},
  {"X1", "Current Sense", "", -1, -3, 3},
  {"X2", "Current Offset", "", 0, -3, 3},
  {"X3", "ADC Voltage", "", 0, -3, 3},
  {"X4", "Voltage Factor", "", 0, -3, 3},
  {"X5", "Voltage Offset", "", 0, -3, 3},
};

void display_main(UIState * ui, unsigned long now) {
  u8g2.clearBuffer();					// clear the internal memory

  UserEvent event = UI_next_event(ui, now);
  if (event == SCROLL_RIGHT) {
    ui->display_choice ++;
  } else if (event == SCROLL_LEFT) {
    ui->display_choice --;
  } else if (event == BUTTON_PRESS) {
    ui->config_mode_enabled = true;
  }

  uint8_t display_choice = ui->display_choice % 4;

  u8g2.setFont(u8g2_font_profont11_tr);	// choose a suitable font 
  switch (display_choice) {
    case 0:
      u8g2.drawStr(0, 7, "Battery Voltage");	
      break;
    case 1:
      u8g2.drawStr(0, 7, "Supply Voltage");	
      break;
    case 2:
      u8g2.drawStr(0, 7, "Load Current");	
      break;
    case 3:
      u8g2.drawStr(0, 7, "Temperature");	
      break;
  } 
  u8g2.drawStr(100, 7, "RPi");
  u8g2.drawStr(100, 19, "OS");
  u8g2.drawStr(100, 31, "Py");
  u8g2.drawStr(100, 43, "BPQ");
  //u8g2.drawStr(0, 56, "YOUR AD");
  //u8g2.drawStr(0, 64, "GOES HERE");
  u8g2.drawStr(0, 44, "PSU");

  u8g2.drawXBMP(122, 0, 6, 7, check_s);
  u8g2.drawXBMP(122, 12, 6, 7, check_s);
  u8g2.drawXBMP(122, 24, 6, 7, x);
  u8g2.drawXBMP(122, 36, 6, 8, interrogative);


  u8g2.setFont(u8g2_font_profont29_mn);	// choose a suitable font 
  switch (display_choice) {
    case 0:
      u8g2.drawStr(0, 30, "12.41");
      break;
    case 1:
      u8g2.drawStr(0, 30, "13.85");
      break;
    case 2:
      u8g2.drawStr(0, 30, "3.00");
      break;
    case 3:
      u8g2.drawStr(0, 30, "80.5");
      break;
  }
  
  // Radio bitmap
  //u8g2.drawXBMP(0, 32, 12, 11, plug);
  //u8g2.drawHLine(12, 37, 28);
  u8g2.drawXBMP(40, 32, 16, 12, radio_glyph);
  
  // Battery icons
  u8g2.setFont(u8g2_font_battery19_tn);
  batt_s[0] = batt_ch;
  u8g2.drawStr(80, 30, batt_s);
  u8g2.drawHLine(0, 46, 128);
  u8g2.drawVLine(96, 0, 46);
  
  // If an error code has been set, copy it and display it for some time
  if (ui->error_code != NoMessage) {
    if ((now - ui->last_error_display) > 2000) {
      ui->display_error_code = ui->error_code;
      ui->last_error_display = now;
      ui->error_code = NoMessage;
      strcpy_P(ui->message_line_1, (char*)pgm_read_word(&(error_messages[ui->display_error_code])));
      strcpy_P(ui->message_line_2, error_0);
    }
  }

  // Display a message
  u8g2.setFont(u8g2_font_profont11_tr);
  if (ui->display_error_code != NoMessage) {
    if ((now - ui->last_error_display) > 3000) {
      ui->display_error_code = NoMessage;
      strcpy_P(ui->message_line_1, error_0);
      strcpy_P(ui->message_line_2, error_0);
      ui->last_error_display = now;
    }
    u8g2.drawStr(0, 56, ui->message_line_1);
    u8g2.drawStr(0, 64, ui->message_line_2);
  } else {
    u8g2.drawStr(0, 56, "YOUR AD");
    u8g2.drawStr(0, 64, "GOES HERE");
  }

  u8g2.sendBuffer();					// transfer internal memory to the display
  if ((now - ui->last_display_update_ms) > 500) {
    if (batt_up) {
      batt_ch++;
    } else {
      batt_ch--;
    }
    if (batt_ch == 55) {
      batt_ch--;
      batt_up = false;
    }
    if (batt_ch == 47) {
      batt_ch++;
      batt_up = true;
    }
    ui->last_display_update_ms = now;
  }

}

void cycle_selection(uint8_t * var, uint8_t len, bool inc) {
  uint8_t val = *var;
  if (inc) {
    if (val == (len - 1)) {
      *(var) = 0;
    } else {
      *(var) = (val + 1);
    }
  } else {
    if (val == 0) {
      *(var) = (len - 1);
    } else {
      *(var) = (val - 1);
    }
  }
}

void display_set_levels(UIState * ui, UserEvent e, unsigned long now) {
  u8g2.clearBuffer();			
  u8g2.setFont(u8g2_font_profont11_tr);
  u8g2.drawStr(0, 7, "Set Limits");	
  u8g2.drawHLine(0, 9, 128);

  if (e == SCROLL_RIGHT) {
    cycle_selection(&(ui->config_selection_1), menu_1_len, false);
  }
  if (e == SCROLL_LEFT) {
    cycle_selection(&(ui->config_selection_1), menu_1_len, true);
  }
  if (e == BUTTON_PRESS) {

  }

  if (ui->config_selection_1 == 0) {
      ui->config_offset_1 = 0;
  }

  if (ui->config_selection_1 == menu_1_len - 1) {
    ui->config_offset_1 = menu_1_len - 5;
  }

  if (ui->config_selection_1 > (ui->config_offset_1 + 3)) {
    ui->config_offset_1++;
  }

  if (ui->config_selection_1 < ui->config_offset_1) {
    ui->config_offset_1--;
  }

  for (uint8_t i = 0; i < 4; i++) {
    uint8_t idx = i + ui->config_offset_1;
    memcpy_P(&staticConfigSRAM, &staticConfigs[ui->config_selection_1], sizeof(StaticConfig));
    //strcpy_P(ui->menu_buffer, (char*)pgm_read_word(&(menu_1[idx])));
    if (ui->config_selection_1 == idx) {
      u8g2.drawStr(0, 20 + 12*i, ">");	
    }
    u8g2.drawStr(10, 20 + 12*i, ui->menu_buffer);	
  }

  u8g2.sendBuffer();
  return;


}

void display_config(UIState * ui, UserEvent e, unsigned long now) {
  u8g2.clearBuffer();			
  u8g2.setFont(u8g2_font_profont11_tr);
  u8g2.drawStr(0, 7, "Config");	
  u8g2.drawHLine(0, 9, 128);

  if (ui->config_depth == 0) {
    if (e == SCROLL_RIGHT) {
      cycle_selection(&(ui->config_selection_0), menu_0_len, false);
    }
    if (e == SCROLL_LEFT) {
      cycle_selection(&(ui->config_selection_0), menu_0_len, true);
    }
    if (e == BUTTON_PRESS) {
      switch (ui->config_selection_0) {
        case 0:
          // Set Limits
          ui->config_depth = 1;
          ui->config_selection_1 = 0;
          break;
        case 2:
          //save_config();
          ui->error_code = EEPROMSavedMessage;
          ui->config_selection_0 = 0;
          ui->config_selection_1 = 0;
          ui->config_mode_enabled = false;
          break;
        case 3:
          //reset_config();
          ui->error_code = EEPROMResetMessage;
          ui->config_selection_0 = 0;
          ui->config_selection_1 = 0;
          ui->config_mode_enabled = false;
          break;
        case 4:
          //shutdown_rpi();
          ui->error_code = RPIShutdownMessage;
          ui->config_selection_0 = 0;
          ui->config_selection_1 = 0;
          ui->config_mode_enabled = false;
          break;
        default:
          ui->config_selection_0 = 0;
          ui->config_selection_1 = 0;
          ui->config_mode_enabled = false;
          break;
      }
      return;
    }
    
    if (ui->config_selection_0 == 0) {
      ui->config_offset_0 = 0;
    }

    if (ui->config_selection_0 == menu_0_len - 1) {
      ui->config_offset_0 = menu_0_len - 5;
    }

    if (ui->config_selection_0 > (ui->config_offset_0 + 3)) {
      ui->config_offset_0++;
    }

    if (ui->config_selection_0 < ui->config_offset_0) {
      ui->config_offset_0--;
    }

    for (uint8_t i = 0; i < 4; i++) {
      uint8_t idx = i + ui->config_offset_0;
      strcpy_P(ui->menu_buffer, (char*)pgm_read_word(&(menu_0[idx])));
      if (ui->config_selection_0 == idx) {
        u8g2.drawStr(0, 20 + 12*i, ">");	
      }
      u8g2.drawStr(10, 20 + 12*i, ui->menu_buffer);	
    }

    u8g2.sendBuffer();
    return;
  }

  if (ui->config_depth == 1) {
    switch (ui->config_selection_0) {
      case 0:
        display_set_levels(ui, e, now);
        break;
      case 1:
        //display_set_levels(now, event, false, NUM_TUNABLES);
        break;
      case 2:
        //display_info(now, event);
        break;
    } 
    return;
  }

  if (ui->config_depth == 2) {
    if (ui->config_selection_0 == 0) {
        //adjust_set_levels(now, event, true, NUM_CONFIGS);
    } else if (ui->config_selection_0 == 1) {
        //adjust_set_levels(now, event, false, NUM_TUNABLES);
    }
  }
}

void loop(void) {
  unsigned long now = millis();

  UI_encoder_position(&ui, now, myEnc.read());

  if (ui.config_mode_enabled) {
    UserEvent e = UI_next_event(&ui, now);
    display_config(&ui, e, now);
  } else {
    display_main(&ui, now);
  } 
}