#include <Arduino.h>
#include "gamma.h"

#if defined(TEENSYDUINO)
    #include <SPI_TEENSY.h>
    #include <SD.h>
#else
    #include <SPIFFS.h>
    #include <WiFi.h>
    #include <WiFiClient.h>
    #include <WebServer.h>
    #include <ESPmDNS.h>
    #include <HTTPUpdateServer.h>
    #include <HTTPClient.h>
    #include "FastSPI.h"
    #include "soc/soc.h"
    #include "soc/rtc_cntl_reg.h"
#endif

#define SPI_SPEED 25000000

#define PARTS 256
#if ESFERA==1
  #define LEDS_TOTAL 57
  #define CORRECTION PARTS*4/8
#elif ESFERA==2
  #define LEDS_TOTAL 43
  #define CORRECTION PARTS*7/8
#elif ESFERA==3
  #define LEDS_TOTAL 29
  //#define CORRECTION PARTS*8/8
  #define CORRECTION 0
#elif ESFERA==4
  #define LEDS_TOTAL 15
  #define CORRECTION PARTS*3/8
#endif

#define BRIGHTNESS 16

#if !defined(TEENSYDUINO)
    SPIClass SPI2(HSPI);
    #define SEL_A 26
    #define SEL_B 22
    #define IRQ_HALL 4
    #define PIN_LED 2
#else
    #define SEL_A 29
    #define SEL_B 28
    #define IRQ_HALL 18
#endif

uint32_t start_frame = 0x00000000;
//uint32_t ledData[LEDS_TOTAL*PARTS];

uint32_t *ledData;
uint32_t *ledData1;
uint32_t *ledData2;


//uint32_t ledData1[LEDS_TOTAL*PARTS];
//uint32_t ledData2[LEDS_TOTAL*PARTS];

//uint32_t ledData1[1];
uint32_t end_frame = 0xFFFFFFFF;

#if !defined(TEENSYDUINO)
  const char * ssid = "HANGAR_naus";
  const char * password = "nDfEXDwRr4Py4fvRdfMU";

  //const char * ssid = "Miguel";
  //const char * password = "labinteractius";

  //const char * ssid = "Hello_pepe";
  //const char * password = "labinteractius";

  WebServer httpServer(80);
  HTTPUpdateServer httpUpdater;
  const char * host = "esp32";
#else
 uint64_t xthal_get_ccount()
  {
    return(micros());
  }
#endif

volatile bool turn_ok = false; 
//volatile unsigned long time_turn= micros();
//volatile unsigned long time_fps = micros();
volatile uint32_t time_turn = xthal_get_ccount();
volatile uint32_t time_fps = xthal_get_ccount();
volatile uint32_t time_fps_ant = xthal_get_ccount();
volatile unsigned long time_refresh = 10; 
volatile int turn_angle = 0;

#if !defined(TEENSYDUINO)
  portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
  void IRAM_ATTR HALL_ISR()
#else
  void HALL_ISR()
#endif
  {
    #if !defined(TEENSYDUINO)
      portENTER_CRITICAL_ISR(&mux);
    #endif
    digitalWrite(PIN_LED, HIGH);
    time_fps = xthal_get_ccount()-time_turn;
    time_refresh = time_fps/PARTS;
    time_turn = xthal_get_ccount();
    turn_angle=0;
    turn_ok = true;
    #if !defined(TEENSYDUINO)
      portEXIT_CRITICAL_ISR(&mux);
    #endif
  }

void write32(uint32_t *dataA, uint32_t *dataB, uint32_t size)
    {
      while (size>0)
      {
        if (size>=16) 
          {
              SPI.write32b(dataA,16);
              #if defined(TEENSYDUINO)
                  SPI1.write32b(dataB, 16);
              #else
                  SPI2.write32b(dataB, 16);
              #endif
              size-=16;
              dataA+=16;
              dataB+=16;
          }
        else
          {
              SPI.write32b(dataA,size);
              #if defined(TEENSYDUINO)
                    SPI1.write32b(dataB, size);
              #else
                  SPI2.write32b(dataB, size);
              #endif
              size=0;
          }
      }
    }

