/*
 * Code found at http://www.instructables.com/id/How-to-use-OLED-display-arduino-module/
 * Thank you very much!
 * Adapted from Arduino to STM32 HAL by wollud1969
 */

#include <main.h>
#include <spi.h>

#include <stdint.h>
#include <string.h>
#include <stdarg.h>
#include <stdio.h>

#include <oled.h>
#include <oled-fonts.h>
#include <stm32f1xx_hal.h>

#include <logger.h>
#include <PontCoopScheduler.h>


#define HIGH GPIO_PIN_SET
#define LOW GPIO_PIN_RESET

static void __LEDPIN_RST(GPIO_PinState v) {
  HAL_GPIO_WritePin(Display_RES_GPIO_Port, Display_RES_Pin, v);
}

static void __LEDPIN_DC(GPIO_PinState v) {
  HAL_GPIO_WritePin(Display_DC_GPIO_Port, Display_DC_Pin, v);
}

static void __LEDPIN_CS(GPIO_PinState v) {
  HAL_GPIO_WritePin(Display_CS_GPIO_Port, Display_CS_Pin, v);
}

static void oled_WrDat(unsigned char data)   
{
  __LEDPIN_CS(LOW);
  __LEDPIN_DC(HIGH);

  HAL_SPI_Transmit(&displaySpi, &data, 1, 0);

  __LEDPIN_CS(HIGH);
}

static void oled_WrCmd(unsigned char cmd) 
{
  __LEDPIN_CS(LOW);
  __LEDPIN_DC(LOW);

  HAL_SPI_Transmit(&displaySpi, &cmd, 1, 0);

  __LEDPIN_CS(HIGH);
}

static void oled_Set_Pos(unsigned char x, unsigned char y)
{ 
  oled_WrCmd(0xb0+y);
  oled_WrCmd(((x&0xf0)>>4)|0x10);
  oled_WrCmd((x&0x0f)|0x00);
} 


static void oled_Fill(unsigned char bmp_data)
{
  unsigned char y,x;

  for(y=0;y<8;y++)
  {
    oled_WrCmd(0xb0+y);
    oled_WrCmd(0x00);
    oled_WrCmd(0x10);
    for(x=0;x<128;x++)
      oled_WrDat(bmp_data);
  }
} 



static void oled_CLS(void) 
{
  unsigned char y,x;
  for(y=0;y<8;y++)
  {
    oled_WrCmd(0xb0+y);
    oled_WrCmd(0x00);
    oled_WrCmd(0x10);
    for(x=0;x<128;x++)
      oled_WrDat(0);
  }
}

static void oled_DLY_ms(unsigned int ms)
{                         
  uint32_t start = HAL_GetTick();
  while (HAL_GetTick() < start + ms);
}

/*
 * unused
static void SetStartColumn(unsigned char d)
{
  oled_WrCmd(0x00+d%16);	// Set Lower Column Start Address for Page Addressing Mode
  // Default => 0x00
  oled_WrCmd(0x10+d/16);	// Set Higher Column Start Address for Page Addressing Mode
  // Default => 0x10
}
*/

static void SetAddressingMode(unsigned char d)
{
  oled_WrCmd(0x20);			// Set Memory Addressing Mode
  oled_WrCmd(d);			// Default => 0x02
  // 0x00 => Horizontal Addressing Mode
  // 0x01 => Vertical Addressing Mode
  // 0x02 => Page Addressing Mode
}

/*
 * unused
static void SetColumnAddress(unsigned char a, unsigned char b)
{
  oled_WrCmd(0x21);			// Set Column Address
  oled_WrCmd(a);			// Default => 0x00 (Column Start Address)
  oled_WrCmd(b);			// Default => 0x7F (Column End Address)
}
*/
/*
 * unused
static void SetPageAddress(unsigned char a, unsigned char b)
{
  oled_WrCmd(0x22);			// Set Page Address
  oled_WrCmd(a);			// Default => 0x00 (Page Start Address)
  oled_WrCmd(b);			// Default => 0x07 (Page End Address)
}
*/

static void SetStartLine(unsigned char d)
{
  oled_WrCmd(0x40|d);		// Set Display Start Line
  // Default => 0x40 (0x00)
}

static void SetContrastControl(unsigned char d)
{
  oled_WrCmd(0x81);			// Set Contrast Control
  oled_WrCmd(d);			// Default => 0x7F
}

static void Set_Charge_Pump(unsigned char d)
{
  oled_WrCmd(0x8D);			// Set Charge Pump
  oled_WrCmd(0x10|d);		// Default => 0x10
  // 0x10 (0x00) => Disable Charge Pump
  // 0x14 (0x04) => Enable Charge Pump
}

static void Set_Segment_Remap(unsigned char d)
{
  oled_WrCmd(0xA0|d);		// Set Segment Re-Map
  // Default => 0xA0
  // 0xA0 (0x00) => Column Address 0 Mapped to SEG0
  // 0xA1 (0x01) => Column Address 0 Mapped to SEG127
}

