//Metaballs
//16x16 rgb led matrix demo
//Yaroslaw Turbin 20.07.2020
//https://vk.com/ldirko
//https://www.reddit.com/user/ldirko/
#include "FastLED.h"

// Matrix size
#define NUM_ROWS 16
#define NUM_COLS 16
#define NUM_LEDS (NUM_ROWS * NUM_COLS)

enum XY_cfg {LINEAR = 0, SERPENTINE = 1, COLUMNMAJOR = 0,
             ROWMAJOR = 2, FLIPMAJOR = 4, FLIPMINOR = 8};
#define XY_MATRIX         (SERPENTINE | ROWMAJOR /*| FLIPMINOR*/)
#define kMatrixWidth      NUM_ROWS
#define kMatrixHeight     NUM_COLS


// LEDs pin
#define DATA_PIN 2

// LED brightness
#define BRIGHTNESS 64


// Define the array of leds
CRGB leds[NUM_LEDS + 1];

DEFINE_GRADIENT_PALETTE( lava) {
  0,      255,  30,  0,
  32,     0,    0,   0,
  128,    0,    0,   0,
  150,    180,  180, 0,
  255,    255,  50,  0
};

CRGBPalette16 myPal = lava;


void setup() {
  Serial.begin(250000);
  FastLED.addLeds<WS2812B, DATA_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(BRIGHTNESS);
}


void loop() {

  uint8_t bx1 = beatsin8(15, 0, NUM_COLS - 1, 0, 0);
  uint8_t by1 = beatsin8(18, 0, NUM_ROWS - 1, 0, 0);
  uint8_t bx2 = beatsin8(28, 0, NUM_COLS - 1, 0, 32);
  uint8_t by2 = beatsin8(23, 0, NUM_ROWS - 1, 0, 32);
  uint8_t bx3 = beatsin8(30, 0, NUM_COLS - 1, 0, 64);
  uint8_t by3 = beatsin8(24, 0, NUM_ROWS - 1, 0, 64);
  uint8_t bx4 = beatsin8(17, 0, NUM_COLS - 1, 0, 128);
  uint8_t by4 = beatsin8(25, 0, NUM_ROWS - 1, 0, 128);
  uint8_t bx5 = beatsin8(19, 0, NUM_COLS - 1, 0, 170);
  uint8_t by5 = beatsin8(21, 0, NUM_ROWS - 1, 0, 170);

  for (uint8_t i = 0; i < NUM_COLS; i++)    {
    for (uint8_t j = 0; j < NUM_ROWS; j++) {

      float sum =  2 * dist(i, j, bx1, by1);
      sum += 2 * dist(i, j, bx2, by2);
      sum += 2 * dist(i, j, bx3, by3);
      sum += 2 * dist(i, j, bx4, by4);
      sum += 2 * dist(i, j, bx5, by5);

      byte col =  constrain(120 * sum, 128, 255);
      leds[XY (i, j)] =  ColorFromPalette(myPal, col, BRIGHTNESS);

    }
  }

  FastLED.show();
  static int frame = 0;
  if (frame++ % 32 == 0)
    Serial.println(FastLED.getFPS());
} //loop



uint16_t XY(uint8_t x, uint8_t y) {
  uint8_t major, minor, sz_major, sz_minor;
  if (x >= kMatrixWidth || y >= kMatrixHeight)
    return NUM_LEDS;
  if (XY_MATRIX & ROWMAJOR)
    major = x, minor = y, sz_major = kMatrixWidth,  sz_minor = kMatrixHeight;
  else
    major = y, minor = x, sz_major = kMatrixHeight, sz_minor = kMatrixWidth;
  if ((XY_MATRIX & FLIPMAJOR) ^ (minor & 1 && (XY_MATRIX & SERPENTINE)))
    major = sz_major - 1 - major;
  if (XY_MATRIX & FLIPMINOR)
    minor = sz_minor - 1 - minor;
  return (uint16_t) minor * sz_major + major;
}


float dist (uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2)  {
//  float dist = 1 / sqrt(sq(y2 - y1) + sq(x2 - x1));
  float dist = Q_rsqrt(sq(y2 - y1) + sq(x2 - x1));
  return dist;
}


float Q_rsqrt( float number )
{
  long i;
  float x2, y;
  const float threehalfs = 1.5F;

  x2 = number * 0.5F;
  y  = number;
  i  = * ( long * ) &y;                       // evil floating point bit level hacking
  i  = 0x5f3759df - ( i >> 1 );               // what the fuck?
  y  = * ( float * ) &i;
  y  = y * ( threehalfs - ( x2 * y * y ) );   // 1st iteration
//  y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed

  return y;
}