#include <avr/io.h>
#include <avr/pgmspace.h>
#include <util/delay.h>

#include "display.h"
#include "font.h"

uint8_t cursor_y = 0;
uint8_t cursor_x = 0;

static uint8_t i2c_transfer(uint8_t usisr_val) {
  USISR = usisr_val;
  // 2-wire i2c + (USICS1+USICLK = timer0 clock) + 3-wire master
  uint8_t cr = (1 << USIWM1) | (1 << USICS1) | (1 << USICLK) | (1 << USITC);
  do {
    _delay_us(I2C_DELAY_US);
    USICR = cr;
    while (!(PINA & (1 << DISPLAY_SCL)))
      ; // Wait for next clock falling edge
    _delay_us(I2C_DELAY_US);
    USICR = cr;
  } while (!(USISR & (1 << USIOIF)));
  _delay_us(I2C_DELAY_US);

  uint8_t data = USIDR;
  USIDR = 0xFF;
  DDRA |= (1 << DISPLAY_SDA);
  return data;
}

static void i2c_init(void) {
  PORTA |= (1 << DISPLAY_SDA) | (1 << DISPLAY_SCL);
  DDRA |= (1 << DISPLAY_SDA) | (1 << DISPLAY_SCL);
  USIDR = 0xFF;
  // 2-wire i2c + (USICS1+USICLK = timer0 clock)
  USICR = (1 << USIWM1) | (1 << USICS1) | (1 << USICLK);
  // Start interrupt + overflow interrupt + stop interrupt + output collision
  USISR = (1 << USISIF) | (1 << USIOIF) | (1 << USIPF) | (1 << USIDC);
}

void i2c_start(void) {
  PORTA |= (1 << DISPLAY_SCL);
  while (!(PINA & (1 << DISPLAY_SCL)))
    ; // Wait for next clock falling edge
  _delay_us(I2C_DELAY_US);
  PORTA &= ~(1 << DISPLAY_SDA); // SDA low while SCL high = START
  _delay_us(I2C_DELAY_US);
  PORTA &= ~(1 << DISPLAY_SCL);
  PORTA |= (1 << DISPLAY_SDA);
}

void i2c_stop(void) {
  PORTA &= ~(1 << DISPLAY_SDA);
  _delay_us(I2C_DELAY_US);
  PORTA |= (1 << DISPLAY_SCL);
  while (!(PINA & (1 << DISPLAY_SCL)))
    ;
  _delay_us(I2C_DELAY_US);
  PORTA |= (1 << DISPLAY_SDA); // SDA high while SCL high = STOP
  _delay_us(I2C_DELAY_US);
}

void i2c_write(uint8_t data) {
  PORTA &= ~(1 << DISPLAY_SCL);
  USIDR = data;
  // transfer 8 bits: counter=0 overflows after 16 edges (8 clocks)
  i2c_transfer((1 << USISIF) | (1 << USIOIF) | (1 << USIPF) | (1 << USIDC) |
               0x00);
  DDRA &= ~(1 << DISPLAY_SDA);
  // transfer 1 bit: counter=0x0E overflows after 2 edges (1 clock)
  i2c_transfer((1 << USISIF) | (1 << USIOIF) | (1 << USIPF) | (1 << USIDC) |
               0x0E);
}

void display_cmd(uint8_t cmd) {
  i2c_start();
  i2c_write(OLED_ADDR);
  i2c_write(0x00); // Control byte: Co=0, D/C#=0
  i2c_write(cmd);
  i2c_stop();
}

void display_clear(void) {
  for (uint8_t page = 0; page < 8; page++) {
    display_cmd(DCMD_SET_PAGE_START | page);
    display_cmd(0x00);
    display_cmd(DCMD_SET_START_ADDR_HIGHER_COL);
    i2c_start();
    i2c_write(OLED_ADDR);
    i2c_write(0x40);
    for (uint8_t col = 0; col < 128; col++)
      i2c_write(0x00);
    i2c_stop();
  }
}

