#include <ESP32-HUB75-MatrixPanel-I2S-DMA.h>
#include <math.h>

// ===== Panel Configuration =====
const int panelResX = 64;
const int panelResY = 64;
const int panel_chain = 2;
const int WIDTH  = panelResX * panel_chain;  // 128
const int HEIGHT = panelResY;                // 64
const int CENTER_X = WIDTH / 2;
const int CENTER_Y = HEIGHT / 2;

MatrixPanel_I2S_DMA *matrix = nullptr;

// ===== Two Swirls (Galaxies) =====
#define NUM_STARS  100       
#define NUM_ARMS   3         
#define MAX_RADIUS 40.0f     
#define THICKNESS  6.0f      

// Each swirl star
struct SwirlStar {
  float baseX, baseY, baseZ;  
  uint8_t baseHue;
};

// We'll have two sets of swirl stars
SwirlStar swirlA[NUM_STARS];
SwirlStar swirlB[NUM_STARS];

// Initial swirl center positions
float swirlACenterX_init = -30.0f;
float swirlACenterY_init = 0.0f;
float swirlBCenterX_init = +30.0f;
float swirlBCenterY_init = 0.0f;

// Current swirl center positions (they orbit)
float swirlACenterX, swirlACenterY;
float swirlBCenterX, swirlBCenterY;

// Rotation angles for each swirl
float rotAx = 0.0f, rotAy = 0.0f, rotAz = 0.0f;
float rotBx = 0.0f, rotBy = 0.0f, rotBz = 0.0f;

// Speeds for each swirl
float speedAx = 0.01f, speedAy = 0.015f, speedAz = 0.008f;
float speedBx = 0.012f, speedBy = 0.013f, speedBz = 0.01f;

// Warp effect
const float WARP_MAX_ANGLE = M_PI / 9;  
const uint32_t WARP_PERIOD_MS = 10000;  

// Color cycle
const uint32_t COLOR_CYCLE_MS = 8000;

// Fade factor for trails
const uint8_t FADE_FACTOR = 230;

// We'll orbit the swirl centers around each other
float swirlOrbitRadius = 20.0f;
float swirlOrbitAngle = 0.0f;
float swirlOrbitSpeed = 0.003f;

// ---------- HSV to RGB Utility ----------
void hsvToRgb(uint8_t h, uint8_t s, uint8_t v,
              uint8_t &r, uint8_t &g, uint8_t &b) {
  uint8_t region = h / 43; 
  uint8_t remainder = (h - region * 43) * 6;
  uint8_t p = (v * (255 - s)) / 255;
  uint8_t q = (v * (255 - ((s * remainder) / 255))) / 255;
  uint8_t t = (v * (255 - ((s * (255 - remainder)) / 255))) / 255;
  switch (region) {
    case 0: r = v; g = t; b = p; break;
    case 1: r = q; g = v; b = p; break;
    case 2: r = p; g = v; b = t; break;
    case 3: r = p; g = q; b = v; break;
    case 4: r = t; g = p; b = v; break;
    default: r = v; g = p; b = q; break;
  }
}

// ========== Fade Buffer ==========
static uint8_t frameBuf[HEIGHT][WIDTH][3];

// ========== Explosion State ==========
bool inExplosion = false;       // are we currently in an explosion?
uint32_t explosionStartTime = 0; // when explosion started
const uint32_t EXPLOSION_DURATION = 2000; // 2 seconds
float collisionX = 0, collisionY = 0;     // where collision happened

// ========== Spiral Generation ==========
void generateSpiral(SwirlStar *swirl) {
  for (int i = 0; i < NUM_STARS; i++) {
    int arm = random(NUM_ARMS);
    float r = sqrtf((float)random(0, 10001) / 10000.0f) * MAX_RADIUS;
    float baseArmAngle = ((float)arm / NUM_ARMS) * 2.0f * M_PI;
    float twist = r * 0.8f;
    float noise = (((float)random(-100, 101)) / 1000.0f) * M_PI;
    float angle = baseArmAngle + twist + noise;
    float x = r * cosf(angle);
    float y = r * sinf(angle);
    float z = (((float)random(-100, 101)) / 100.0f) * (THICKNESS / 2.0f);
    swirl[i].baseX = x;
    swirl[i].baseY = y;
    swirl[i].baseZ = z;
    swirl[i].baseHue = (uint8_t)random(0, 256);
  }
}

void setup() {
  // Panel config
  HUB75_I2S_CFG mxconfig(panelResX, panelResY, panel_chain);
  mxconfig.double_buff = true;
  mxconfig.gpio.e = 18;
  mxconfig.clkphase = false;
  matrix = new MatrixPanel_I2S_DMA(mxconfig);
  matrix->begin();
  matrix->setBrightness8(200);
  matrix->fillScreen(matrix->color565(0, 0, 0));

  // Clear fade buffer
  for (int y = 0; y < HEIGHT; y++) {
    for (int x = 0; x < WIDTH; x++) {
      frameBuf[y][x][0] = 0;
      frameBuf[y][x][1] = 0;
      frameBuf[y][x][2] = 0;
    }
  }

  randomSeed(millis());
  generateSpiral(swirlA);
  generateSpiral(swirlB);

  // set initial swirl centers
  swirlACenterX = swirlACenterX_init;
  swirlACenterY = swirlACenterY_init;
  swirlBCenterX = swirlBCenterX_init;
  swirlBCenterY = swirlBCenterY_init;
}

