#include "LEDStrip.h"
#include "driver/i2s.h"
#include <Arduino.h>
#include "HSVTable.h"

static const char* tag = "LEDStrip";

LEDSTRIP::LEDSTRIP(int port, int size, int pin, LED_ORDER ledOrder, int shift, int offset)
{
  this->port = port;
  this->size = size;
  this->shift = shift;
  this->offset = offset;
  this->effSize = this->size - this->offset;
  this->ledOrder = ledOrder;

  // create pixel array and buffer array
  pixels = new  rgbpixel_t[size];
  out_buffer_size = size * PIXEL_SIZE;
  out_buffer = new uint8_t[out_buffer_size + RESET_BUFFER_SIZE];
  reset_buffer = &out_buffer[out_buffer_size + 1]; // pointer to reset buffer section
  active_out_buffer = &out_buffer[0] + (offset * PIXEL_SIZE); // start of the first active
  
  // initialize buffer
  memset(pixels, 0, this->size * sizeof(rgbpixel_t));
  memset(out_buffer, 0, out_buffer_size + RESET_BUFFER_SIZE);

  dma_frame_size = I2S_TX_DMA_BUFF_SIZE;
  if((size * PIXEL_SIZE) < I2S_TX_DMA_BUFF_SIZE) {
    dma_frame_size = size * PIXEL_SIZE; 
  }

  // Although the buffer can be smaller than total buffer size it will require more cpu interrupts
  // So this provides for the entire size rounded up to the closes buffer chunk (I2S_TX_DMA_BUFF_SIZE)
  int dma_buff_count = 1 + ((out_buffer_size + RESET_BUFFER_SIZE + 2) / I2S_TX_DMA_BUFF_SIZE);

  // install driver
  i2s_driver_config_t i2s_config = {
    .mode = i2s_mode_t(I2S_MODE_MASTER | I2S_MODE_TX),
    .sample_rate = SAMPLE_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_8BIT,
    .channel_format = i2s_channel_fmt_t(I2S_CHANNEL_FMT_RIGHT_LEFT),
    .communication_format = (i2s_comm_format_t)(I2S_COMM_FORMAT_STAND_I2S),
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL2, // high interrupt priority,
    //.intr_alloc_flags = ESP_INTR_FLAG_LOWMED, // high interrupt priority,
    .dma_buf_count = dma_buff_count, //
    .dma_buf_len = I2S_TX_DMA_BUFF_SIZE,
    .use_apll = true
    }; // possibly needed for accuracy

  //i2sEventQueue = xQueueCreate(8, sizeof(i2s_event_t));
  esp_err_t err = i2s_driver_install((i2s_port_t)port, &i2s_config, 8, &i2sEventQueue);

  if(err){
    if(err == ESP_ERR_INVALID_ARG) {ESP_LOGE(tag, "I2S driver error: Parameter error");}
    else if(err == ESP_ERR_NO_MEM) {ESP_LOGE(tag, "I2S driver error: Out of Memory");}
    else if(err == ESP_ERR_INVALID_STATE) {ESP_LOGE(tag, "I2S driver error: Port is in use");}
  }
  else{
    i2s_pin_config_t pin_config = {.bck_io_num = I2S_PIN_NO_CHANGE,
                                  .ws_io_num = I2S_PIN_NO_CHANGE,
                                  .data_out_num = pin,
                                  .data_in_num = I2S_PIN_NO_CHANGE };

    ESP_LOGV(tag, "I2S port pin configured....");
    err = i2s_set_pin((i2s_port_t)port, &pin_config);
    if(err){
      if(err == ESP_ERR_INVALID_ARG) {ESP_LOGE(tag, "I2S driver error: Parameter error");}
      else if(err == ESP_FAIL) {ESP_LOGE(tag, "I2S driver error: IO error");}
    }
    else{
      ESP_LOGV(tag, "I2S pin config success!");
    }
  }

}


