// PixelBallLEDMap.h
// Header file for storing and managing LED position mappings for a spherical LED ball
#ifndef PIXEL_BALL_LED_MAP_H
#define PIXEL_BALL_LED_MAP_H

#include <Arduino.h>

// Coordinate systems
struct CartesianCoord {
  float x, y, z; // Normalized pixel position on unit sphere
};

struct PolarCoord {
  float theta;  // Azimuthal angle (0 to 2PI) - around equator
  float phi;    // Polar angle (0 to PI) - from north pole to south pole
};

struct LEDPosition {
  int ledIndex;
  PolarCoord polarPos;
  CartesianCoord cartesianPos;
  bool isCalibrated;  // True if directly calibrated with sensor
  bool isSet;         // True if position is set (either calibrated or interpolated)
};

// Default configuration constants
#define SPHERE_NUM_LEDS 99

int sphereCardinalLEDs[6]; // +X, -X, +Y, -Y, +Z, -Z

// LED position storage
LEDPosition sphereLedPositions[SPHERE_NUM_LEDS] = {
  { 0, {4.722005f, 1.680841f}, {0.009558f, -0.109823f, -0.993905f}, true, true },
  { 1, {4.788909f, 1.193535f}, {0.071069f, 0.368376f, -0.926956f}, true, true },
  { 2, {5.212116f, 1.203100f}, {0.447156f, 0.359467f, -0.819045f}, true, true },
  { 3, {5.466645f, 1.511634f}, {0.683549f, 0.059128f, -0.727506f}, true, true },
  { 4, {5.532517f, 1.934996f}, {0.683271f, -0.356201f, -0.637386f}, true, true },
  { 5, {5.186481f, 2.091025f}, {0.396134f, -0.497079f, -0.772004f}, true, true },
  { 6, {4.740677f, 2.160064f}, {0.023514f, -0.555752f, -0.831015f}, true, true },
  { 7, {4.351302f, 1.861544f}, {-0.338463f, -0.286669f, -0.896250f}, true, true },
  { 8, {4.044606f, 1.664757f}, {-0.616515f, -0.093823f, -0.781733f}, true, true },
  { 9, {3.993980f, 1.306080f}, {-0.635261f, 0.261635f, -0.726629f}, true, true },
  { 10, {4.243778f, 1.190356f}, {-0.419355f, 0.371329f, -0.828406f}, true, true },
  { 11, {4.754203f, 0.954914f}, {0.034121f, 0.577679f, -0.815551f}, true, true },
  { 12, {5.309190f, 1.098850f}, {0.500564f, 0.454621f, -0.736719f}, true, true },
  { 13, {5.445891f, 1.439448f}, {0.663708f, 0.130971f, -0.736436f}, true, true },
  { 14, {5.761053f, 1.667743f}, {0.862687f, -0.096794f, -0.496388f}, true, true },
  { 15, {5.707815f, 1.884728f}, {0.797987f, -0.308800f, -0.517551f}, true, true },
  { 16, {5.887464f, 2.223052f}, {0.733300f, -0.606980f, -0.306343f}, true, true },
  { 17, {5.443985f, 2.425432f}, {0.438575f, -0.754332f, -0.488503f}, true, true },
  { 18, {4.906968f, 2.525408f}, {0.111744f, -0.816090f, -0.567020f}, true, true },
  { 19, {4.351120f, 2.651390f}, {-0.166411f, -0.882237f, -0.440414f}, true, true },
  { 20, {3.782044f, 2.522192f}, {-0.465498f, -0.814227f, -0.346910f}, true, true },
  { 21, {3.977813f, 2.124145f}, {-0.570247f, -0.525539f, -0.631369f}, true, true },
  { 22, {3.654780f, 2.261683f}, {-0.671405f, -0.637221f, -0.378371f}, true, true },
  { 23, {3.571942f, 2.038879f}, {-0.811063f, -0.451176f, -0.372313f}, true, true },
  { 24, {3.679546f, 1.770901f}, {-0.841623f, -0.198772f, -0.502155f}, true, true },
  { 25, {3.434191f, 1.421995f}, {-0.946917f, 0.148253f, -0.285254f}, true, true },
  { 26, {3.610324f, 1.159366f}, {-0.817693f, 0.399921f, -0.414056f}, true, true },
  { 27, {4.071808f, 0.875846f}, {-0.459055f, 0.640347f, -0.615811f}, true, true },
  { 28, {4.142575f, 0.556120f}, {-0.284786f, 0.849310f, -0.444488f}, true, true },
  { 29, {4.797850f, 0.629970f}, {0.050286f, 0.808045f, -0.586971f}, true, true },
  { 30, {5.289500f, 0.472121f}, {0.248129f, 0.890606f, -0.381122f}, true, true },
  { 31, {5.958092f, 0.548173f}, {0.493832f, 0.853478f, -0.166447f}, true, true },
  { 32, {6.053112f, 0.959190f}, {0.797153f, 0.574184f, -0.186710f}, true, true },
  { 33, {5.737214f, 1.229618f}, {0.805364f, 0.334598f, -0.489320f}, true, true },
  { 34, {6.150683f, 1.378696f}, {0.973001f, 0.190921f, -0.129685f}, true, true },
  { 35, {6.067979f, 1.806182f}, {0.949993f, -0.233218f, -0.207660f}, true, true },
  { 36, {6.097087f, 2.181641f}, {0.805020f, -0.573559f, -0.151566f}, true, true },
  { 37, {6.100440f, 2.514549f}, {0.576983f, -0.809766f, -0.106630f}, true, true },
  { 38, {5.489286f, 2.693025f}, {0.304037f, -0.901069f, -0.309250f}, true, true },
  { 39, {5.075155f, 2.856025f}, {0.099965f, -0.959502f, -0.263368f}, true, true },
  { 40, {3.976761f, 2.909430f}, {-0.154398f, -0.973171f, -0.170585f}, true, true },
  { 41, {3.262183f, 2.808998f}, {-0.324125f, -0.945198f, -0.039277f}, true, true },
  { 42, {3.309308f, 2.355606f}, {-0.697596f, -0.706690f, -0.118107f}, true, true },
  { 43, {3.309287f, 1.909537f}, {-0.929943f, -0.332300f, -0.157424f}, true, true },
  { 44, {3.155199f, 1.640890f}, {-0.997452f, -0.070036f, -0.013573f}, true, true },
  { 45, {3.116120f, 1.346101f}, {-0.974546f, 0.222810f, 0.024830f}, true, true },
  { 46, {3.171874f, 1.092934f}, {-0.887573f, 0.459882f, -0.026885f}, true, true },
  { 47, {3.534754f, 0.928881f}, {-0.739840f, 0.598731f, -0.306853f}, true, true },
  { 48, {3.186461f, 0.716240f}, {-0.655893f, 0.754280f, -0.029449f}, true, true },
  { 49, {3.574593f, 0.462708f}, {-0.405178f, 0.894847f, -0.187296f}, true, true },
  { 50, {3.422837f, 0.209074f}, {-0.199400f, 0.978223f, -0.057607f}, true, true },
  { 51, {5.543229f, 0.211150f}, {0.154777f, 0.977791f, -0.141313f}, true, true },
  { 52, {0.050250f, 0.463718f}, {0.446712f, 0.894396f, 0.022466f}, true, true },
  { 53, {2.276281f, 0.265952f}, {-0.170418f, 0.964843f, 0.200090f}, true, true },
  { 54, {2.574532f, 0.573546f}, {-0.457686f, 0.839982f, 0.291468f}, true, true },
  { 55, {2.760685f, 1.022286f}, {-0.792144f, 0.521417f, 0.317226f}, true, true },
  { 56, {2.665084f, 1.276888f}, {-0.850497f, 0.289696f, 0.439011f}, true, true },
  { 57, {2.746205f, 1.690155f}, {-0.916282f, -0.119076f, 0.382425f}, true, true },
  { 58, {3.018958f, 2.074613f}, {-0.869169f, -0.482771f, 0.107128f}, true, true },
  { 59, {2.766822f, 2.234349f}, {-0.733129f, -0.615920f, 0.288385f}, true, true },
  { 60, {2.355115f, 2.503171f}, {-0.420930f, -0.803037f, 0.421840f}, true, true },
  { 61, {2.098711f, 2.721890f}, {-0.205266f, -0.913210f, 0.352013f}, true, true },
  { 62, {1.150030f, 2.847986f}, {0.118211f, -0.957206f, 0.264164f}, true, true },
  { 63, {0.411776f, 2.466204f}, {0.572941f, -0.780464f, 0.250229f}, true, true },
  { 64, {0.347653f, 2.117805f}, {0.802989f, -0.520134f, 0.290980f}, true, true },
  { 65, {0.650241f, 2.031285f}, {0.713029f, -0.444385f, 0.542320f}, true, true },
  { 66, {0.468167f, 1.785865f}, {0.871838f, -0.213414f, 0.440855f}, true, true },
  { 67, {0.692198f, 1.568242f}, {0.769842f, 0.002554f, 0.638229f}, true, true },
  { 68, {0.752468f, 1.298027f}, {0.703015f, 0.269399f, 0.658175f}, true, true },
  { 69, {0.354930f, 1.271551f}, {0.896000f, 0.294800f, 0.332080f}, true, true },
  { 70, {0.197350f, 0.873815f}, {0.751900f, 0.641906f, 0.150344f}, true, true },
  { 71, {0.692024f, 1.055446f}, {0.669954f, 0.492840f, 0.555221f}, true, true },
  { 72, {0.632532f, 0.755421f}, {0.552955f, 0.727983f, 0.405316f}, true, true },
  { 73, {0.919540f, 0.408816f}, {0.240973f, 0.917592f, 0.316159f}, true, true },
  { 74, {1.665801f, 0.444851f}, {-0.040821f, 0.902675f, 0.428383f}, true, true },
  { 75, {1.895625f, 0.682324f}, {-0.201253f, 0.776109f, 0.597622f}, true, true },
  { 76, {2.153725f, 1.004066f}, {-0.464411f, 0.536876f, 0.704334f}, true, true },
  { 77, {2.492594f, 1.149908f}, {-0.727159f, 0.408571f, 0.551642f}, true, true },
  { 78, {2.567090f, 1.653385f}, {-0.836601f, -0.082495f, 0.541566f}, true, true },
  { 79, {2.579677f, 1.969323f}, {-0.779920f, -0.388061f, 0.491054f}, true, true },
  { 80, {2.444908f, 2.284597f}, {-0.579739f, -0.654711f, 0.485031f}, true, true },
  { 81, {2.038705f, 2.472685f}, {-0.279691f, -0.784499f, 0.553474f}, true, true },
  { 82, {1.703808f, 2.603873f}, {-0.067925f, -0.858879f, 0.507655f}, true, true },
  { 83, {1.313270f, 2.564777f}, {0.138897f, -0.838203f, 0.527374f}, true, true },
  { 84, {0.860895f, 2.452227f}, {0.414550f, -0.771650f, 0.482395f}, true, true },
  { 85, {0.997538f, 2.000042f}, {0.493169f, -0.416185f, 0.763921f}, true, true },
  { 86, {1.086850f, 1.633909f}, {0.464349f, -0.063071f, 0.883404f}, true, true },
  { 87, {1.039999f, 1.447087f}, {0.502352f, 0.123394f, 0.855813f}, true, true },
  { 88, {0.980733f, 1.202456f}, {0.519093f, 0.360068f, 0.775173f}, true, true },
  { 89, {1.091011f, 0.891214f}, {0.359040f, 0.628468f, 0.690013f}, true, true },
  { 90, {1.280420f, 1.147487f}, {0.261042f, 0.410780f, 0.873566f}, true, true },
  { 91, {1.735715f, 1.221816f}, {-0.154276f, 0.341940f, 0.926971f}, true, true },
  { 92, {1.973886f, 1.525576f}, {-0.391861f, 0.045205f, 0.918913f}, true, true },
  { 93, {1.749387f, 1.809353f}, {-0.172612f, -0.236300f, 0.956226f}, true, true },
  { 94, {2.093461f, 2.020744f}, {-0.449506f, -0.434918f, 0.780250f}, true, true },
  { 95, {1.636930f, 2.072531f}, {-0.057940f, -0.480947f, 0.874833f}, true, true },
  { 96, {1.236707f, 2.111446f}, {0.281141f, -0.514693f, 0.809970f}, true, true },
  { 97, {1.443513f, 1.642164f}, {0.126617f, -0.071307f, 0.989385f}, true, true },
  { 98, {1.571219f, 1.369100f}, {-0.000414f, 0.200332f, 0.979728f}, true, true },
};