static void Set_Entire_Display(unsigned char d)
{
  oled_WrCmd(0xA4|d);		// Set Entire Display On / Off
  // Default => 0xA4
  // 0xA4 (0x00) => Normal Display
  // 0xA5 (0x01) => Entire Display On
}

static void Set_Inverse_Display(unsigned char d)
{
  oled_WrCmd(0xA6|d);		// Set Inverse Display On/Off
  // Default => 0xA6
  // 0xA6 (0x00) => Normal Display
  // 0xA7 (0x01) => Inverse Display On
}

static void Set_Multiplex_Ratio(unsigned char d)
{
  oled_WrCmd(0xA8);			// Set Multiplex Ratio
  oled_WrCmd(d);			// Default => 0x3F (1/64 Duty)
}

static void Set_Display_On_Off(unsigned char d)
{
  oled_WrCmd(0xAE|d);		// Set Display On/Off
  // Default => 0xAE
  // 0xAE (0x00) => Display Off
  // 0xAF (0x01) => Display On
}

/*
 * unused
static void SetStartPage(unsigned char d)
{
  oled_WrCmd(0xB0|d);		// Set Page Start Address for Page Addressing Mode
  // Default => 0xB0 (0x00)
}
*/

static void Set_Common_Remap(unsigned char d)
{
  oled_WrCmd(0xC0|d);		// Set COM Output Scan Direction
  // Default => 0xC0
  // 0xC0 (0x00) => Scan from COM0 to 63
  // 0xC8 (0x08) => Scan from COM63 to 0
}

static void Set_Display_Offset(unsigned char d)
{
  oled_WrCmd(0xD3);			// Set Display Offset
  oled_WrCmd(d);			// Default => 0x00
}

static void Set_Display_Clock(unsigned char d)
{
  oled_WrCmd(0xD5);			// Set Display Clock Divide Ratio / Oscillator Frequency
  oled_WrCmd(d);			// Default => 0x80
  // D[3:0] => Display Clock Divider
  // D[7:4] => Oscillator Frequency
}

static void Set_Precharge_Period(unsigned char d)
{
  oled_WrCmd(0xD9);			// Set Pre-Charge Period
  oled_WrCmd(d);			// Default => 0x22 (2 Display Clocks [Phase 2] / 2 Display Clocks [Phase 1])
  // D[3:0] => Phase 1 Period in 1~15 Display Clocks
  // D[7:4] => Phase 2 Period in 1~15 Display Clocks
}

static void Set_Common_Config(unsigned char d)
{
  oled_WrCmd(0xDA);			// Set COM Pins Hardware Configuration
  oled_WrCmd(0x02|d);		// Default => 0x12 (0x10)
  // Alternative COM Pin Configuration
  // Disable COM Left/Right Re-Map
}

static void Set_VCOMH(unsigned char d)
{
  oled_WrCmd(0xDB);			// Set VCOMH Deselect Level
  oled_WrCmd(d);			// Default => 0x20 (0.77*VCC)
}

/*
 * unused
static void Set_NOP(void)
{
  oled_WrCmd(0xE3);			// Command for No Operation
}
*/

void oledInit(void)
{
  // LEDPIN_Init();
  //	LED_PORT=0X0F;
  //LED_SCLH;;;
  //LED_RSTL;;;
  //digitalWrite(SCL_PIN,HIGH);;;
  __LEDPIN_RST(LOW);
  //	for(i=0;i<100;i++)asm("nop");
  oled_DLY_ms(50);
  //LED_RSTH;;;
  __LEDPIN_RST(HIGH);

  Set_Display_On_Off(0x00);		  // Display Off (0x00/0x01)
  Set_Display_Clock(0x80);		  // Set Clock as 100 Frames/Sec
  Set_Multiplex_Ratio(0x3F);	  // 1/64 Duty (0x0F~0x3F)
  Set_Display_Offset(0x00);		  // Shift Mapping RAM Counter (0x00~0x3F)
  SetStartLine(0x00);			  // Set Mapping RAM Display Start Line (0x00~0x3F)
  Set_Charge_Pump(0x04);		  // Enable Embedded DC/DC Converter (0x00/0x04)
  SetAddressingMode(0x02);	  // Set Page Addressing Mode (0x00/0x01/0x02)
  Set_Segment_Remap(0x01);	  // Set SEG/Column Mapping
  Set_Common_Remap(0x08);	  // Set COM/Row Scan Direction
  Set_Common_Config(0x10);	  // Set Sequential Configuration (0x00/0x10)
  SetContrastControl(0xCF); // Set SEG Output Current
  Set_Precharge_Period(0xF1);	  // Set Pre-Charge as 15 Clocks & Discharge as 1 Clock
  Set_VCOMH(0x40);			  // Set VCOM Deselect Level
  Set_Entire_Display(0x00);		  // Disable Entire Display On (0x00/0x01)
  Set_Inverse_Display(0x00);	  // Disable Inverse Display On (0x00/0x01)
  Set_Display_On_Off(0x01);		  // Display On (0x00/0x01)
  oled_Fill(0x00);                               //clear all
  oled_Set_Pos(0,0);
} 

