#include <U8g2lib.h>
#include <Wire.h>
#include "tamalib.h"
#include "hw.h"
#include "bitmaps.h"
#include "hardcoded_state.h"
#include "savestate.h"

/***** U8g2 SSD1306 Library Setting *****/
#define DISPLAY_I2C_ADDRESS 0x3C
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
/****************************************/

/***** Tama Setting and Features *****/
#define TAMA_DISPLAY_FRAMERATE 12
#define ENABLE_TAMA_SOUND
#define ENABLE_REAL_TIME
#define ENABLE_SAVE_STATUS
#define AUTO_SAVE_MINUTES 10   // Auto save for every 10 minutes
#define ENABLE_LOAD_STATE_FROM_EEPROM 
#define EMULATOR_CLOCK_SPEED 1000000 
/***************************/

/***** Set display orientation, U8G2_MIRROR_VERTICAL is not supported *****/
//#define U8G2_LAYOUT_NORMAL
#define U8G2_LAYOUT_ROTATE_180
//#define U8G2_LAYOUT_MIRROR
/**************************************************************************/

#ifdef U8G2_LAYOUT_NORMAL1002714
U8G2_SSD1306_128X64_NONAME_2_HW_I2C display(U8G2_R0);
#endif

#ifdef U8G2_LAYOUT_ROTATE_180
U8G2_SSD1306_128X64_NONAME_2_HW_I2C display(U8G2_R2);
#endif

#ifdef U8G2_LAYOUT_MIRROR
U8G2_SSD1306_128X64_NONAME_2_HW_I2C display(U8G2_MIRROR);
#endif

#define PIN_BTN_L 12 // D6
#define PIN_BTN_M 13 // D7
#define PIN_BTN_R 15 // D8
#define PIN_BUZZER 0 // D3

/**** TamaLib Specific Variables ****/
static uint16_t current_freq = 0; 
static bool_t matrix_buffer[LCD_HEIGHT][LCD_WIDTH/8] = {{0}};
static byte runOnceBool = 0;
static bool_t icon_buffer[ICON_NUM] = {0};
static cpu_state_t cpuState;
static unsigned long lastSaveTimestamp = 0;
/************************************/

static void hal_halt(void) {
  //Serial.println("Halt!");
}

static void hal_log(log_level_t level, char *buff, ...) {
  Serial.println(buff); 
}

static void hal_sleep_until(timestamp_t ts) {
  #ifdef ENABLE_REAL_TIME
  int32_t remaining = (int32_t)(ts - hal_get_timestamp());
  if (remaining > 0) {
    delayMicroseconds(remaining);
  }
  #endif
}

// Get the current timestamp in microseconds
static timestamp_t hal_get_timestamp(void) {
  return micros();
}

static void hal_update_screen(void) {
  displayTama();
} 

static void hal_set_lcd_matrix(u8_t x, u8_t y, bool_t val) {
  uint8_t mask;
  if (val) {
    mask = 0b10000000 >> (x % 8);
    matrix_buffer[y][x/8] = matrix_buffer[y][x/8] | mask;   
  } else { 
    mask = 0b01111111;
    for(byte i=0;i<(x % 8);i++) {
      mask = (mask >> 1) | 0b10000000;
    }
    matrix_buffer[y][x/8] = matrix_buffer[y][x/8] & mask;  
  }
}

static void hal_set_lcd_icon(u8_t icon, bool_t val) {
  icon_buffer[icon] = val;
}

static void hal_set_frequency(u32_t freq) {
  current_freq = freq;
}

static void hal_play_frequency(bool_t en) {
#ifdef ENABLE_TAMA_SOUND 
  if (en) {
    tone(PIN_BUZZER, current_freq); 
  } else {
    noTone(PIN_BUZZER); 
  }
#endif  
}

static bool_t button4state = 0;

