#include <FastLED.h>
#include <Servo.h>

#define LED_PIN  2
#define MIN_ANGLE 0
#define MAX_ANGLE 180
#define WIDTH 3
#define HEIGHT 3
#define NUM_SERVOS 24
Servo servos[NUM_SERVOS];

#define INTERVAL 50
unsigned long previousMillis = 0;

// Param for different pixel layouts
const bool    matrixSerpentineLayout = false;
const bool    matrixVertical = false;

uint16_t XY( uint8_t x, uint8_t y)
{
  uint16_t i;
  
  if( matrixSerpentineLayout == false) {
    if (matrixVertical == false) {
      i = (y * WIDTH) + x;
    } else {
      i = HEIGHT * (WIDTH - (x+1))+y;
    }
  }

  if( matrixSerpentineLayout == true) {
    if (matrixVertical == false) {
      if( y & 0x01) {
        // Odd rows run backwards
        uint8_t reverseX = (WIDTH - 1) - x;
        i = (y * WIDTH) + reverseX;
      } else {
        // Even rows run forwards
        i = (y * WIDTH) + x;
      }
    } else { // vertical positioning
      if ( x & 0x01) {
        i = HEIGHT * (WIDTH - (x+1))+y;
      } else {
        i = HEIGHT * (WIDTH - x) - (y+1);
      }
    }
  }
  
  return i;
}

#define NUM_LEDS (WIDTH * HEIGHT)
CRGB leds_plus_safety_pixel[ NUM_LEDS + 1];
CRGB* const leds( leds_plus_safety_pixel + 1);

void setServoAngle(int coord, int angle) {
  servos[coord].write(angle);
}

void setup() {
  for( int i = 0; i < NUM_SERVOS; i++){
    servos[i].attach(i + 22);      // PWM pins from 22
  }
}

void loop()
{
  int i = 0;
  while (i < 360) {
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= INTERVAL) {
      for (uint8_t y = 0; y < HEIGHT; y++){
        for (uint8_t x = 0; x < WIDTH; x++){
        float angle = radians(i + x * 180 / (WIDTH - 1));
        float phaseY = y * 50 / (HEIGHT - 1);
        float brightness = (75) + (25) * sin(angle) - (phaseY);
        int coord = XY(x, y);
        int servoAngle = map(brightness * 10, 0, 1000, MIN_ANGLE, MAX_ANGLE);
        setServoAngle(coord, servoAngle);
        }
      }
      previousMillis = currentMillis;
      i++;
    }
  }
}