// Convert from accelerometer reading to cartesian unit vector
CartesianCoord accelToCartesian(float ax, float ay, float az) {
  CartesianCoord cart;
  // Normalize acceleration vector
  float magnitude = sqrt(ax*ax + ay*ay + az*az);
  if (magnitude > 0) {
    cart.x = ax / magnitude;
    cart.y = ay / magnitude;
    cart.z = az / magnitude;
  } else {
    cart.x = 0;
    cart.y = 1; // Default to pointing up if no acceleration
    cart.z = 0;
  }
  return cart;
}

// Convert from cartesian to polar coordinates
PolarCoord cartesianToPolar(CartesianCoord cart) {
  PolarCoord polar;
  polar.phi = acos(constrain(cart.y, -1.0, 1.0));
  polar.theta = atan2(cart.z, cart.x);
  if (polar.theta < 0) polar.theta += TWO_PI;
  return polar;
}

// Convert from polar to cartesian coordinates
CartesianCoord polarToCartesian(PolarCoord polar) {
  CartesianCoord cart;
  cart.x = sin(polar.phi) * cos(polar.theta);
  cart.y = cos(polar.phi);  // y is up/down axis
  cart.z = sin(polar.phi) * sin(polar.theta);
  return cart;
}

// Find LED closest to a given direction
int findClosestLED(float x, float y, float z) {
  int closestLED = 0;
  float bestMatch = -1.0;

  for (int i = 0; i < SPHERE_NUM_LEDS; i++) {
    if (sphereLedPositions[i].isCalibrated) {
      float dotProduct = sphereLedPositions[i].cartesianPos.x * x +
                         sphereLedPositions[i].cartesianPos.y * y +
                         sphereLedPositions[i].cartesianPos.z * z;

      if (dotProduct > bestMatch) {
        bestMatch = dotProduct;
        closestLED = i;
      }
    }
  }
  return closestLED;
}
// Update cardinal directions after calibration
void updateCardinalDirections() {
  sphereCardinalLEDs[0] = findClosestLED( 1.0,  0.0,  0.0);  // +X
  sphereCardinalLEDs[1] = findClosestLED(-1.0,  0.0,  0.0);  // -X
  sphereCardinalLEDs[2] = findClosestLED( 0.0,  1.0,  0.0);  // +Y (up)
  sphereCardinalLEDs[3] = findClosestLED( 0.0, -1.0,  0.0);  // -Y (down)
  sphereCardinalLEDs[4] = findClosestLED( 0.0,  0.0,  1.0);  // +Z
  sphereCardinalLEDs[5] = findClosestLED( 0.0,  0.0, -1.0);  // -Z
}