//char* frame[5] =  {"/Petronas/Particle00.bmp", "/Petronas/Particle01.bmp", "/Petronas/Particle02.bmp", "/Petronas/Particle03.bmp", "/Petronas/Particle04.bmp"};
//char* frame[4] =  {"/cats.bmp", "/test_azul.bmp", "/test_rojo.bmp", "/test_verde.bmp",};

#if ESFERA==1
  //char* frame[4] =  {"/test_rojo_57.bmp", "/test_azul.bmp", "/test_rojo.bmp", "/test_verde.bmp",};
  char* frame[4] =  {"/fire.bmp", "/mapamundi_57.bmp", "/test_rojo_57.bmp", "/test_verde.bmp",};
#elif ESFERA==2
  char* frame[4] =  {"/mapamundi_43.bmp", "/test_azul.bmp", "/test_rojo.bmp", "/test_verde.bmp",};
#elif ESFERA==3
  char* frame[4] =  {"/test_blue_29.bmp", "/test_azul.bmp", "/test_rojo.bmp", "/test_verde.bmp",};
#elif ESFERA==4
  char* frame[4] =  {"/test_white_15.bmp", "/test_azul.bmp", "/test_rojo.bmp", "/test_verde.bmp",};
#endif

volatile int num_image = 0;

#if !defined(TEENSYDUINO)
  TaskHandle_t Task1;
  TaskHandle_t Task2;
#endif

//volatile unsigned long time_to_show = micros(); 
volatile unsigned long time_to_show = xthal_get_ccount();
volatile int column = 0;
uint32_t *imagePointer;
uint32_t *imagePointer1;
volatile bool change = false;