#if(HIGH_RES_BIT_RATE == 1)
static const uint8_t bitpatterns[2] = {0b11000000, 0b11111100};
void LEDSTRIP::updateBuff(void) {
  uint8_t* buff = active_out_buffer;
  for (uint16_t i = 0; i < this->effSize; i++) 
  {
    uint8_t red = pixels[i].red >> powerDiv;
    uint8_t grn = pixels[i].grn >> powerDiv;
    uint8_t blu = pixels[i].blu >> powerDiv;

    *buff++ = bitpatterns[red >> 7 & 0x01];
    *buff++ = bitpatterns[red >> 6 & 0x01];
    *buff++ = bitpatterns[red >> 5 & 0x01];
    *buff++ = bitpatterns[red >> 4 & 0x01];
    *buff++ = bitpatterns[red >> 3 & 0x01];
    *buff++ = bitpatterns[red >> 2 & 0x01];
    *buff++ = bitpatterns[red >> 1 & 0x01];
    *buff++ = bitpatterns[red & 0x01];

    *buff++ = bitpatterns[grn >> 7 & 0x01];
    *buff++ = bitpatterns[grn >> 6 & 0x01];
    *buff++ = bitpatterns[grn >> 5 & 0x01];
    *buff++ = bitpatterns[grn >> 4 & 0x01];
    *buff++ = bitpatterns[grn >> 3 & 0x01];
    *buff++ = bitpatterns[grn >> 2 & 0x01];
    *buff++ = bitpatterns[grn >> 1 & 0x01];
    *buff++ = bitpatterns[grn & 0x01];

    *buff++ = bitpatterns[blu >> 7 & 0x01];
    *buff++ = bitpatterns[blu >> 6 & 0x01];
    *buff++ = bitpatterns[blu >> 5 & 0x01];
    *buff++ = bitpatterns[blu >> 4 & 0x01];
    *buff++ = bitpatterns[blu >> 3 & 0x01];
    *buff++ = bitpatterns[blu >> 2 & 0x01];
    *buff++ = bitpatterns[blu >> 1 & 0x01];
    *buff++ = bitpatterns[blu & 0x01];
  }

  *buff++ = 0;
}
#else
static const uint16_t bitpatterns[4] = {0x88, 0x8e, 0xe8, 0xee}; 
void LEDSTRIP::updateBuff(void) 
{
  uint8_t* buff = active_out_buffer;

  switch(this->ledOrder){
    case ORDER_RGB:
      for (uint16_t i = 0; i < this->effSize; i++) 
      {
        uint8_t red = pixels[i].red >> powerDiv;
        uint8_t grn = pixels[i].grn >> powerDiv;
        uint8_t blu = pixels[i].blu >> powerDiv;

        *buff++ = bitpatterns[red >> 6 & 0x03]; 
        *buff++ = bitpatterns[red >> 4 & 0x03];
        *buff++ = bitpatterns[red >> 2 & 0x03];
        *buff++ = bitpatterns[red & 0x03]; 

        *buff++ = bitpatterns[grn >> 6 & 0x03];
        *buff++ = bitpatterns[grn >> 4 & 0x03]; 
        *buff++ = bitpatterns[grn >> 2 & 0x03]; 
        *buff++ = bitpatterns[grn & 0x03]; 

        *buff++ = bitpatterns[blu >> 6 & 0x03];
        *buff++ = bitpatterns[blu >> 4 & 0x03];
        *buff++ = bitpatterns[blu >> 2 & 0x03];
        *buff++ = bitpatterns[blu & 0x03];
      }
      break;
    case ORDER_RBG:
      for (uint16_t i = 0; i < this->size; i++) 
      {
        uint8_t red = pixels[i].red >> powerDiv;
        uint8_t grn = pixels[i].grn >> powerDiv;
        uint8_t blu = pixels[i].blu >> powerDiv;

        *buff++ = bitpatterns[red >> 6 & 0x03];
        *buff++ = bitpatterns[red >> 4 & 0x03];
        *buff++ = bitpatterns[red >> 2 & 0x03];
        *buff++ = bitpatterns[red & 0x03];

        *buff++ = bitpatterns[blu >> 6 & 0x03];
        *buff++ = bitpatterns[blu >> 4 & 0x03];
        *buff++ = bitpatterns[blu >> 2 & 0x03];
        *buff++ = bitpatterns[blu & 0x03];

        *buff++ = bitpatterns[grn >> 6 & 0x03];
        *buff++ = bitpatterns[grn >> 4 & 0x03];
        *buff++ = bitpatterns[grn >> 2 & 0x03];
        *buff++ = bitpatterns[grn & 0x03];
      }
      break;
    case ORDER_GRB:
      for (uint16_t i = 0; i < this->size; i++) 
      {
        uint8_t red = pixels[i].red >> powerDiv;
        uint8_t grn = pixels[i].grn >> powerDiv;
        uint8_t blu = pixels[i].blu >> powerDiv;

        *buff++ = bitpatterns[grn >> 6 & 0x03];
        *buff++ = bitpatterns[grn >> 4 & 0x03];
        *buff++ = bitpatterns[grn >> 2 & 0x03];
        *buff++ = bitpatterns[grn & 0x03];

        *buff++ = bitpatterns[red >> 6 & 0x03];
        *buff++ = bitpatterns[red >> 4 & 0x03];
        *buff++ = bitpatterns[red >> 2 & 0x03];
        *buff++ = bitpatterns[red & 0x03];

        *buff++ = bitpatterns[blu >> 6 & 0x03];
        *buff++ = bitpatterns[blu >> 4 & 0x03];
        *buff++ = bitpatterns[blu >> 2 & 0x03];
        *buff++ = bitpatterns[blu & 0x03]; 
      }
      break;
    case ORDER_GBR:
      for (uint16_t i = 0; i < this->size; i++) 
      {
        uint8_t red = pixels[i].red >> powerDiv;
        uint8_t grn = pixels[i].grn >> powerDiv;
        uint8_t blu = pixels[i].blu >> powerDiv;

        *buff++ = bitpatterns[grn >> 6 & 0x03];
        *buff++ = bitpatterns[grn >> 4 & 0x03];
        *buff++ = bitpatterns[grn >> 2 & 0x03];
        *buff++ = bitpatterns[grn & 0x03];

        *buff++ = bitpatterns[blu >> 6 & 0x03];
        *buff++ = bitpatterns[blu >> 4 & 0x03];
        *buff++ = bitpatterns[blu >> 2 & 0x03];
        *buff++ = bitpatterns[blu & 0x03];

        *buff++ = bitpatterns[red >> 6 & 0x03];
        *buff++ = bitpatterns[red >> 4 & 0x03];
        *buff++ = bitpatterns[red >> 2 & 0x03];
        *buff++ = bitpatterns[red & 0x03]; 
      }
      break;
    case ORDER_BRG:
      for (uint16_t i = 0; i < this->size; i++) 
      {
        uint8_t red = pixels[i].red >> powerDiv;
        uint8_t grn = pixels[i].grn >> powerDiv;
        uint8_t blu = pixels[i].blu >> powerDiv;

        *buff++ = bitpatterns[blu >> 6 & 0x03];
        *buff++ = bitpatterns[blu >> 4 & 0x03];
        *buff++ = bitpatterns[blu >> 2 & 0x03];
        *buff++ = bitpatterns[blu & 0x03];

        *buff++ = bitpatterns[red >> 6 & 0x03];
        *buff++ = bitpatterns[red >> 4 & 0x03];
        *buff++ = bitpatterns[red >> 2 & 0x03];
        *buff++ = bitpatterns[red & 0x03];

        *buff++ = bitpatterns[grn >> 6 & 0x03];
        *buff++ = bitpatterns[grn >> 4 & 0x03];
        *buff++ = bitpatterns[grn >> 2 & 0x03];
        *buff++ = bitpatterns[grn & 0x03]; 
      }
      break;
    default: //ORDER_BGR:
      for (uint16_t i = 0; i < this->size; i++) 
      {
        uint8_t red = pixels[i].red >> powerDiv;
        uint8_t grn = pixels[i].grn >> powerDiv;
        uint8_t blu = pixels[i].blu >> powerDiv;

        *buff++ = bitpatterns[blu >> 6 & 0x03];
        *buff++ = bitpatterns[blu >> 4 & 0x03];
        *buff++ = bitpatterns[blu >> 2 & 0x03];
        *buff++ = bitpatterns[blu & 0x03];

        *buff++ = bitpatterns[grn >> 6 & 0x03];
        *buff++ = bitpatterns[grn >> 4 & 0x03];
        *buff++ = bitpatterns[grn >> 2 & 0x03];
        *buff++ = bitpatterns[grn & 0x03];

        *buff++ = bitpatterns[red >> 6 & 0x03];
        *buff++ = bitpatterns[red >> 4 & 0x03];
        *buff++ = bitpatterns[red >> 2 & 0x03];
        *buff++ = bitpatterns[red & 0x03]; 
      }
      break;
  }
}
#endif

