/*
 * peripherals_ssd1036.hpp
 *
 *  Created on: May 14, 2021
 *      Author: erki
 */

#ifndef SKULLC_PERIPHERALS_SSD1036_DISPLAY_HPP_
#define SKULLC_PERIPHERALS_SSD1036_DISPLAY_HPP_

#include <array>
#include <cstdint>

namespace Peripherals
{

struct SSD1306WriteAdapter
{
  template<typename G, typename S>
  struct Spi
  {
    using gpio = G;
    using registers_handle = S;

    gpio dc;
    gpio cs;
    registers_handle registers;

    Spi() = delete;
    Spi(const gpio& dc, const gpio& cs, const registers_handle& registers)
        : dc(dc), cs(cs), registers(registers)
    {}

    /**
     * Writes a command byte.
     *
     * When writing data into the command registers, the DC/RS pin must be set LOW.
     *
     * @param data The command byte to write.
     */
    void writeCommand(std::uint8_t data)
    {
      dc.Set(false);

      cs.Set(false);
      registers.Transmit(&data, 1);
      cs.Set(true);
    }

    void writeCommand(std::uint8_t command, std::uint8_t data)
    {
      dc.Set(false);

      cs.Set(false);
      registers.Transmit(&command, 1);
      registers.Transmit(&data, 1);
      cs.Set(true);
    }

    void writeData(std::uint8_t* data, const std::uint32_t len)
    {
      dc.Set(true);

      cs.Set(false);
      registers.Transmit(data, len);
      cs.Set(true);
    }
  };

  template<typename S>
  struct I2c
  {
    // TODO: implement
  };
};

/**
 * A generalized version of the SSD1306 display. Works with both I2C and SPI interfaces.
 *
 * The data is held in a compressed manner, where each bit of a byte represents a single pixel.
 * These bits are arrenged into a byte per column.
 *
 * Datasheet: https://cdn-shop.adafruit.com/datasheets/SSD1306.pdf
 */
template<typename G, typename A, typename HAL, std::size_t W, std::size_t H>
class SSD1306Display
{
public:
  using gpio = G;
  using write_adapter = A;
  using hal = HAL;
  constexpr static std::size_t width = W;
  constexpr static std::size_t height = H;
  constexpr static std::size_t rows = height / 8;

  SSD1306Display() = delete;
  SSD1306Display(const gpio& reset, const write_adapter& registers)
      : reset_(reset), registers_(registers)
  {}

  void setup()
  {
    hal::Delay(1);
    reset_.Set(false);
    hal::Delay(5);
    reset_.Set(true);
    hal::Delay(5);

    // reference impl: https://gitlab.com/TTYRobotiklubi/micromouse/mousetrap2/-/blob/master/Src/Peripherals/lcd.c
    //Set the display to sleep mode for the rest of the init.
    registers_.writeCommand(Regs_::DISPLAY_OFF_YES_SLEEP);

    //Set the clock speed, nominal ~105FPS
    //Low nibble is divide ratio
    //High level is oscillator frequency. Should be about 177 Hz.
    registers_.writeCommand(Regs_::CLOCK_DIVIDE_PREFIX, 0x80);

    //Set the multiplex ratio to 1/32
    //Default is 0x3F (1/64 Duty), we need 0x1F (1/32 Duty)
    registers_.writeCommand(Regs_::MULTIPLEX_RATIO_PREFIX, 0x1F);

    //Set the display offset to 0 (default)
    registers_.writeCommand(Regs_::DISPLAY_OFFSET_PREFIX, 0x00);

    //Set the display RAM display start line to 0 (default)
    //Bits 0-5 can be set to 0-63 with a bitwise or
    registers_.writeCommand(Regs_::DISPLAY_START_LINE);

    //Enable DC/DC converter, 7.5v
    registers_.writeCommand(Regs_::DCDC_CONFIG_PRESET, Regs_::DCDC_CONFIG_7p5v);

    //Map the columns correctly for our OLED glass layout.
    registers_.writeCommand(Regs_::SEG0_IS_COL_0);

    //Set COM output scan correctly for our OLED glass layout.
    registers_.writeCommand(Regs_::SCAN_DIR_UP);

    //Set COM pins correctly for our OLED glass layout
    registers_.writeCommand(Regs_::COM_CONFIG_PREFIX, Regs_::COM_CONFIG_SEQUENTIAL_LEFT);

    // Contrast.
    registers_.writeCommand(Regs_::CONTRAST_PREFIX, 0xBF);

    // Set precharge (low nibble) / discharge (high nibble) timing.
    // precharge = 1 clock
    // discharge = 15 clocks
    registers_.writeCommand(Regs_::PRECHARGE_PERIOD_PREFIX, 0xF1);

    // Set VCOM deselect level.
    registers_.writeCommand(Regs_::VCOMH_DESELECT_PREFIX, Regs_::VCOMH_DESELECT_0p83xVCC);

    // Set display to normal.
    registers_.writeCommand(Regs_::ENTIRE_DISPLAY_NORMAL);

    // Uninvert the display.
    registers_.writeCommand(Regs_::INVERSION_NORMAL);

    // Unset sleep mode.
    registers_.writeCommand(Regs_::DISPLAY_ON_NO_SLEEP);
  }