static int hal_handler(void) {
#ifdef ENABLE_SERIAL_DUMP 
  if (Serial.available() > 0) {
    int incomingByte = Serial.read();
    Serial.println(incomingByte, DEC);
    if (incomingByte==48) {          // 0
      dumpStateToSerial();
    }
  }
#endif
#ifdef ENABLE_SERIAL_DEBUG_INPUT 
  if (Serial.available() > 0) {
    int incomingByte = Serial.read();
    Serial.println(incomingByte, DEC);
    if (incomingByte==49) {  // 1
      hw_set_button(BTN_LEFT, BTN_STATE_PRESSED );
    } else if (incomingByte==52) {  // 4 which is above 1 on a pad
      hw_set_button(BTN_LEFT, BTN_STATE_RELEASED );
    } else if (incomingByte==50) {  // 2
      hw_set_button(BTN_MIDDLE, BTN_STATE_PRESSED );
    } else if (incomingByte==53) {  // 5 which is above 2 on a pad
      hw_set_button(BTN_MIDDLE, BTN_STATE_RELEASED );
    } else if (incomingByte==51) {  // 3
      hw_set_button(BTN_RIGHT, BTN_STATE_PRESSED );
    } else if (incomingByte==54) {  // 6 which is above 3 on a pad
      hw_set_button(BTN_RIGHT, BTN_STATE_RELEASED );
    }  
  } 
#else  
  if (digitalRead(PIN_BTN_L) == HIGH) {
    hw_set_button(BTN_LEFT, BTN_STATE_PRESSED );
  } else {
    hw_set_button(BTN_LEFT, BTN_STATE_RELEASED );
  }
  if (digitalRead(PIN_BTN_M) == HIGH) {
    hw_set_button(BTN_MIDDLE, BTN_STATE_PRESSED );
  } else {
    hw_set_button(BTN_MIDDLE, BTN_STATE_RELEASED );
  }
  if (digitalRead(PIN_BTN_R) == HIGH) {
    hw_set_button(BTN_RIGHT, BTN_STATE_PRESSED );
  } else {
    hw_set_button(BTN_RIGHT, BTN_STATE_RELEASED );
  }
#endif  
  return 0;
}

static hal_t hal = {
  .halt = &hal_halt,
  .log = &hal_log,
  .sleep_until = &hal_sleep_until,
  .get_timestamp = &hal_get_timestamp,
  .update_screen = &hal_update_screen,
  .set_lcd_matrix = &hal_set_lcd_matrix,
  .set_lcd_icon = &hal_set_lcd_icon,
  .set_frequency = &hal_set_frequency,
  .play_frequency = &hal_play_frequency,
  .handler = &hal_handler,
};

void drawTamaRow(uint8_t tamaLCD_y, uint8_t ActualLCD_y, uint8_t thick) {
  uint8_t i;
  for (i = 0; i < LCD_WIDTH; i++) {
    uint8_t mask = 0b10000000;
    mask = mask >> (i % 8);
    if ((matrix_buffer[tamaLCD_y][i/8] & mask) != 0) {
        display.drawBox(i+i+i+16, ActualLCD_y, 2, thick);
    }
  }
}

void drawTamaSelection(uint8_t y) {
  uint8_t i;
  for (i = 0; i < 8; i++) {
    if (icon_buffer[i]) {
      display.drawXBMP(i * 16 + 4, y + 6, 8, 8, bitmaps + i * 8);
    }
  }
}

void displayTama() {
  uint8_t j;
  display.firstPage();
#ifdef U8G2_LAYOUT_ROTATE_180
  drawTamaSelection(49);
  display.nextPage();

  for (j = 11; j < LCD_HEIGHT; j++) {
    drawTamaRow(j, j + j + j, 2);
  }
  display.nextPage();

  for (j = 5; j <= 10; j++) {
    if (j == 5) {
      drawTamaRow(j, j + j + j + 1, 1);
    } else {
      drawTamaRow(j, j + j + j, 2);
    }
  }
  display.nextPage();

  for (j = 0; j <= 5; j++) {
    if (j == 5) {
      drawTamaRow(j, j + j + j, 1);
    } else {
      drawTamaRow(j, j + j + j, 2);
    }    
  }
  display.nextPage();
#else
  for (j = 0; j < LCD_HEIGHT; j++) {
    if (j != 5) drawTamaRow(j, j + j + j, 2);
    if (j == 5) {
      drawTamaRow(j, j + j + j, 1);
      display.nextPage();
      drawTamaRow(j, j + j + j + 1, 1);
    }
    if (j == 10) display.nextPage();
  }
  display.nextPage();
  drawTamaSelection(49);
  display.nextPage();
#endif  
}

