#include "Arduino_ESP32S2PAR8Q.h"

#if defined(ESP32) && (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3)

Arduino_ESP32S2PAR8Q::Arduino_ESP32S2PAR8Q(int8_t dc, int8_t cs, int8_t wr, int8_t rd)
    : _dc(dc), _cs(cs), _wr(wr), _rd(rd)
{
}

bool Arduino_ESP32S2PAR8Q::begin(int32_t speed, int8_t dataMode)
{
  pinMode(_dc, OUTPUT);
  digitalWrite(_dc, HIGH); // Data mode
  if (_dc >= 32)
  {
    _dcPinMask = digitalPinToBitMask(_dc);
    _dcPortSet = (PORTreg_t)GPIO_OUT1_W1TS_REG;
    _dcPortClr = (PORTreg_t)GPIO_OUT1_W1TC_REG;
  }
  else if (_dc != GFX_NOT_DEFINED)
  {
    _dcPinMask = digitalPinToBitMask(_dc);
    _dcPortSet = (PORTreg_t)GPIO_OUT_W1TS_REG;
    _dcPortClr = (PORTreg_t)GPIO_OUT_W1TC_REG;
  }

  if (_cs != GFX_NOT_DEFINED)
  {
    pinMode(_cs, OUTPUT);
    digitalWrite(_cs, HIGH); // disable chip select
  }
  if (_cs >= 32)
  {
    _csPinMask = digitalPinToBitMask(_cs);
    _csPortSet = (PORTreg_t)GPIO_OUT1_W1TS_REG;
    _csPortClr = (PORTreg_t)GPIO_OUT1_W1TC_REG;
  }
  else if (_cs != GFX_NOT_DEFINED)
  {
    _csPinMask = digitalPinToBitMask(_cs);
    _csPortSet = (PORTreg_t)GPIO_OUT_W1TS_REG;
    _csPortClr = (PORTreg_t)GPIO_OUT_W1TC_REG;
  }

  pinMode(_wr, OUTPUT);
  digitalWrite(_wr, HIGH); // Set write strobe high (inactive)
  if (_wr >= 32)
  {
    _wrPinMask = digitalPinToBitMask(_wr);
    _wrPortSet = (PORTreg_t)GPIO_OUT1_W1TS_REG;
    _wrPortClr = (PORTreg_t)GPIO_OUT1_W1TC_REG;
  }
  else
  {
    _wrPinMask = digitalPinToBitMask(_wr);
    _wrPortSet = (PORTreg_t)GPIO_OUT_W1TS_REG;
    _wrPortClr = (PORTreg_t)GPIO_OUT_W1TC_REG;
  }

  if (_rd != GFX_NOT_DEFINED)
  {
    pinMode(_rd, OUTPUT);
    digitalWrite(_rd, HIGH);
  }

  pinMode(0, OUTPUT);
  pinMode(1, OUTPUT);
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);

  // INIT 8-bit mask
  _dataClrMask = (1 << _wr) | 0xFF;
  _dataPortSet = (PORTreg_t)GPIO_OUT_W1TS_REG;
  _dataPortClr = (PORTreg_t)GPIO_OUT_W1TC_REG;
  *_dataPortClr = _dataClrMask;

  return true;
}

void Arduino_ESP32S2PAR8Q::beginWrite()
{
  DC_HIGH();
  CS_LOW();
}

void Arduino_ESP32S2PAR8Q::endWrite()
{
  CS_HIGH();
}

void Arduino_ESP32S2PAR8Q::writeCommand(uint8_t c)
{
  DC_LOW();

  WRITE(c);

  DC_HIGH();
}

void Arduino_ESP32S2PAR8Q::writeCommand16(uint16_t c)
{
  DC_LOW();

  _data16.value = c;
  WRITE(_data16.msb);
  WRITE(_data16.lsb);

  DC_HIGH();
}

void Arduino_ESP32S2PAR8Q::writeCommandBytes(uint8_t *data, uint32_t len)
{
  DC_LOW();

  while (len--)
  {
    WRITE(*data++);
  }

  DC_HIGH();
}

void Arduino_ESP32S2PAR8Q::write(uint8_t d)
{
  WRITE(d);
}