#if !defined(TEENSYDUINO)
  void Task_refresh( void * parameter) {
#else
  void Task_refresh() {
#endif
  for(;;) {
      if ((xthal_get_ccount()-time_to_show)>=time_refresh)
          {
            time_to_show = xthal_get_ccount();

            if((turn_ok)&&(turn_angle==CORRECTION))
              {
                digitalWrite(PIN_LED, LOW);
                column = 0;
                turn_ok = false;
                if (num_image%2==0) ledData = ledData1;
                else ledData = ledData2;
              }
            if(column>(PARTS-1)) column=0;

            //if ((column==0)&&(num_image%2!=0)) change = true;
            //else if((column==0)&&(num_image%2==0))change = false;

            //unsigned long time_measure = micros();
            digitalWrite(SEL_A, HIGH);
            imagePointer=ledData + LEDS_TOTAL*column;
            if (column<=PARTS*3/4) imagePointer1=ledData + LEDS_TOTAL*(column + PARTS/4); 
            else imagePointer1=ledData + LEDS_TOTAL*(column - PARTS*3/4); 
            SPI.write32(start_frame);
            #if defined(TEENSYDUINO)
              SPI1.write32(start_frame);
              write32(imagePointer, imagePointer1, LEDS_TOTAL);
              SPI1.write32(end_frame);
            #else
              SPI2.write32(start_frame);
              write32(SPI2.bus(), SPI.bus(), imagePointer, imagePointer1, LEDS_TOTAL);
              SPI2.write32(end_frame);
            #endif
            SPI.write32(end_frame);
            
            digitalWrite(SEL_A, LOW);

            digitalWrite(SEL_B, HIGH);
            if (column<=PARTS/2) imagePointer=ledData + LEDS_TOTAL*(column + PARTS/2); 
            else imagePointer=ledData + LEDS_TOTAL*(column - PARTS/2); 
            if (column<=PARTS/4) imagePointer1=ledData + LEDS_TOTAL*(column + PARTS*3/4); 
            else imagePointer1=ledData + LEDS_TOTAL*(column - PARTS/4); 
            
            SPI.write32(start_frame);
            #if defined(TEENSYDUINO)
              SPI1.write32(start_frame);
              write32(imagePointer, imagePointer1, LEDS_TOTAL);
              SPI1.write32(end_frame);
            #else
              SPI2.write32(start_frame);
              write32(SPI2.bus(), SPI.bus(), imagePointer, imagePointer1, LEDS_TOTAL);
              SPI2.write32(end_frame);
            #endif
            SPI.write32(end_frame);
            
            digitalWrite(SEL_B, LOW);
            //Serial.println(micros()-time_measure);
            column++;
            turn_angle++;
          }
  }
}

void readBMP()
  {  
    uint8_t sdBuf[1024];  // One SD block (also for NeoPixel color data)
    char    *inName;
    uint8_t *brightness;
    int       bmpWidth,            // BMP width in pixels
              bmpHeight,           // BMP height in pixels
              bmpStartCol,         // First BMP column to process (crop/center)
              column;              // and column (X)
    uint32_t  bmpImageoffset;      // Start of image data in BMP file
    uint8_t  BLUE,
             GREEN,
             RED;

    /*Serial.print(F("Reading file '"));
    Serial.print("mapamundi.bmp");
    Serial.print(F("'..."));*/
    #if defined(TEENSYDUINO)
      File inFile = SD.open(frame[num_image]);
    #else
      File inFile = SPIFFS.open(frame[num_image]);
    #endif

    if(!inFile) {
      Serial.println(F("error"));
      return;
    }
    if(inFile.read(sdBuf, 34)             &&    // Load header
      (*(uint16_t *)&sdBuf[ 0] == 0x4D42) &&    // BMP signature
      (*(uint16_t *)&sdBuf[26] == 1)      &&    // Planes: must be 1
      (*(uint16_t *)&sdBuf[28] == 24)     &&    // Bits per pixel: must be 24
      (*(uint32_t *)&sdBuf[30] == 0)) 
          {         // Compression: must be 0 (none)
              // Supported BMP format -- proceed!
              bmpImageoffset = *(uint32_t *)&sdBuf[10]; // Start of image data
              bmpWidth       = *(uint32_t *)&sdBuf[18]; // Image dimensions
              bmpHeight      = *(uint32_t *)&sdBuf[22];
              /*Serial.print(F("Start: "));
              Serial.println(bmpImageoffset);
              Serial.print(bmpWidth);
              Serial.write('x');
              Serial.print(bmpHeight);
              Serial.println(F(" pixels"));*/
          }
      else 
        {
          Serial.println(F("error to read"));
          inFile.close();
          return;
        }
      inFile.read(sdBuf, bmpImageoffset-34);
      for(int row = 0; row<LEDS_TOTAL; row++)
        {
          inFile.read(sdBuf, 3*bmpWidth);
          column = 0;
          for(int j = 0; j<3*PARTS; j=j+3)
            {
              BLUE = pgm_read_byte(&gamma_led[*(uint8_t *)&sdBuf[j]]);
              GREEN =pgm_read_byte(&gamma_led[*(uint8_t *)&sdBuf[j+1]]);
              RED =  pgm_read_byte(&gamma_led[*(uint8_t *)&sdBuf[j+2]]);
              //ledData[1+(LEDS_TOTAL*column) + row] |= RED<<24|GREEN<<16|BLUE<<8;
              #if defined(TEENSYDUINO)
                if(num_image%2==0) ledData1[(LEDS_TOTAL*column) + row] |= RED<<16|GREEN<<8|BLUE;
                else ledData2[(LEDS_TOTAL*column) + row] |= RED<<16|GREEN<<8|BLUE;
              #else
                //if(num_image%2==0) ledData1[(LEDS_TOTAL*column) + row] |= RED<<24|GREEN<<16|BLUE<<8;
                //else ledData2[(LEDS_TOTAL*column) + row] |= RED<<24|GREEN<<16|BLUE<<8;
                if(num_image%2==0) ledData1[(LEDS_TOTAL*column) + row] = RED<<24|GREEN<<16|BLUE<<8|(0xE0|BRIGHTNESS);
                else ledData2[(LEDS_TOTAL*column) + row] = RED<<24|GREEN<<16|BLUE<<8|(0xE0|BRIGHTNESS);
              #endif
              column++;
            }
        }
      inFile.close();
  }

unsigned long time_change_frame = millis();

void Task_image( void * parameter) {
  for(;;) {   
    if ((millis() - time_change_frame)>=1000)
      {
        /*Serial.print(millis()-time_change_frame);
        Serial.print(" ");*/
        
        if (num_image==1) num_image=0;
        else num_image++;
        readBMP();             
        //Serial.println(millis()-time_change_frame);
        time_change_frame = millis();
      }
      vTaskDelay(1);
    }
}

void test_matrix()
  {
    #if ESFERA==1
      uint8_t BLUE = 32,
            GREEN =0,
            RED = 0;
    #elif ESFERA==2
      uint8_t BLUE = 0,
            GREEN =32,
            RED = 0;
    #elif ESFERA==3
      uint8_t BLUE = 0,
            GREEN =0,
            RED = 32;
    #elif ESFERA==4
      uint8_t BLUE = 32,
            GREEN =32,
            RED = 32;
    #endif
    #if !defined(TEENSYDUINO)
      for(int i=0; i<LEDS_TOTAL; i++) ledData[i] |= 0<<24|32<<16|32<<8;
    //#else
    //  for(int i=0; i<LEDS_TOTAL; i++) ledData[i] |= 0<<16|32<<8|32<<0;
    #endif

    for(column=1; column<PARTS; column++) 
      {
        if ((column%32)==0) 
          {
            for(int i=0; i<LEDS_TOTAL; i++) 
              #if !defined(TEENSYDUINO)
                ledData[i+LEDS_TOTAL*column] |= BLUE<<24|GREEN<<16|RED<<8;
              #else
                ledData[i+LEDS_TOTAL*column] |= RED<<24|GREEN<<16|BLUE<<8;
              #endif

              
          }
        else 
          {
            #if !defined(TEENSYDUINO)
              ledData[0+LEDS_TOTAL*column] |= BLUE<<24|GREEN<<16|RED<<8;
              ledData[LEDS_TOTAL*3/4+LEDS_TOTAL*column] |= BLUE<<24|GREEN<<16|RED<<8;
              ledData[LEDS_TOTAL/2+LEDS_TOTAL*column] |= BLUE<<24|GREEN<<16|RED<<8;
              ledData[LEDS_TOTAL/4+LEDS_TOTAL*column] |= BLUE<<24|GREEN<<16|RED<<8;
              ledData[LEDS_TOTAL-1+LEDS_TOTAL*column] |= BLUE<<24|GREEN<<16|RED<<8;
            /*#else
              ledData[0+LEDS_TOTAL*column] |= RED<<16|GREEN<<8|BLUE;
              ledData[LEDS_TOTAL*3/4+LEDS_TOTAL*column] |= RED<<16|GREEN<<8|BLUE;
              ledData[LEDS_TOTAL/2+LEDS_TOTAL*column] |= RED<<16|GREEN<<8|BLUE;
              ledData[LEDS_TOTAL/4+LEDS_TOTAL*column] |= RED<<16|GREEN<<8|BLUE;
              ledData[LEDS_TOTAL-1+LEDS_TOTAL*column] |= RED<<16|GREEN<<8|BLUE;*/
            #endif
          }
      }
  }

void setup() {
  Serial.begin(115200);
  pinMode(SEL_A, OUTPUT);
  digitalWrite(SEL_A, LOW);
  pinMode(SEL_B, OUTPUT);
  digitalWrite(SEL_B, LOW);
  pinMode(IRQ_HALL, INPUT_PULLUP);

  #if !defined(TEENSYDUINO)
    pinMode(PIN_LED, OUTPUT);
    digitalWrite(PIN_LED, LOW);
    //ledData1=(uint32_t*)heap_caps_malloc(LEDS_TOTAL*PARTS, MALLOC_CAP_32BIT);
    //ledData2=(uint32_t*)heap_caps_malloc(LEDS_TOTAL*PARTS, MALLOC_CAP_32BIT);
    ledData1=(uint32_t*)malloc(LEDS_TOTAL*PARTS*4);
    ledData2=(uint32_t*)malloc(LEDS_TOTAL*PARTS*4);
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
  #else
    ledData1=(uint32_t*)malloc(LEDS_TOTAL*PARTS*4);
    ledData2=(uint32_t*)malloc(LEDS_TOTAL*PARTS*4);
  #endif

  #if !defined(TEENSYDUINO)
    
    while (WiFi.waitForConnectResult() != WL_CONNECTED) {
        WiFi.begin(ssid, password);
        Serial.println("WiFi failed, retrying.");
      }

    MDNS.begin(host);
    if (MDNS.begin(host)) {
      Serial.println("mDNS responder started");
    }
    
    httpUpdater.setup(&httpServer);
    httpServer.begin();

    MDNS.addService("http", "tcp", 80);
    Serial.printf("HTTPUpdateServer ready! Open http://%s.local/update in your browser\n", host);

    Serial.println("");
    Serial.print("Connected to ");
    Serial.println(ssid);
    Serial.print("IP address: ");
    Serial.println(WiFi.localIP());
  #endif

  #if defined(TEENSYDUINO)
      SD.begin(BUILTIN_SDCARD);
      SPI.begin();
      SPI1.begin();
      SPI1.beginTransaction(SPISettings(SPI_SPEED, MSBFIRST, SPI_MODE0));
  #else
      if(!SPIFFS.begin(true)){
        Serial.println(F("An Error has occurred while mounting SPIFFS"));
        return;
      }
      SPI.begin(14,12,13,15);
      SPI2.begin(18,19,23,5);
      SPI2.beginTransaction(SPISettings(SPI_SPEED, MSBFIRST, SPI_MODE0));
  #endif
  SPI.beginTransaction(SPISettings(SPI_SPEED, MSBFIRST, SPI_MODE0));
  

  for(int i=0; i<LEDS_TOTAL*PARTS; i++) ledData1[i] = 0;
  for(int i=0; i<LEDS_TOTAL*PARTS; i++) ledData2[i] = 0;
  uint8_t temp_brightness = BRIGHTNESS;
  if(BRIGHTNESS>31) temp_brightness = 31;
  #if defined(TEENSYDUINO)
    for(int i=0; i<LEDS_TOTAL*PARTS; i++) ledData1[i] = (0xE0|temp_brightness); //(0x07|8<<4);
    for(int i=0; i<LEDS_TOTAL*PARTS; i++) ledData2[i] = (0xE0|temp_brightness); //(0x07|8<<4);
  #else
    for(int i=0; i<LEDS_TOTAL*PARTS; i++) ledData1[i] = (0xE0|temp_brightness); //(0x07|8<<4);
    for(int i=0; i<LEDS_TOTAL*PARTS; i++) ledData2[i] = (0xE0|temp_brightness); //(0x07|8<<4);
  #endif

  if (num_image%2==0)   ledData = ledData1;
  else ledData = ledData2;

  //readBMP();
  test_matrix();

  #if !defined(TEENSYDUINO)
    xTaskCreatePinnedToCore(
        Task_refresh, 
        "Task_refresh", 
        10000, 
        NULL,
        1, 
        &Task1,
        1);

    /*xTaskCreatePinnedToCore(
        Task_image, 
        "Task_image", 
        10000, 
        NULL,
        2, 
        &Task2,
        0);*/
  #endif

  attachInterrupt(IRQ_HALL, HALL_ISR, RISING); // start interrupt
}



void loop() {
  #if !defined(TEENSYDUINO)
    //httpServer.handleClient(); //Servidor para programar inalambricamente
  #else
    Task_refresh();
  #endif
}