#include <util/twi.h>
#include <stdio.h>
#include <util/delay.h>
#include <string.h>
#include "i2c.h"
#include "ssd1306_display_driver.h"


#define SSD1306_ADDR      0x3c
#define SSD1306_CMD_REG   0x00
#define SSD1306_DATA_REG  0x40

//on the datasheet we have 32 extra rows (4 more pages) of GDRAM that don't fit on the display; we just ignore them everywhere
#define SCREEN_PAGE_CNT   SCREEN_RES_Y / 8 

#define I2C_WRITE   0
#define I2C_READ    1



void screen_init() {
#ifdef DEBUG_BUILD
    printf("initlizing SSD1306 display driver\n");
#endif
  //turn on screen while we configure shit, it might look cooler
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xaf); 
 
  //don't inverse display
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xa6); 

  //set contrast to 255 (try chainging?)
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0x81, 0xff}); 

  //set lower nibble of column ptr to 0
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x00); 
  //set upper nibble of column ptr to 0
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x10); 

  //memory mode is paging
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0x20, 0x02}); 

  //no vertical shift
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0xd3, 0x00}); 

  //page address is at 0
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xb0); 

  //ram display start register is at 0
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x40); 

  //column address 0 is mapped to seg0
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xa0);  //a1

  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xc8); 


  //set mux ratio to 32 mux 
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0xa8, 0x1f});  

  //alternative COM hardware mapping (stolen from lib)
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0xda, 0x02});  

  //volage regulator on
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0x8d, 0x14});  


  //set clock divider to zero, and oscillator frequency as high as it goes
  //Maybe we save battery to run slower?
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 2, (uint8_t[]){0xd5, 0xf0}); //TODO look

  //deactivate scroll
  i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x2e); 

}

void screen_off() { i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xaf); }

void screen_on() { i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xae); }

void screen_update() {
  for(int on_page = 0; on_page < SCREEN_PAGE_CNT; on_page++) {
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xb0 + on_page);
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x00);
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x10);
    i2c_write_reg_multi(SSD1306_ADDR, SSD1306_DATA_REG, sizeof(screen_buffer), screen_buffer);
  }
}

#ifdef DEBUG_BUILD
//ugly code below

void scroll_test() {
  //sets scroll area to whole screen
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 3, (uint8_t[]){0xa3, 0x00, 0x20});
  //set up scroll options
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 6, (uint8_t[]){0x29, 0x00, 0x00, 0x00, 0x04, 0x00});
  //actually activate scroll
  i2c_write_reg_multi(SSD1306_ADDR, SSD1306_CMD_REG, 6, (uint8_t[]){0x2f});
}

void screen_testdraw() {
  unsigned int on_pix;
  unsigned int on_page;

  printf("running screen time tests\n");
  printf("generating test image...\n");
  for(on_pix = 0; on_pix < sizeof(screen_buffer); on_pix++) 
    screen_buffer[on_pix] = ((uint8_t)0xaa << (on_pix % 8));

  printf("page mode, individual byte per i2c call...\n");
  for(on_page = 0; on_page < SCREEN_PAGE_CNT; on_page++) {
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xb0 + on_page);
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x00);
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x10);
    for(on_pix = 0; on_pix < SCREEN_RES_X; on_pix++)
      i2c_write_reg(SSD1306_ADDR, SSD1306_DATA_REG, screen_buffer[(on_page * 8) + on_pix]);
  }
  printf("page mode, 1 page per i2c call...\n");
  for(on_page = 0; on_page < SCREEN_PAGE_CNT; on_page++) {
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0xb0 + on_page);
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x00);
    i2c_write_reg(SSD1306_ADDR, SSD1306_CMD_REG, 0x10);
    i2c_write_reg_multi(SSD1306_ADDR, SSD1306_DATA_REG, SCREEN_RES_X, &screen_buffer[on_page * SCREEN_RES_X]);
  }
}

#endif