// Initialize all LED positions as uncalibrated if not already set
// Set forceClear to true to reset all calibration data
void initializeLEDMap(bool forceClear = false) {
  bool hasCalibrated = false;
  int numCalibrated = 0;

  // Check existing calibration
  for (int i = 0; i < SPHERE_NUM_LEDS; i++) {
    // Make sure index is set correctly
    sphereLedPositions[i].ledIndex = i;

    // Check if this position looks calibrated and we're not forcing a clear
    if (!forceClear && (sphereLedPositions[i].isCalibrated || sphereLedPositions[i].isSet) ) {
      // Likely has valid data, mark as calibrated
      numCalibrated++;
      hasCalibrated = true;
    } else {
      // Initialize with default spherical mapping (which will be overwritten during calibration)
      sphereLedPositions[i].isCalibrated = false;
      sphereLedPositions[i].isSet = false;
      sphereLedPositions[i].polarPos = {0, 0};
      sphereLedPositions[i].cartesianPos = {0, 0, 1};
    }
  }

  // Reset cardinal directions if no calibration exists or we're forcing a clear
  if (!hasCalibrated || forceClear) {
    for (int i = 0; i < 6; i++) {
      sphereCardinalLEDs[i] = -1;
    }

    if (forceClear) {
      Serial.println("All calibration data cleared");
    }
  } else {
    // Update cardinal directions if we have calibration data
    updateCardinalDirections();
    Serial.println("Found " + String(numCalibrated) + " calibrated LEDs");
  }
}

