// Define OutputArray
#define ARRAY_X_SIZE 3
#define ARRAY_Y_SIZE 4
#define DEBUG true

// Outputs [x][y]
const int Array_Pins[ARRAY_X_SIZE][ARRAY_Y_SIZE] = {
  {5, 4, 3, 2},
  {9, 8, 7, 6},
  {13, 12, 11, 10}
};
int Array_Outputs[ARRAY_X_SIZE][ARRAY_Y_SIZE] = {
  {0, 0, 0, 0},
  {0, 0, 0, 0},
  {0, 0, 0, 0}
};
int Point_Spread = 180;
int Point_Power = 255;
// Circle plot variables
float X_In = 0.0;
float Y_In = 0.0;
float theta = 0.0;       // angle in radians
float radius = 2.5;
float stepSize = 0.05;   // radians

void setup() {
  #if DEBUG
    Serial.begin(9600);
  #endif
  for (int x = 0; x < ARRAY_X_SIZE; x++) {
    for (int y = 0; y < ARRAY_Y_SIZE; y++) {
      pinMode(Array_Pins[x][y], OUTPUT);
    }
  }
}

void loop() {
  // update moving point
  plotCircle(radius, stepSize);

  // compute outputs
  #if DEBUG
    ////Serial.println("Begin Array: ");
  #endif
  for (int x = 0; x < ARRAY_X_SIZE; x++) {
    for (int y = 0; y < ARRAY_Y_SIZE; y++) {
      float dx = X_In - (float)x;
      float dy = Y_In - (float)y;
      float pout = sqrt((dx * dx) + (dy * dy));
      Array_Outputs[x][y] = constrain(round(Point_Power - pout * Point_Spread), 0, Point_Power);
      #if DEBUG
        ////Serial.print("(" + String(x) + "," + String(y) + ")\t");

        ////Serial.print(String(pout)+"\t");
      #endif
    }
    #if DEBUG
      ////Serial.print("\n");
    #endif
  }
  #if DEBUG
    ////Serial.println("End Array");
  #endif

  // write outputs
  for (int x = 0; x < ARRAY_X_SIZE; x++) {
    for (int y = 0; y < ARRAY_Y_SIZE; y++) {
      analogWrite(Array_Pins[x][y], Array_Outputs[x][y]);
    }
  }
}
Serial.println(Array_Outputs[3][1]);

// Function to move X_In, Y_In around a circle
void plotCircle(float r, float step) {
  theta += step;   // advance the angle
  if (theta > TWO_PI) theta -= TWO_PI; // wrap around

  // Center circle around middle of array
  float cx = (ARRAY_X_SIZE - 1) / 2.0;
  float cy = (ARRAY_Y_SIZE - 1) / 2.0;

  X_In = cx + r * cos(theta);
  Y_In = cy + r * sin(theta);

  #if DEBUG
    ////Serial.print("Circle X: ");
     ////Serial.print(X_In);
    ////Serial.print(" Y: "); Serial.println(Y_In);
  #endif
}