#include "LCDDisplay.hpp"

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

#include <iostream>
#include <string>

/* lcd size hardcoded to 4x40 idgaf */
LCDDisplay::LCDDisplay(std::string dev, uint32_t baud) {
    std::cout << "[LCDDisplay] Opening LCD at " << dev << std::endl;
    struct termios term;
    int lcd = open(dev.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);

    // handle errors
    if (lcd < 0) {
        std::cerr << "[LCDDisplay] Error opening serial port " << dev << std::endl;
        exit(lcd);
    }

    // Setup the serial port 8 data bits, no parity ,1 stopbit, no flow control
    term.c_cflag = baud | CS8 | CSTOPB | CLOCAL | CREAD;
    term.c_iflag = 0;
    term.c_oflag = 0;
    term.c_lflag = 0;
    tcflush(lcd, TCIFLUSH);
    tcsetattr(lcd, TCSANOW, &term);
    _lcd = lcd;
}

LCDDisplay::~LCDDisplay() {
    std::cout << "[LCDDisplay] Closing LCD port" << std::endl;
    close(_lcd);
}

void LCDDisplay::clear() {
    uint8_t cmdbuf[] = {254, 88};
    write(_lcd, cmdbuf, 2);
}

void LCDDisplay::setContrast(uint8_t contrast) {
    std::cout << "[LCDDisplay] Set contrast to " << std::to_string(contrast) << std::endl;
    uint8_t cmdbuf[] = {254, 80, contrast};
    write(_lcd, cmdbuf, 3);
}

void LCDDisplay::setBacklight(bool enabled) {
    std::cout << "[LCDDisplay] Backlight set to " << std::to_string(enabled) << std::endl;
    uint8_t cmdbuf[] = {254, (uint8_t)(enabled ? 66 : 70), 255};
    write(_lcd, cmdbuf, enabled ? 3 : 2);
}

void LCDDisplay::setCursor(uint8_t row, uint8_t col) {
    uint8_t cmdbuf[] = {254, 71, (uint8_t)(col + 1), (uint8_t)(row + 1)};
    write(_lcd, cmdbuf, 4);
}

void LCDDisplay::writeStr(std::string str) {
    write(_lcd, str.c_str(), str.size());
}

void LCDDisplay::writeLines(std::array<std::string, 4> lines) {
    for (auto& line : lines) {
        if(line.size() > 40) {
            std::cerr << "Line " << line << " too long! (" << line.size() << " chars)" << std::endl;
            continue;
        }
        line.insert(line.end(), 40 - line.size(), ' ');
    }

    setCursor(0, 0);
    writeStr(lines[0] + lines[1]);
    setCursor(2, 0);
    writeStr(lines[2] + lines[3]);
}

void LCDDisplay::writeSStream(std::stringstream& ss) {
    std::string s = ss.str();
    if (s.size() != 160)
        std::cerr << "[LCDDisplay] stringstream wrong (" + std::to_string((int)(160 - s.size())) + " chars too short)" << std::endl;
    setCursor(0, 0);
    writeStr(s.substr(0, 80));
    setCursor(2, 0);
    writeStr(s.substr(80, 159));
}