#include "HermitCrab.h"
#include "SSD1306.h"

//#define DISPLAY_WIDTH SCREEN_WIDTH
//#define DISPLAY_HEIGHT SCREEN_HEIGHT


#define WIRE_WRITE          Wire.write
#define SETWIRECLOCK        Wire.setClock(400000UL)    ///< Set before I2C transfer
#define RESWIRECLOCK        Wire.setClock(100000UL)    ///< Restore after I2C xfer
#define TRANSACTION_START   SETWIRECLOCK
#define TRANSACTION_END     RESWIRECLOCK
#define WIRE_MAX            I2C_BUFFER_LENGTH


// Constructor for SSD1306 class
//SSD1306::SSD1306(Adafruit_SSD1306 &display)
SSD1306::SSD1306()
    : Adafruit_GFX(SCREEN_WIDTH, SCREEN_HEIGHT)
    , i2caddr(DISPLAY_I2C_ADDRESS)
{}

void SSD1306::dim(bool dim) {
  // the range of contrast to too small to be really useful
  // it is useful to dim the display
  TRANSACTION_START;
  ssd1306_command1(SSD1306_SETCONTRAST);
  ssd1306_command1(dim ? 0 : contrast);
  TRANSACTION_END;
}

void SSD1306::setContrast(uint8_t con) {
  // the range of contrast to too small to be really useful
  // it is useful to dim the display
  TRANSACTION_START;
  ssd1306_command1(SSD1306_SETCONTRAST);
  ssd1306_command1(100 * con / contrast);
  TRANSACTION_END;
}

bool SSD1306::begin(uint8_t vcs, uint8_t addr) {
    vccstate = vcs;
    i2caddr = addr;

    TRANSACTION_START;

    // Init sequence
    static const uint8_t init1[] = {SSD1306_DISPLAYOFF,         // 0xAE
                                        SSD1306_SETDISPLAYCLOCKDIV, // 0xD5
                                        0x80, // the suggested ratio 0x80
                                        SSD1306_SETMULTIPLEX,
                                        SCREEN_HEIGHT - 1}; // 0xA8
    ssd1306_commandList(init1, sizeof(init1));

    static const uint8_t init2[] = {SSD1306_SETDISPLAYOFFSET, // 0xD3
                                        0x0,                      // no offset
                                        SSD1306_SETSTARTLINE | 0x0, // line #0
                                        SSD1306_CHARGEPUMP};        // 0x8D
    ssd1306_commandList(init2, sizeof(init2));

    ssd1306_command1((vccstate == SSD1306_EXTERNALVCC) ? 0x10 : 0x14);

    static const uint8_t init3[] = {SSD1306_MEMORYMODE, // 0x20
                                        0x00, // 0x0 act like ks0108
                                        SSD1306_SEGREMAP | 0x1,
                                        SSD1306_COMSCANDEC};
    ssd1306_commandList(init3, sizeof(init3));

    uint8_t comPins = 0x02;
    contrast = 0x8F;

    if ((WIDTH == 128) && (HEIGHT == 32)) {
        comPins = 0x02;
        contrast = 0x8F;
    } else if ((WIDTH == 128) && (HEIGHT == 64)) {
        comPins = 0x12;
        contrast = (vccstate == SSD1306_EXTERNALVCC) ? 0x9F : 0xCF;
    } else if ((WIDTH == 96) && (HEIGHT == 16)) {
        comPins = 0x2; // ada x12
        contrast = (vccstate == SSD1306_EXTERNALVCC) ? 0x10 : 0xAF;
    } else {
        // Other screen varieties -- TBD
    }

    ssd1306_command1(SSD1306_SETCOMPINS);
    ssd1306_command1(comPins);
    ssd1306_command1(SSD1306_SETCONTRAST);
    ssd1306_command1(contrast);

    ssd1306_command1(SSD1306_SETPRECHARGE); // 0xd9
    ssd1306_command1((vccstate == SSD1306_EXTERNALVCC) ? 0x22 : 0xF1);
    static const uint8_t init5[] = {
        SSD1306_SETVCOMDETECT, // 0xDB
        0x40,
        SSD1306_DISPLAYALLON_RESUME, // 0xA4
        SSD1306_NORMALDISPLAY,       // 0xA6
        SSD1306_DEACTIVATE_SCROLL,
        SSD1306_DISPLAYON}; // Main screen turn on
    ssd1306_commandList(init5, sizeof(init5));

    TRANSACTION_END;
    clearDisplayBuffer();
    return true;
}

