// add include tiar_font5x8
enum SSD1306 {
  LCDWIDTH = 128,
  LCDHEIGHT = 64,
  SETCONTRAST = 0x81, // 2byte cmd,256 contrast steps, reset = 7F
  DISPLAYON = 0xAF,   // entire display on
  DISPLAYOFF = 0xAE,  // entire display off
  DISPLAYALLON_RESUME = 0xA4,
  NORMALDISPLAY = 0xA6,     // invert = 0xA7
  DEACTIVATE_SCROLL = 0x2E, // stop control scroll conf by 26 27 29 2A
  MEMORYMODE =
      0x20, // 2byte cmd, 0 horizontal, 1 vertical, 2 page addressing, 3 invalid
  COLUMNADDR = 0x21,     // 3bytes, start, end (included) valid in horizontal or
                         // vertical mode
  PAGEADDR = 0x22,       // 3bytes, start, end (included) valid in horizontal or
                         // vertical mode
  SETSTARTLINE_0 = 0x40, // set display RAM start line at 0
  SEGREMAP = 0xA0,       // segment remap 0 mapped to SEG0
  SETMULTIPLEX = 0xA8,   // 2 byte cmd, set mux ratio
  COMSCANDEC = 0xC8, // scan from COM[N-1] to COM[0] (0xC0 is COM0 to COM[N-1])
  SETDISPLAYOFFSET = 0xD3,   // 2 byte cmd, vertical shift
  SETCOMPINS = 0xDA,         // 2 byte cmd, seq com pin conf, left right remap
  SETDISPLAYCLOCKDIV = 0xD5, // 2 byte cmd, low nibble A[3:0]+1 = div ratio,
                             // high nibble A[7:4] freq, reset 1000b
  SETPRECHARGE =
      0xD9, // 2 byte cmd, precharge period A[3:0] phase1 A[7:4] phase2, reset:2
  SETVCOMDETECT = 0xDB, // 2 byte Vcomh deselect level A[6:4] 000b 0.65xVcc 010b
                        // 0.77(reset) 011b 0.83
  CHARGEPUMP = 0x8D,    // Enable charge pump seq: 0x8D, 0x14, 0xAF (Charge pump
                        // setting, enable charge pump, display on)
  EXTERNALVCC = 0x1,
  SWITCHCAPVCC = 0x2,
};
uint8_t nibbleToByte[16] = {0b00000000, 0b00000011, 0b00001100, 0b00001111,
                            0b00110000, 0b00110011, 0b00111100, 0b00111111,
                            0b11000000, 0b11000011, 0b11001100, 0b11001111,
                            0b11110000, 0b11110011, 0b11111100, 0b11111111};

uint8_t cpt = 0;
uint8_t *txbuf;
uint8_t *rxbuf;
int32_t modeA;
int32_t modeB;

uint8_t textA[11 * 4] = {' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
                         ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
                         ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
                         ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' '};
uint8_t textB[11 * 4] = {' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
                         ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
                         ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
                         ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' '};
int8_t tYA[128];
int8_t tYB[128];
bool drawn = true;

// SETUP
// ------------------
void cmd(uint8_t c, uint8_t add) {
  txbuf[0] = 0;
  txbuf[1] = c;
  i2cMasterTransmitTimeout(&I2CD1, add, txbuf, 2, rxbuf, 0, 30);
  // chThdSleepMilliseconds(1);
}

// _____________________________________________________________________
void fill(uint8_t v, uint8_t add) {
  cmd(COLUMNADDR, add);
  cmd(0, add);
  cmd(127, add); // Column start end
  cmd(PAGEADDR, add);
  cmd(0, add);
  cmd(7, add); // Page start end
  txbuf[0] = 0x40;
  for (int i = 1; i < 129; i++)
    txbuf[i] = v;
  for (int p = 0; p < 8; p++) {
    i2cMasterTransmitTimeout(&I2CD1, add, txbuf, 129, rxbuf, 0, 30);
    // chThdSleepMilliseconds(1);
  }
}
// _____________________________________________________________________
// scaled x2 text
// returns a page to be sent to the SSD1306 based on contents of text
void calcTextPage(int page, uint8_t tPage[128], uint8_t add) {
  uint8_t *text;
  if (add == 0x3C)
    text = textA;
  else
    text = textB;
  int i = 0;
  int tLine = page / 2;
  for (int nc = 0; nc < 11; nc++) {
    int ascii_32 = text[nc + 11 * tLine] - ' ';
    if (ascii_32 < 0 || ascii_32 >= 128 - 32)
      ascii_32 = 0;
    const uint8_t *adChar = tiar_font5x8 + ascii_32 * 5;
    for (int slice = 0; slice < 5; slice++) {
      uint8_t s;
      if ((page & 1) == 0) {
        s = nibbleToByte[adChar[slice] & 15]; // low nibble
      } else {
        s = nibbleToByte[(adChar[slice] >> 4) & 15]; // high nibble
      }
      tPage[i] = s;
      i++;
      tPage[i] = s;
      i++;
    }
    tPage[i] = 0;
    i++; // separator space
  }
  while (i < 128) {
    tPage[i] = 0;
    i++;
  } // space padding
}
// _____________________________________________________________________
// opt function draw

