#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\r\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);
  }
}

void screen_test() {
  printf("testing screen\n");
  for(int on_pix = 0; on_pix < (SCREEN_RES_X * SCREEN_RES_Y) / 8; on_pix++) 
    screen_buffer[on_pix] = 0b10101010;
  printf("done testing screen\n");
  screen_update();
}