void display_init(void) {
  i2c_init();
  _delay_ms(40);

  display_cmd(DCMD_DISPLAY_OFF);
  display_cmd(DCMD_SET_CLOCK_DIV);
  display_cmd(0x80);
  display_cmd(DCMD_SET_MUX_RATIO);
  display_cmd(63); // 64 rows
  display_cmd(DCMD_SET_OFFSET);
  display_cmd(0x00);
  display_cmd(DCMD_SET_START_LINE);
  display_cmd(DCMD_SET_CHARGE_PUMP);
  display_cmd(DCMD_CHARGE_PUMP_ENABLE);
  display_cmd(DCMD_SET_ADDRESSING_MODE);
  display_cmd(DCMD_ADDR_HORIZONTAL);
  display_cmd(DCMD_SET_SEGMENT_REMAP);
  display_cmd(DCMD_SET_COM_SCAN_DIRECTION);
  display_cmd(DCMD_SET_COM_PINS);
  display_cmd(0x12);
  display_cmd(DCMD_SET_CONTRAST);
  display_cmd(207);
  display_cmd(DCMD_SET_PRECHARGE_PERIOD);
  display_cmd(0xF1);
  display_cmd(DCMD_SET_VCOMH_DESELECT);
  display_cmd(0x40);
  display_cmd(DCMD_OUTPUT_FOLLOWS_RAM);
  display_cmd(DCMD_NORMAL_MODE);
  display_cmd(DCMD_DISPLAY_ON);

  display_clear();
}

static uint8_t scale_column(uint8_t font_byte, uint8_t page) {
  uint8_t out = 0;
  for (uint8_t b = 0; b < 8; b++) {
    uint8_t src = (page * 8 + b) / FONT_SCALE_Y;
    if (src < FONT_GLYPH_HEIGHT && (font_byte & (1 << src)))
      out |= (1 << b);
  }
  return out;
}

void display_draw_char(uint8_t c) {
  if (c < FONT_GLYPH_FIRST_CHAR || c > FONT_GLYPH_LAST_CHAR)
    return;
  uint16_t offset = (uint16_t)(c - FONT_GLYPH_FIRST_CHAR) * FONT_GLYPH_WIDTH;

  uint8_t pages = (FONT_GLYPH_HEIGHT * FONT_SCALE_Y + 7) / 8;
  for (uint8_t p = 0; p < pages; p++) {
    display_cmd(DCMD_SET_PAGE_START | ((cursor_y + p) & 0x07));
    display_cmd(cursor_x & 0x0F);
    display_cmd(DCMD_SET_START_ADDR_HIGHER_COL | (cursor_x >> 4));

    i2c_start();
    i2c_write(OLED_ADDR);
    i2c_write(0x40);
    for (uint8_t col = 0; col < FONT_GLYPH_WIDTH; col++) {
      uint8_t glyph = pgm_read_byte(&FONT_BITMAP[offset + col]);
      uint8_t scaled = scale_column(glyph, p);
      for (uint8_t r = 0; r < FONT_SCALE_X; r++)
        i2c_write(scaled);
    }
    for (uint8_t r = 0; r < FONT_SCALE_X; r++)
      i2c_write(0x00);
    i2c_stop();
  }

  cursor_x += (FONT_GLYPH_WIDTH + CHAR_SPACING) * FONT_SCALE_X;
}

void display_draw_float2(float num) {
  cursor_x = 5;
  cursor_y = 0;

  if (num < 0) {
    display_draw_char('-');
    num = -num;
  }

  int val = (int)(num * 10); // include 1 decimal place
  int digit;
  uint8_t is_first = 1;

  for (uint16_t i = 1000; i >= 1; i /= 10, is_first = 0) {
    // 2 digits left means tis decimal time
    if (i == 1)
      display_draw_char('.');
    digit = (val / i) % 10;
    val %= i;
    // if first digit is 0, draw space (tis garbage code)
    if (is_first && digit == 0)
      display_draw_char(' ');
    else
      display_draw_char(digit + '0');
  }
}