MY_IRAM_ATTR void SSD1306::updateScreen() {
    TRANSACTION_START;
    uint8_t dlist1[6];
    dlist1[0] = SSD1306_PAGEADDR;
    dlist1[1] = 0;
    dlist1[2] = (height() / 8) - 1;
    dlist1[3] = SSD1306_COLUMNADDR;
    dlist1[4] = 0;
    dlist1[5] = width() - 1;
    ssd1306_commandList(dlist1, sizeof(dlist1));

    uint16_t count = height() * width() / 8;
    uint8_t *ptr = getBuffer();

    Wire.beginTransmission(i2caddr);
    WIRE_WRITE((uint8_t)0x40);
    uint16_t bytesOut = 1;
    // Loop through each page in the range
    while (count--) {
        if (bytesOut >= WIRE_MAX) {
                Wire.endTransmission();
                Wire.beginTransmission(i2caddr);
                WIRE_WRITE((uint8_t)0x40);
                bytesOut = 1;
        }
        WIRE_WRITE(*ptr++);
        bytesOut++;
    }
    Wire.endTransmission(); // End transmission for the page

    TRANSACTION_END;
}

MY_IRAM_ATTR void SSD1306::updateRegion(uint8_t pageStart, uint8_t pageEnd, uint8_t colStart = 0, uint8_t colEnd = 127) {
    TRANSACTION_START;
    uint8_t dlist1[6];
    dlist1[0] = SSD1306_PAGEADDR;
    dlist1[1] = pageStart;
    dlist1[2] = pageEnd;
    dlist1[3] = SSD1306_COLUMNADDR;
    dlist1[4] = colStart;
    dlist1[5] = colEnd;
    ssd1306_commandList(dlist1, sizeof(dlist1));

    uint16_t count = (colEnd - colStart + 1) * (pageEnd - pageStart + 1);
    uint8_t *ptr = getBuffer() + pageStart * SCREEN_WIDTH + colStart;
    uint16_t cols = colEnd - colStart + 1;
    uint16_t offset = SCREEN_WIDTH - cols;

    Wire.beginTransmission(i2caddr);
    WIRE_WRITE((uint8_t)0x40);
    uint16_t bytesOut = 1;
    // Loop through each page in the range
    for (uint8_t page = pageStart; page <= pageEnd; page++) {
        // Send column data for the current page
        for (uint8_t col = 0; col < cols; col++) {
            if (bytesOut >= WIRE_MAX) {
                Wire.endTransmission();
                Wire.beginTransmission(i2caddr);
                WIRE_WRITE((uint8_t)0x40);
                bytesOut = 1;
            }
            WIRE_WRITE(*ptr++);
            bytesOut++;
        }
        ptr += offset;
    }
    Wire.endTransmission(); // End transmission for the page
    TRANSACTION_END;
}

MY_IRAM_ATTR void SSD1306::ssd1306_hscroll(uint8_t dir, uint8_t pageStart, uint8_t pageEnd, uint8_t offset, uint8_t interval) {
    ssd1306_command1(dir ? SSD1306_RIGHT_HORIZONTAL_SCROLL : SSD1306_LEFT_HORIZONTAL_SCROLL);
    ssd1306_command1(0x00); // Dummy
    ssd1306_command1(pageStart);
    ssd1306_command1(interval);
    ssd1306_command1(pageEnd);
    ssd1306_command1(offset);
    ssd1306_command1(SSD1306_ACTIVATE_SCROLL);
}

MY_IRAM_ATTR void SSD1306::drawPixel(int16_t x, int16_t y, uint16_t color) {
  if ((x >= 0) && (x < width()) && (y >= 0) && (y < height())) {
    switch (color) {
    case SSD1306_WHITE:
      buffer[x + (y / 8) * WIDTH] |= (1 << (y & 7));
      break;
    case SSD1306_BLACK:
      buffer[x + (y / 8) * WIDTH] &= ~(1 << (y & 7));
      break;
    case SSD1306_INVERSE:
      buffer[x + (y / 8) * WIDTH] ^= (1 << (y & 7));
      break;
    }
  }
}

MY_IRAM_ATTR void SSD1306::ssd1306_commandList(const uint8_t *c, uint8_t n) {
    Wire.beginTransmission(i2caddr);
    WIRE_WRITE((uint8_t)0x00); // Co = 0, D/C = 0
    uint16_t bytesOut = 1;
    while (n--) {
      if (bytesOut >= WIRE_MAX) {
        Wire.endTransmission();
        Wire.beginTransmission(i2caddr);
        WIRE_WRITE((uint8_t)0x00); // Co = 0, D/C = 0
        bytesOut = 1;
      }
      WIRE_WRITE(*c++);
      bytesOut++;
    }
    Wire.endTransmission();
}

MY_IRAM_ATTR void SSD1306::ssd1306_command1(uint8_t c) {
    Wire.beginTransmission(i2caddr);
    WIRE_WRITE((uint8_t)0x00); // Co = 0, D/C = 0
    WIRE_WRITE(c);
    Wire.endTransmission();
}