void LEDSTRIP::show(bool update)
{
  if(update){ updateBuff(); };

  //i2s_stop((i2s_port_t)this->port);
  i2s_start((i2s_port_t)this->port);
  i2s_zero_dma_buffer((i2s_port_t)this->port);
  i2s_write_data(out_buffer, out_buffer_size + RESET_BUFFER_SIZE);

  /*
  i2s_event_t i2s_event;
  while(1){
    if(xQueueReceive(i2sEventQueue, &i2s_event, 100)){
      if(i2s_event.type == I2S_EVENT_TX_DONE) { break;}
    }else{
      break;
    }
  }
  */
}

void LEDSTRIP::setPowerDiv(uint8_t div){
  if(div < 0){div = 0;}
  if(div > 4){div = 4;}
  powerDiv = div;
}

// Function to write data to the I2S buffer
void LEDSTRIP::i2s_write_data(const void *data, size_t len) 
{
  size_t bytesWritten = 0;

  while (bytesWritten < len) {
    size_t bytesToWrite = len - bytesWritten;

    size_t written = 0;
    esp_err_t err = i2s_write(I2S_NUM_0, (const char*)data + bytesWritten, bytesToWrite, &written, portMAX_DELAY);
    if (err != ESP_OK) {
      // Handle error
      ESP_LOGE(tag, "i2s send error: %d", err);
      break;
    }

    bytesWritten += written;
  }
}