// Draw swirl function
void drawSwirl(SwirlStar *swirl, float centerX, float centerY,
               float rotX, float rotY, float rotZ,
               float tiltAngle, uint8_t hueOffset)
{
  for (int i = 0; i < NUM_STARS; i++) {
    float x = swirl[i].baseX;
    float y = swirl[i].baseY;
    float z = swirl[i].baseZ;

    // Rotation around X
    float y1 = y * cosf(rotX) - z * sinf(rotX);
    float z1 = y * sinf(rotX) + z * cosf(rotX);
    // Rotation around Y
    float x2 = x * cosf(rotY) + z1 * sinf(rotY);
    float z2 = -x * sinf(rotY) + z1 * cosf(rotY);
    // Rotation around Z
    float x3 = x2 * cosf(rotZ) - y1 * sinf(rotZ);
    float y3 = x2 * sinf(rotZ) + y1 * cosf(rotZ);
    float z3 = z2;

    // Warp tilt around X
    float yw = y3 * cosf(tiltAngle) - z3 * sinf(tiltAngle);
    float zw = y3 * sinf(tiltAngle) + z3 * cosf(tiltAngle);

    // Translate swirl center in XY, keep Z
    float finalX = x3 + centerX;
    float finalY = yw + centerY;
    float finalZ = zw;

    // Perspective
    float cameraDist = 100.0f;
    float depth = cameraDist - finalZ;
    if (depth < 1.0f) continue;
    float scale = cameraDist / depth;
    float projX = finalX * scale;
    float projY = finalY * scale;

    int screenX = (int)(CENTER_X + projX);
    int screenY = (int)(CENTER_Y - projY);
    if (screenX < 0 || screenX >= WIDTH || screenY < 0 || screenY >= HEIGHT)
      continue;

    // Outer => faster => brighter
    float baseRadius = sqrtf(x*x + y*y);
    uint8_t brightness = (uint8_t)fmin(255.0f * (baseRadius / MAX_RADIUS), 255.0f);

    // Hue shift
    uint8_t hue = swirl[i].baseHue + hueOffset;
    uint8_t rr, gg, bb;
    hsvToRgb(hue, 255, brightness, rr, gg, bb);

    matrix->drawPixel(screenX, screenY, matrix->color565(rr, gg, bb));
    frameBuf[screenY][screenX][0] = rr;
    frameBuf[screenY][screenX][1] = gg;
    frameBuf[screenY][screenX][2] = bb;
  }
}

// Explosion drawing
void drawExplosion(uint32_t now, float cx, float cy) {
  // Explosion progress = 0..1
  float elapsed = (now - explosionStartTime);
  float progress = elapsed / (float)EXPLOSION_DURATION;
  if (progress>1.0f) progress=1.0f;

  // The radius of the blast grows from 0.. ~1.5*screen width
  float maxR = sqrtf((float)(WIDTH*WIDTH + HEIGHT*HEIGHT)) * 0.75f;
  float blastRadius = maxR * progress;

  // The color might fade or shift
  // Let's do a bright white with a slight red tint
  // that dims as it expands
  int baseVal = (int)(255 * (1.0f - progress));
  if (baseVal<0) baseVal=0;
  uint8_t rr = 255, gg = (uint8_t)baseVal, bb = (uint8_t)baseVal/2;

  // Convert center to screen coords
  // Because swirl centers are in the swirl's local space,
  // we might just treat them as screen coords. Or we do
  // perspective if we want. For simplicity, treat them
  // as 2D screen coords for the explosion center.
  int scx = (int)(CENTER_X + cx);
  int scy = (int)(CENTER_Y - cy);

  // Draw a radial fill in 2D
  for (int y=0; y<HEIGHT; y++) {
    for (int x=0; x<WIDTH; x++) {
      int dx = x - scx;
      int dy = y - scy;
      float dist = sqrtf(dx*dx + dy*dy);
      if (dist <= blastRadius) {
        // fraction how close to the edge
        float f = 1.0f - (dist/blastRadius);
        int cval = (int)(baseVal + (255-baseVal)*f);
        if (cval>255) cval=255;
        // store
        uint8_t oldR = frameBuf[y][x][0];
        uint8_t oldG = frameBuf[y][x][1];
        uint8_t oldB = frameBuf[y][x][2];
        // blend or just overwrite? let's do overwrite for dramatic effect
        uint8_t newR = (uint8_t)cval;
        uint8_t newG = (uint8_t)(cval/4);
        uint8_t newB = (uint8_t)(cval/6);
        // write
        frameBuf[y][x][0] = newR;
        frameBuf[y][x][1] = newG;
        frameBuf[y][x][2] = newB;
        matrix->drawPixel(x,y, matrix->color565(newR,newG,newB));
      }
    }
  }
}