// LSB up
uint8_t tBar[9] = {0b00000000, 0b10000000, 0b11000000, 0b11100000, 0b11110000,
                   0b11111000, 0b11111100, 0b11111110, 0b11111111};

/*
page0
    1
    .
    7
*/

uint8_t vBar(uint8_t val, int page) {
  uint8_t _page = 7 - (val / 8);
  if (page > _page)
    return 0b11111111; // below => light
  else if (page < _page)
    return 0; // above => dark
  else
    return tBar[val & 7];
}
void _calcScopePage(int page, uint8_t tPage[128]) {
  for (int i = 0; i < 128; i++) {
    uint16_t y = ((attr_scopeA.t[i] + 64) * 3) / 8;
    tPage[i] = vBar(y, page);
  }
}
void calcScopePage(int page, uint8_t tPage[128], uint8_t add) {
  if (page < 2) {
    calcTextPage(page, tPage, add);
  } else {
    int8_t *tY = add == 0x3C ? tYA : tYB;
    int16_t y0 = tY[0];
    int16_t y1 = tY[1];
    int16_t y2;
    for (int i = 0; i < 128; i++) {
      if (i < 127)
        y2 = tY[i + 1];
      int16_t yM, ym;
      yM = ym = y1;
      int16_t y = (y0 + y1) >> 1;
      yM = y > yM ? y : yM;
      ym = y < ym ? y : ym;
      y = (y2 + y1) >> 1;
      yM = y > yM ? y : yM;
      ym = y < ym ? y : ym;
      if (ym == yM)
        if (yM > 0)
          ym--;
        else
          yM++;
      tPage[i] = vBar(yM, page) & ~vBar(ym, page);
      y0 = y1;
      y1 = y2;
    }
  }
}
// _____________________________________________________________________
int8_t scale3_4(int8_t x) {
  // if(x==-128) return -24;
  // if(x==127) return 70;
  return (int8_t)(((((int16_t)x) + 64) * 3) >> 3);
}
void sendTextPage(int page, uint8_t add) {
  cmd(COLUMNADDR, add);
  cmd(0, add);
  cmd(127, add); // Column start end
  cmd(PAGEADDR, add);
  cmd(page, add);
  cmd(page, add); // Page start end

  if (attr_type == 1106) {
    cmd(0xB0 + page, add);     // set page address
    cmd(2 & 0xf, add);         // set lower column address
    cmd(0x10 | (2 >> 4), add); // set higher column address
  }

  txbuf[0] = 0x40;
  int32_t mode = add == 0x3C ? modeA : modeB;
  if (mode == 0) {
    calcTextPage(page, txbuf + 1, add);
    i2cMasterTransmitTimeout(&I2CD1, add, txbuf, 129, rxbuf, 0, 30);
  } else if (mode == 1) {
    if (page == 1 && drawn) {
      for (int i = 0; i < 128; i++) {
        tYA[i] = scale3_4(attr_scopeA.t[i]);
        tYB[i] = scale3_4(attr_scopeB.t[i]);
      }
      drawn = false;
    }
    calcScopePage(page, txbuf + 1, add);
    i2cMasterTransmitTimeout(&I2CD1, add, txbuf, 129, rxbuf, 0, 30);
  }
}
// _____________________________________________________________________
void init() {
  for (int a = 0x3C; a <= 0x3D; a++) {
    // Init sequence
    cmd(DISPLAYOFF, a);

    // 2 byte cmd, low nibble A[3:0]+1 = div ratio, high nibble A[7:4] freq,
    // reset 1000b
    cmd(SETDISPLAYCLOCKDIV, a);
    cmd(0x80, a);
    cmd(SETMULTIPLEX, a);
    cmd(LCDHEIGHT - 1, a);
    cmd(SETDISPLAYOFFSET, a);
    cmd(attr_type == 1306 ? 0x00 : 0x01, a);
    cmd(SETSTARTLINE_0, a);
    cmd(CHARGEPUMP, a);
    cmd(0x14, a);
    cmd(MEMORYMODE, a);
    cmd(0x00, a);
    cmd(SEGREMAP | 0x1, a);
    cmd(COMSCANDEC, a);
    // 128 x 64
    cmd(SETCOMPINS, a);
    cmd(0x12, a);
    cmd(SETCONTRAST, a);
    cmd(0xCF, a);
    cmd(SETPRECHARGE, a);
    cmd(0xF1, a);
    cmd(SETVCOMDETECT, a);
    cmd(0x40, a);
    cmd(DISPLAYALLON_RESUME, a);
    cmd(NORMALDISPLAY, a);
    cmd(DEACTIVATE_SCROLL, a);
    cmd(DISPLAYON, a);
  }
}
// _____________________________________________________________________
void setup() {
  static uint8_t _txbuf[129] __attribute__((section(".sram2")));
  static uint8_t _rxbuf[8] __attribute__((section(".sram2")));
  txbuf = _txbuf;
  rxbuf = _rxbuf;
  init();
}
// _____________________________________________________________________
void loop() {
  uint8_t p[] = {0, 1, 4, 5, 3, 6, 2, 7};
  sendTextPage(p[cpt % 8], 0x3C);
  sendTextPage(p[cpt % 8], 0x3D);
  if ((cpt & 7) == 7)
    drawn = true;
  cpt++;
}
// _____________________________________________________________________
// THREADS
msg_t ThreadX2() {
  setup();
  while (!chThdShouldTerminate()) {
    loop();
    chThdSleepMilliseconds(1);
  }
  chThdExit((msg_t)0);
}