inline int LEDSTRIP::calcIndex(int index) 
{
  //int x = (index + shift) % effSize;
  //if(x < 0) { // convert nex index to positive range
  //  x += effSize;
  //}
  //return x + offset;
  int x = (index + shift) % effSize;
  x = (x < 0) ? (x + effSize) : x;
  return (x + offset) % effSize;
}

void LEDSTRIP::setPixel(int index, rgbpixel_t& col) 
{
  pixels[calcIndex(index)] = col;
}

void LEDSTRIP::setPixel(int index, const rgbpixel_t col)
{
  pixels[calcIndex(index)] = col;
}

/*
void LEDSTRIP::setPixel(int index, rgbpixel_t col, uint8_t scale) 
{
  uint16_t n = scale + 1;
  uint8_t r = (col.red * n) >> 8;
  uint8_t g = (col.grn * n) >> 8;
  uint8_t b = (col.blu * n) >> 8;
  pixels[calcIndex(index)] = {r, g, b};
}
*/

void LEDSTRIP::setPixelRaw(int index, rgbpixel_t& col){
  pixels[index] = col;
}

void LEDSTRIP::setPixelMirrored(int index, rgbpixel_t col)
{
  pixels[calcIndex(index)] = col; 
  pixels[calcIndex(-index-1)] = col;
}

rgbpixel_t LEDSTRIP::getPixel(int index) 
{
  return pixels[calcIndex(index)];
}