// Reset the entire scene after explosion
void resetScene() {
  // Regenerate swirl star positions
  generateSpiral(swirlA);
  generateSpiral(swirlB);

  // reset swirl angles
  rotAx = rotAy = rotAz = 0.0f;
  rotBx = rotBy = rotBz = 0.0f;
  swirlOrbitAngle = 0.0f;

  // reset swirl center positions
  swirlACenterX = swirlACenterX_init;
  swirlACenterY = swirlACenterY_init;
  swirlBCenterX = swirlBCenterX_init;
  swirlBCenterY = swirlBCenterY_init;

  // clear fade buffer
  for (int y=0; y<HEIGHT; y++){
    for (int x=0; x<WIDTH; x++){
      frameBuf[y][x][0] = 0;
      frameBuf[y][x][1] = 0;
      frameBuf[y][x][2] = 0;
      matrix->drawPixel(x,y, 0); // black
    }
  }
  matrix->flipDMABuffer();
}

void loop() {
  uint32_t now = millis();

  // 1) Fade old frame
  for (int y = 0; y < HEIGHT; y++) {
    for (int x = 0; x < WIDTH; x++) {
      uint8_t r = frameBuf[y][x][0];
      uint8_t g = frameBuf[y][x][1];
      uint8_t b = frameBuf[y][x][2];
      r = (r * FADE_FACTOR) >> 8;
      g = (g * FADE_FACTOR) >> 8;
      b = (b * FADE_FACTOR) >> 8;
      frameBuf[y][x][0] = r;
      frameBuf[y][x][1] = g;
      frameBuf[y][x][2] = b;
      matrix->drawPixel(x, y, matrix->color565(r, g, b));
    }
  }

  // If in explosion mode
  if (inExplosion) {
    // draw explosion
    drawExplosion(now, collisionX, collisionY);

    // check if explosion ended
    if (now - explosionStartTime > EXPLOSION_DURATION) {
      inExplosion = false;
      // reset scene
      resetScene();
    }

    // done with this frame
    matrix->flipDMABuffer();
    yield();
    return;
  }

  // 2) Normal swirl dance
  // Warp tilt
  float warpPhase = 2.0f * M_PI * (float)((now % WARP_PERIOD_MS) / (double)WARP_PERIOD_MS);
  float tiltAngle = WARP_MAX_ANGLE * sinf(warpPhase);

  // Color cycle
  uint8_t timeHueOffset = (now % COLOR_CYCLE_MS) * 256 / COLOR_CYCLE_MS;

  // swirl orbit update
  swirlOrbitAngle += swirlOrbitSpeed;
  if (swirlOrbitAngle > 2*M_PI) swirlOrbitAngle -= 2*M_PI;
  float cosO = cosf(swirlOrbitAngle);
  float sinO = sinf(swirlOrbitAngle);
  swirlACenterX = swirlACenterX_init + swirlOrbitRadius * cosO;
  swirlACenterY = swirlACenterY_init + swirlOrbitRadius * sinO;
  swirlBCenterX = swirlBCenterX_init - swirlOrbitRadius * cosO;
  swirlBCenterY = swirlBCenterY_init - swirlOrbitRadius * sinO;

  // update swirl rotations
  rotAx += speedAx; if (rotAx>2*M_PI) rotAx-=2*M_PI;
  rotAy += speedAy; if (rotAy>2*M_PI) rotAy-=2*M_PI;
  rotAz += speedAz; if (rotAz>2*M_PI) rotAz-=2*M_PI;
  rotBx += speedBx; if (rotBx>2*M_PI) rotBx-=2*M_PI;
  rotBy += speedBy; if (rotBy>2*M_PI) rotBy-=2*M_PI;
  rotBz += speedBz; if (rotBz>2*M_PI) rotBz-=2*M_PI;

  // draw swirl A
  drawSwirl(swirlA, swirlACenterX, swirlACenterY, rotAx, rotAy, rotAz, tiltAngle, timeHueOffset);
  // draw swirl B
  drawSwirl(swirlB, swirlBCenterX, swirlBCenterY, rotBx, rotBy, rotBz, tiltAngle, timeHueOffset);

  // 3) Check collision
  float dx = swirlACenterX - swirlBCenterX;
  float dy = swirlACenterY - swirlBCenterY;
  float distSq = dx*dx + dy*dy;
  // if they are very close => collision
  if (distSq < 8.0f*8.0f) {
    inExplosion = true;
    explosionStartTime = now;
    // store collision coords in swirl local space for the explosion center
    collisionX = (swirlACenterX + swirlBCenterX)*0.5f; 
    collisionY = (swirlACenterY + swirlBCenterY)*0.5f;
  }

  matrix->flipDMABuffer();
  yield();
}