void Arduino_ESP32S2PAR8Q::write16(uint16_t d)
{
  _data16.value = d;
  WRITE(_data16.msb);
  WRITE(_data16.lsb);
}

void Arduino_ESP32S2PAR8Q::writeRepeat(uint16_t p, uint32_t len)
{
  _data16.value = p;
  if (_data16.msb == _data16.lsb)
  {
    *_dataPortClr = _dataClrMask;
    *_dataPortSet = _data16.msb;
    *_wrPortSet = _wrPinMask;
    *_wrPortClr = _wrPinMask;
    *_wrPortSet = _wrPinMask;
    while (--len)
    {
      *_wrPortClr = _wrPinMask;
      *_wrPortSet = _wrPinMask;
      *_wrPortClr = _wrPinMask;
      *_wrPortSet = _wrPinMask;
    }
  }
  else
  {
    while (len--)
    {
      *_dataPortClr = _dataClrMask;
      *_dataPortSet = _data16.msb;
      *_wrPortSet = _wrPinMask;

      *_dataPortClr = _dataClrMask;
      *_dataPortSet = _data16.lsb;
      *_wrPortSet = _wrPinMask;
    }
  }
}

void Arduino_ESP32S2PAR8Q::writePixels(uint16_t *data, uint32_t len)
{
  while (len--)
  {
    _data16.value = *data++;
    WRITE(_data16.msb);
    WRITE(_data16.lsb);
  }
}

void Arduino_ESP32S2PAR8Q::writeC8D8(uint8_t c, uint8_t d)
{
  DC_LOW();

  WRITE(c);

  DC_HIGH();

  WRITE(d);
}

void Arduino_ESP32S2PAR8Q::writeC8D16(uint8_t c, uint16_t d)
{
  DC_LOW();

  WRITE(c);

  DC_HIGH();

  _data16.value = d;
  WRITE(_data16.msb);
  WRITE(_data16.lsb);
}

void Arduino_ESP32S2PAR8Q::writeC8D16D16(uint8_t c, uint16_t d1, uint16_t d2)
{
  DC_LOW();

  WRITE(c);

  DC_HIGH();

  _data16.value = d1;
  WRITE(_data16.msb);
  WRITE(_data16.lsb);

  _data16.value = d2;
  WRITE(_data16.msb);
  WRITE(_data16.lsb);
}

void Arduino_ESP32S2PAR8Q::writeBytes(uint8_t *data, uint32_t len)
{
  while (len--)
  {
    WRITE(*data++);
  }
}

void Arduino_ESP32S2PAR8Q::writeIndexedPixels(uint8_t *data, uint16_t *idx, uint32_t len)
{
  while (len--)
  {
    _data16.value = idx[*data++];
    WRITE(_data16.msb);
    WRITE(_data16.lsb);
  }
}

void Arduino_ESP32S2PAR8Q::writeIndexedPixelsDouble(uint8_t *data, uint16_t *idx, uint32_t len)
{
  while (len--)
  {
    _data16.value = idx[*data++];
    WRITE(_data16.msb);
    WRITE(_data16.lsb);
    WRITE(_data16.msb);
    WRITE(_data16.lsb);
  }
}

GFX_INLINE void Arduino_ESP32S2PAR8Q::WRITE(uint8_t d)
{
  *_dataPortClr = _dataClrMask;
  *_dataPortSet = d;
  *_wrPortSet = _wrPinMask;
}

/******** low level bit twiddling **********/

GFX_INLINE void Arduino_ESP32S2PAR8Q::DC_HIGH(void)
{
  *_dcPortSet = _dcPinMask;
}

GFX_INLINE void Arduino_ESP32S2PAR8Q::DC_LOW(void)
{
  *_dcPortClr = _dcPinMask;
}

GFX_INLINE void Arduino_ESP32S2PAR8Q::CS_HIGH(void)
{
  if (_cs != GFX_NOT_DEFINED)
  {
    *_csPortSet = _csPinMask;
  }
}

GFX_INLINE void Arduino_ESP32S2PAR8Q::CS_LOW(void)
{
  if (_cs != GFX_NOT_DEFINED)
  {
    *_csPortClr = _csPinMask;
  }
}

#endif // #if defined(ESP32) && (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3)