void LEDSTRIP::rotatePixels(LED_DIR dir)
{
  // store the shifted out pixels
  rgbpixel_t tPix;
  if(dir == DIR_FWD){
    tPix = pixels[size -1];
    for(int i=size-1; i > offset; i--){
      pixels[i] = pixels[i-1];
    }
    pixels[offset] = tPix;
  }else{
    tPix = pixels[offset];
    for(int i=offset; i < size-1; i++){
      pixels[i] = pixels[i+1];
    }
    pixels[size-1] = tPix;
  }
}

void LEDSTRIP::shiftPixels(int numPixels, LED_DIR dir)
{
  // Calculate the shift direction and absolute shift count
  int shiftDir = (dir == DIR_FWD) ? 1 : -1;
  int absShiftCount = (numPixels % this->effSize) * shiftDir;

  // Shift the pixels by swapping them in place using circular buffering
  for (int i = 0; i < this->effSize; i++) {
    rgbpixel_t temp = pixels[i];
    int j = i;

    while (true) {
      int k = j + absShiftCount;

      if (k < 0) {
        k += this->effSize;
      } else if (k >= this->effSize) {
        k -= this->effSize;
      }

      if (k == i) {
        break;
      }

      pixels[j] = pixels[k];
      j = k;
    }

    pixels[j] = temp;

    // Check if we have completed a cycle
    if (j == i + absShiftCount) {
      break;
    }
  }
}

void scalePixel(rgbpixel_t& pix, uint8_t _scale)
{
  uint16_t n = _scale + 1;
  pix.red = (pix.red * n) >> 8;
  pix.grn = (pix.grn * n) >> 8;
  pix.blu = (pix.blu * n) >> 8;
}

void linearizePixel(rgbpixel_t& pix){
  pix.red = ((int)pix.red * (int)pix.red ) >> 8;
  pix.grn = ((int)pix.grn * (int)pix.grn ) >> 8;
  pix.blu = ((int)pix.blu * (int)pix.blu ) >> 8;
}


void PixelFadeToBlack(rgbpixel_t& pix, uint8_t fadeValue) {
	pix.red = (pix.red <= 8) ? 0 : pix.red - ((pix.red * fadeValue)>>8);
	pix.grn = (pix.grn <= 8) ? 0 : pix.grn - ((pix.grn * fadeValue)>>8);
	pix.blu = (pix.blu <= 8) ? 0 : pix.blu - ((pix.blu * fadeValue)>>8);
}

void LEDSTRIP::zeroPixels(void)
{
  for(int i=0; i < this->size; i++){
    pixels[i] = {0,0,0};
  }
}

void LEDSTRIP::fill(rgbpixel_t color, int startIndex, int count)
{
  int endIndex = startIndex + count;
  for (int i = startIndex; i < endIndex; i++) {
    pixels[calcIndex(i)] = color;
  }
}

// a factor of 0=no changer, factor=255 newCol replaces 100%
void LEDSTRIP::transitionPixel(int index, rgbpixel_t& newCol, uint8_t factor) 
{
  int i = calcIndex(index);
  int f = factor + 1;
  pixels[i].red = (pixels[i].red * (256 - f) + newCol.red * f) >> 8;
  pixels[i].grn = (pixels[i].grn * (256 - f) + newCol.grn * f) >> 8;
  pixels[i].blu = (pixels[i].blu * (256 - f) + newCol.blu * f) >> 8;
}

LED_ORDER getRGBOrder(const char* order){
  if(strcmp(order, "rgb")==0){ return ORDER_RGB; }
  if(strcmp(order, "rbg")==0){ return ORDER_RBG; }
  if(strcmp(order, "grb")==0){ return ORDER_GRB; }
  if(strcmp(order, "gbr")==0){ return ORDER_GBR; }
  if(strcmp(order, "brg")==0){ return ORDER_BRG; }
  if(strcmp(order, "bgr")==0){ return ORDER_BGR; }
  else{ return ORDER_GRB; }
}