/*
 * unused
static void oled_P6x8Char(unsigned char x,unsigned char y,unsigned char ch)
{
  unsigned char c=0,i=0;

  c =ch-32;
  if(x>122)
  {
    x=0;
    y++;
  }
  oled_Set_Pos(x,y);
  for(i=0;i<6;i++)
  {
    oled_WrDat(F6x8[c][i]);
  }
}
*/

static void oled_P6x8Str(unsigned char x,unsigned char y,char ch[])
{
  // coloredMsg(LOG_BLUE, false, "OLED: %d %d %s", x, y, ch);

  unsigned char c=0,i=0,j=0;
  while (ch[j]!='\0')
  {
    c =ch[j]-32;
    if(x>126)
    {
      x=0;
      y++;
    }
    oled_Set_Pos(x,y);
    for(i=0;i<6;i++)
    {
      oled_WrDat(F6x8[c][i]);
    }
    x+=6;
    j++;
  }
}

/*
 * unused
static void oled_P8x16Str(unsigned char x,unsigned char y,char ch[])
{
  unsigned char c=0,i=0,j=0;
  while (ch[j]!='\0')
  {
    c =ch[j]-32;
    if(x>120)
    {
      x=0;
      y++;
    }
    oled_Set_Pos(x,y);
    for(i=0;i<8;i++)
    {
      oled_WrDat(F8X16[(c<<4)+i]);
    }
    oled_Set_Pos(x,y+1);
    for(i=0;i<8;i++)
    {
      oled_WrDat(F8X16[(c<<4)+i+8]);
    }
    x+=8;
    j++;
  }
}
*/


/*
 * unused
static void oled_PrintBMP(unsigned char x0,unsigned char y0,unsigned char x1,unsigned char y1,unsigned char bmp[])
{ 	
  int ii=0;
  unsigned char x,y;
  for(y=y0;y<=y1;y++)
  {
    oled_Set_Pos(x0,y);
    for(x=x0;x<x1;x++)
    {
      oled_WrDat(bmp[ii++]);
    }
  }
}
*/

/*
 * unused
static void oled_Cursor(unsigned char cursor_column, unsigned char cursor_row)
{
  if(cursor_row != 0)
  {
    if(cursor_column == 1) oled_Set_Pos(0, cursor_row + 2);
    else  oled_Set_Pos(80 + (cursor_column - 2)*6, cursor_row + 2);
    oled_WrDat(0xFF);
    oled_WrDat(0xFF);
    oled_WrDat(0xFF);
    oled_WrDat(0xFF);
    oled_WrDat(0xFF);
    oled_WrDat(0xFF);
  }
}
*/


#define MAX_LINES 8
#define MAX_CHARS 21
#define NUM_OF_SCREENS 2

static uint8_t currentLine = 0;
static char lines[NUM_OF_SCREENS][MAX_LINES+1][MAX_CHARS+1];
static oledScreen_t activeScreen = OLED_SCREEN0;

void oledClearActiveScreen() {
  oled_CLS();
  memset(lines[activeScreen], 0, sizeof(lines[activeScreen]));
  currentLine = 0;
}

void oledClearAllScreens() {
  oled_CLS();
  memset(lines, 0, sizeof(lines));
  currentLine = 0;
}

void oledPrint(oledScreen_t screen, char msg[]) {
  if (currentLine < MAX_LINES) {
    strncpy(lines[screen][currentLine], msg, MAX_CHARS);
    memset(lines[screen][currentLine] + strlen(msg), ' ', MAX_CHARS - strlen(msg) - 1);
    lines[screen][currentLine][MAX_CHARS - 1] = 0;  
    currentLine++;
  } else {
    for (uint8_t i = 1; i < MAX_LINES; i++) {
      memcpy(lines[screen][i-1], lines[screen][i], MAX_CHARS);
    }
    strncpy(lines[screen][MAX_LINES - 1], msg, MAX_CHARS);
    memset(lines[screen][MAX_LINES - 1] + strlen(msg), ' ', MAX_CHARS - strlen(msg) - 1);
    lines[screen][MAX_LINES - 1][MAX_CHARS - 1] = 0;
  }

  if (screen == activeScreen) {
    for (uint8_t line = 0; line < MAX_LINES; line++) {
      oled_P6x8Str(1, line, lines[activeScreen][line]);
    }
  }
}

static void oledSwitchBackToScreen0(void *handle) {
    oledSetActiveScreen(OLED_SCREEN0);
}

void oledSetActiveScreen(oledScreen_t screen) {
  activeScreen = screen;
  if (screen == OLED_SCREEN1) {
    schAdd(oledSwitchBackToScreen0, NULL, 10000, 0);
  }
}

void oledPrintf(oledScreen_t screen, const char *format, ...) {
    va_list vl;
    va_start(vl, format);
    char buf[MAX_CHARS+1];
    vsnprintf(buf, MAX_CHARS, format, vl);
    va_end(vl);
    oledPrint(screen, buf);
}