  void blank(const bool black)
  {
    screen_buffer_.fill(black ? 0xFF : 0x00);
  }

  void drawBuffer()
  {
    for (std::uint32_t row = 0; row < rows; row++)
    {
      registers_.writeCommand(0xB0 + row);
      registers_.writeCommand(0x00);
      registers_.writeCommand(0x10);
      registers_.writeData(&screen_buffer_[width * row], width);
    }
  }

  template<typename PB>
  void copyPixelBuffer(const PB& buffer)
  {
    static_assert(PB::width == width, "Pixel buffer's width doesn't match.");
    static_assert(PB::height == height, "Pixel buffer's height doesn't match.");
    static_assert(std::is_same<typename PB::value, std::uint8_t>::value, "Pixel buffer's value type does not match.");

    for (std::uint32_t row = 0; row < rows; row++)
    {
      for (std::uint32_t col = 0; col < width; col++)
      {
        std::uint8_t& byte = screen_buffer_[width * row + col];
        byte = 0;

        for (std::uint32_t bit = 0; bit < 8; bit++)
        {
          if (buffer.at(col, 8 * row + bit) != 0)
          {
            byte |= (1 << bit);
          }
        }
      }
    }
  }

private:
  gpio reset_;
  write_adapter registers_;
  std::array<std::uint8_t, width * rows> screen_buffer_;

  struct Regs_
  {
    static constexpr std::uint8_t DCDC_CONFIG_PRESET = 0x8D;
    static constexpr std::uint8_t DCDC_CONFIG_7p5v = 0x14;
    static constexpr std::uint8_t DCDC_CONFIG_6p0v = 0x15;
    static constexpr std::uint8_t DCDC_CONFIG_8p5v = 0x94;
    static constexpr std::uint8_t DCDC_CONFIG_9p0v = 0x95;

    static constexpr std::uint8_t DISPLAY_OFF_YES_SLEEP = 0xAE;
    static constexpr std::uint8_t DISPLAY_ON_NO_SLEEP = 0xAF;

    static constexpr std::uint8_t CLOCK_DIVIDE_PREFIX = 0xD5;
    static constexpr std::uint8_t MULTIPLEX_RATIO_PREFIX = 0xA8;
    static constexpr std::uint8_t DISPLAY_OFFSET_PREFIX = 0xD3;

    static constexpr std::uint8_t DISPLAY_START_LINE = 0x40;
    static constexpr std::uint8_t SEG0_IS_COL_0 = 0xA0;
    static constexpr std::uint8_t SEG0_IS_COL_127 = 0xA1;
    static constexpr std::uint8_t SCAN_DIR_UP = 0xC0;
    static constexpr std::uint8_t SCAN_DIR_DOWN = 0xC8;

    static constexpr std::uint8_t COM_CONFIG_PREFIX = 0xDA;
    static constexpr std::uint8_t COM_CONFIG_SEQUENTIAL_LEFT = 0x02;
    static constexpr std::uint8_t COM_CONFIG_ALTERNATE_LEFT = 0x12;
    static constexpr std::uint8_t COM_CONFIG_SEQUENTIAL_RIGHT = 0x22;
    static constexpr std::uint8_t COM_CONFIG_ALTERNATE_RIGHT = 0x32;

    static constexpr std::uint8_t CONTRAST_PREFIX = 0x81;
    static constexpr std::uint8_t PRECHARGE_PERIOD_PREFIX = 0xD9;

    static constexpr std::uint8_t VCOMH_DESELECT_PREFIX = 0xDB;
    static constexpr std::uint8_t VCOMH_DESELECT_0p65xVCC = 0x00;
    static constexpr std::uint8_t VCOMH_DESELECT_0p71xVCC = 0x10;
    static constexpr std::uint8_t VCOMH_DESELECT_0p77xVCC = 0x20;
    static constexpr std::uint8_t VCOMH_DESELECT_0p83xVCC = 0x30;

    static constexpr std::uint8_t ENTIRE_DISPLAY_FORCE_ON = 0xA5;
    static constexpr std::uint8_t ENTIRE_DISPLAY_NORMAL = 0xA4;

    static constexpr std::uint8_t INVERSION_NORMAL = 0xA6;
    static constexpr std::uint8_t INVERSION_INVERTED = 0xA7;
  };
};

}// namespace Peripherals


#endif /* SKULLC_PERIPHERALS_SSD1036_DISPLAY_HPP_ */