// Save the position for an LED based on current accelerometer orientation
void saveLEDPosition(int ledIndex, float ax, float ay, float az) {
  if (ledIndex >= 0 && ledIndex < SPHERE_NUM_LEDS) {
    // Convert accelerometer reading to unit vector
    CartesianCoord position = accelToCartesian(ax, ay, az);
    PolarCoord polarPos = cartesianToPolar(position);

    // Save the position
    sphereLedPositions[ledIndex].cartesianPos = position;
    sphereLedPositions[ledIndex].polarPos = polarPos;
    sphereLedPositions[ledIndex].isCalibrated = true;
    sphereLedPositions[ledIndex].isSet = true;
  }
}

// Interpolate position between two reference positions
PolarCoord interpolatePolarPosition(PolarCoord start, PolarCoord end, float fraction) {
  // Adjust theta for shortest path
  if (fabs(end.theta - start.theta) > PI) {
    if (end.theta > start.theta) {
      start.theta += TWO_PI;
    } else {
      end.theta += TWO_PI;
    }
  }

  // Linear interpolation of polar coordinates
  PolarCoord interpolated;
  interpolated.theta = start.theta + fraction * (end.theta - start.theta);
  interpolated.phi = start.phi + fraction * (end.phi - start.phi);

  // Normalize theta to [0, 2π)
  while (interpolated.theta >= TWO_PI) interpolated.theta -= TWO_PI;
  while (interpolated.theta < 0) interpolated.theta += TWO_PI;

  return interpolated;
}