#if defined(ENABLE_DUMP_STATE_TO_SERIAL_WHEN_START) || defined(ENABLE_SERIAL_DUMP)
void dumpStateToSerial() {
  uint16_t i, count = 0;
  char tmp[10];
  cpu_get_state(&cpuState);
  u4_t *memTemp = cpuState.memory;
  uint8_t *cpuS = (uint8_t *)&cpuState;

  Serial.println("");
  Serial.println("static const uint8_t hardcodedState[] PROGMEM = {");
  for (i = 0; i < sizeof(cpu_state_t); i++, count++) {
    sprintf(tmp, "0x%02X,", cpuS[i]);
    Serial.print(tmp);
    if ((count % 16) == 15) Serial.println("");
  }
  for (i = 0; i < MEMORY_SIZE; i++, count++) {
    sprintf(tmp, "0x%02X,", memTemp[i]);
    Serial.print(tmp);
    if ((count % 16) == 15) Serial.println("");
  }  
  Serial.println("};");
}
#endif

uint8_t reverseBits(uint8_t num) {
  uint8_t reverse_num = 0;
  uint8_t i;
  for (i = 0; i < 8; i++) {
    if ((num & (1 << i)))
      reverse_num |= 1 << ((8 - 1) - i);  
  }
  return reverse_num;
}

void setup() {
  Serial.begin(9600);
  pinMode(PIN_BTN_L, INPUT);
  pinMode(PIN_BTN_M, INPUT);
  pinMode(PIN_BTN_R, INPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

  display.setI2CAddress(DISPLAY_I2C_ADDRESS * 2);  // required if display does not use default address of 0x3C
  display.begin();  // initialize U8g2 graphics library for selected display module

  tamalib_register_hal(&hal);
  tamalib_set_framerate(TAMA_DISPLAY_FRAMERATE);
  // Tamagochi clock init
  tamalib_init(EMULATOR_CLOCK_SPEED);

#if defined(ENABLE_SAVE_STATUS) || defined(AUTO_SAVE_MINUTES) || defined(ENABLE_LOAD_STATE_FROM_EEPROM)
  initEEPROM();
#endif

#ifdef ENABLE_LOAD_STATE_FROM_EEPROM
  if (validEEPROM()) {
    loadStateFromEEPROM(&cpuState);
  } else {
    Serial.println(F("No magic number in state, skipping state restore"));
  }
#elif defined(ENABLE_LOAD_HARCODED_STATE_WHEN_START)
  loadHardcodedState(&cpuState);
#endif

#ifdef ENABLE_DUMP_STATE_TO_SERIAL_WHEN_START
  dumpStateToSerial();
#endif 
}

uint32_t middle_long_press_started = 0;
uint32_t right_long_press_started = 0;
bool is_display_off = false;
const uint32_t AUTO_SAVE_INTERVAL = AUTO_SAVE_MINUTES * 60 * 1000;

void loop() {
  tamalib_mainloop_step_by_step();
  
  unsigned long currentMillis = millis();
  
#ifdef AUTO_SAVE_MINUTES    
  if ((currentMillis - lastSaveTimestamp) > AUTO_SAVE_INTERVAL) {
    digitalWrite(LED_BUILTIN, LOW);
    lastSaveTimestamp = currentMillis;
    saveStateToEEPROM(&cpuState);
    digitalWrite(LED_BUILTIN, HIGH);
  }
#endif

  bool middleLeftPressed = (digitalRead(PIN_BTN_M) == HIGH) && (digitalRead(PIN_BTN_L) == HIGH);
  bool rightPressed = (digitalRead(PIN_BTN_R) == HIGH);

  if (middleLeftPressed) {
    if ((currentMillis - middle_long_press_started) > 2000) {
      eraseStateFromEEPROM();
#if defined(ESP8266) || defined(ESP32)
      ESP.restart();
#endif
    }
  } else {
    middle_long_press_started = currentMillis;
  }

  if (rightPressed) {
    if ((currentMillis - right_long_press_started) > 2000) {
      if (!is_display_off) {
        display.sleepOn();
        is_display_off = true;
      }
    } else if (is_display_off) {
      display.sleepOff();
      is_display_off = false;
    }
  } else {
    right_long_press_started = currentMillis;
  }
}