static msg_t ThreadX(void *arg) { ((attr_parent *)arg)->ThreadX2(); }

WORKING_AREA(waThreadX, 1024);
Thread *Thd;

// setup the pins
palSetPadMode(GPIOB, 8,
              PAL_MODE_ALTERNATE(4) | PAL_STM32_PUDR_PULLUP |
                  PAL_STM32_OTYPE_OPENDRAIN); // SCL
palSetPadMode(GPIOB, 9,
              PAL_MODE_ALTERNATE(4) | PAL_STM32_PUDR_PULLUP |
                  PAL_STM32_OTYPE_OPENDRAIN); // SDA
static const I2CConfig i2cfg = {
    OPMODE_I2C,
    400000,
    FAST_DUTY_CYCLE_2,
};
/*static const I2CConfig i2cfg = {
    OPMODE_I2C,
    100000,
    STD_DUTY_CYCLE,
};*/
i2cStart(&I2CD1, &i2cfg);
Thd = chThdCreateStatic(waThreadX, sizeof(waThreadX), NORMALPRIO, ThreadX,
                        (void *)this);

modeA = inlet_modeA;

if (inlet_line1A != NULL) {
  int i = 0;
  while (i < 11 & inlet_line1A[i] != '\0') {
    textA[i] = inlet_line1A[i];
    i++;
  }
  while (i < 11) {
    textA[i] = ' ';
    i++;
  }
}
if (modeA == 0) {
  if (inlet_line2A != NULL) {
    int i = 0;
    while (i < 11 & inlet_line2A[i] != '\0') {
      textA[i + 11] = inlet_line2A[i];
      i++;
    }
    while (i < 11) {
      textA[i + 11] = ' ';
      i++;
    }
  }
  if (inlet_line3A != NULL) {
    int i = 0;
    while (i < 11 & inlet_line3A[i] != '\0') {
      textA[i + 2 * 11] = inlet_line3A[i];
      i++;
    }
    while (i < 11) {
      textA[i + 2 * 11] = ' ';
      i++;
    }
  }
  if (inlet_line4A != NULL) {
    int i = 0;
    while (i < 11 & inlet_line4A[i] != '\0') {
      textA[i + 3 * 11] = inlet_line4A[i];
      i++;
    }
    while (i < 11) {
      textA[i + 3 * 11] = ' ';
      i++;
    }
  }
}

modeB = inlet_modeB;

if (inlet_line1B != NULL) {
  int i = 0;
  while (i < 11 & inlet_line1B[i] != '\0') {
    textB[i] = inlet_line1B[i];
    i++;
  }
  while (i < 11) {
    textB[i] = ' ';
    i++;
  }
}
if (modeA == 0) {
  if (inlet_line2B != NULL) {
    int i = 0;
    while (i < 11 & inlet_line2B[i] != '\0') {
      textB[i + 11] = inlet_line2B[i];
      i++;
    }
    while (i < 11) {
      textB[i + 11] = ' ';
      i++;
    }
  }
  if (inlet_line3B != NULL) {
    int i = 0;
    while (i < 11 & inlet_line3B[i] != '\0') {
      textB[i + 2 * 11] = inlet_line3B[i];
      i++;
    }
    while (i < 11) {
      textB[i + 2 * 11] = ' ';
      i++;
    }
  }
  if (inlet_line4B != NULL) {
    int i = 0;
    while (i < 11 & inlet_line4B[i] != '\0') {
      textB[i + 3 * 11] = inlet_line4B[i];
      i++;
    }
    while (i < 11) {
      textB[i + 3 * 11] = ' ';
      i++;
    }
  }
}

chThdTerminate(Thd);
chThdWait(Thd);
i2cStop(&I2CD1);
palSetPadMode(GPIOB, 8, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOB, 9, PAL_MODE_INPUT_ANALOG);