// Interpolate positions for LEDs between two calibrated LEDs
bool interpolateBetweenLEDs(int startLedIndex, int endLedIndex) {
  // Handle wrap-around case
  bool isWraparound = false;
  if (endLedIndex >= SPHERE_NUM_LEDS) {
    endLedIndex %= SPHERE_NUM_LEDS;
    isWraparound = true;
  }

  // Validate indices
  if (startLedIndex < 0 || startLedIndex >= SPHERE_NUM_LEDS ||
      endLedIndex < 0 || endLedIndex >= SPHERE_NUM_LEDS ||
      (startLedIndex >= endLedIndex && !isWraparound)) {
    return false;
  }

  // Check if start and end LEDs are calibrated
  if (!sphereLedPositions[startLedIndex].isCalibrated ||
      !sphereLedPositions[endLedIndex].isCalibrated) {
    return false;
  }

  // Get reference coordinates
  PolarCoord startPolar = sphereLedPositions[startLedIndex].polarPos;
  PolarCoord endPolar = sphereLedPositions[endLedIndex].polarPos;

  // Calculate number of segments and LEDs to interpolate
  int segments;
  if (isWraparound) {
    segments = SPHERE_NUM_LEDS - startLedIndex + endLedIndex;
  } else {
    segments = endLedIndex - startLedIndex;
  }

  if (segments <= 1) return true; // Nothing to interpolate

  // Interpolate each LED between start and end
  for (int i = 1; i < segments; i++) {
    int ledIndex = (startLedIndex + i) % SPHERE_NUM_LEDS;
    float fraction = (float)i / segments;

    // Interpolate polar coordinates
    PolarCoord interpolatedPolar = interpolatePolarPosition(startPolar, endPolar, fraction);

    // Convert to cartesian
    CartesianCoord interpolatedCartesian = polarToCartesian(interpolatedPolar);

    // Update LED position
    sphereLedPositions[ledIndex].polarPos = interpolatedPolar;
    sphereLedPositions[ledIndex].cartesianPos = interpolatedCartesian;
    sphereLedPositions[ledIndex].isCalibrated = false; // Not directly calibrated
    sphereLedPositions[ledIndex].isSet = true;         // But position is set
  }

  return true;
}

// Generate JSON representation of the LED mapping
String generateMappingJSON() {
  String json = "{";
  json += "\"numLeds\": " + String(SPHERE_NUM_LEDS) + ",";
  json += "\"leds\": [";

  for (int i = 0; i < SPHERE_NUM_LEDS; i++) {
    json += "{";
    json += "\"index\": " + String(i) + ",";
    json += "\"isCalibrated\": " + String(sphereLedPositions[i].isCalibrated ? "true" : "false") + ",";
    json += "\"isSet\": " + String(sphereLedPositions[i].isSet ? "true" : "false") + ",";
    json += "\"polar\": {";
    json += "\"theta\": " + String(sphereLedPositions[i].polarPos.theta, 6) + ",";
    json += "\"phi\": " + String(sphereLedPositions[i].polarPos.phi, 6) + "";
    json += "},";
    json += "\"cartesian\": {";
    json += "\"x\": " + String(sphereLedPositions[i].cartesianPos.x, 6) + ",";
    json += "\"y\": " + String(sphereLedPositions[i].cartesianPos.y, 6) + ",";
    json += "\"z\": " + String(sphereLedPositions[i].cartesianPos.z, 6) + "";
    json += "}";
    json += "}";
    json += (i < SPHERE_NUM_LEDS - 1 ? "," : "");
    json += "";
  }

  json += "],";
  json += "\"cardinal\": [";
  for (int i = 0; i < 6; i++) {
    json += "" + String(sphereCardinalLEDs[i]) + (i < 5 ? "," : "") + "";
  }
  json += "]";
  json += "}";

  return json;
}

// Get a summary of LED calibration status
String getLEDSummary() {
  int calibrated = 0;
  int interpolated = 0;
  int unset = 0;

  for (int i = 0; i < SPHERE_NUM_LEDS; i++) {
    if (sphereLedPositions[i].isCalibrated) {
      calibrated++;
    } else if (sphereLedPositions[i].isSet) {
      interpolated++;
    } else {
      unset++;
    }
  }

  return "LED Status: " + String(calibrated) + " calibrated, " +
         String(interpolated) + " interpolated, " +
         String(unset) + " unset. Total: " + String(SPHERE_NUM_LEDS);
}

#endif // PIXEL_BALL_LED_MAP_H