/* Focus on diagram (e.g. by clicking on the matrix)
    w                p
  a   s          o
    d
*/



#include "FastLED.h"


// Matrix size
#define HEIGHT 16
#define WIDTH 16

// Define pins
#define DATA_PIN 3
#define BUTTON_0 9
#define BUTTON_1 10
#define BUTTON_2 11
#define BUTTON_3 12
#define BUTTON_4 5
#define BUTTON_5 6

// LED brightness
#define BRIGHTNESS 255

// Define the array of leds
#define NUM_LEDS HEIGHT * WIDTH
CRGB leds[NUM_LEDS];

byte pressed = 0;
bool button0Pressed = false;
bool button1Pressed = false;
bool button2Pressed = false;
bool button3Pressed = false;
bool button4Pressed = false;
bool button5Pressed = false;
void setup() {
  pinMode(BUTTON_0, INPUT_PULLUP);
  pinMode(BUTTON_1, INPUT_PULLUP);
  pinMode(BUTTON_2, INPUT_PULLUP);
  pinMode(BUTTON_3, INPUT_PULLUP);
  pinMode(BUTTON_4, INPUT_PULLUP);
  pinMode(BUTTON_5, INPUT_PULLUP);
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
  FastLED.setBrightness(BRIGHTNESS);
}

void loop() {
  // Read the button inputs
  button0Pressed = digitalRead(BUTTON_0) == LOW;
  button1Pressed = digitalRead(BUTTON_1) == LOW;
  button2Pressed = digitalRead(BUTTON_2) == LOW;
  button3Pressed = digitalRead(BUTTON_3) == LOW;
  button4Pressed = digitalRead(BUTTON_4) == LOW;
  button5Pressed = digitalRead(BUTTON_5) == LOW;
  if (button0Pressed) {
    pressed = 1;
  } else if (button1Pressed) {
    pressed = 2;
  } else if (button2Pressed) {
    pressed = 3;
  } else if (button3Pressed) {
    pressed = 4;
  } else if (button4Pressed) {
    pressed = 5;
  }  else if (button5Pressed) {
    pressed = 6;
  } else pressed = 0;
  draw();
  FastLED.show();
}

#define M_W 24
#define M_H 24
#define M_L 24
#define RAY_L M_W +M_H

int lPosY = 32;
int lPosX = 32;
int lPosZ = 16;
int alpha;
int beta;
float angleRa;
float angleRb;
byte rot = 2;
byte PoV = WIDTH / 2;
byte PooV = HEIGHT / 2;
bool mape[M_W][M_H][M_L];
bool loadingFlag = true;
void draw() {
  if (loadingFlag) {
    loadingFlag = false;
  }
  FastLED.clear();
  if (pressed == 1) {
    lPosX += sin(angleRa);
    lPosY += cos(angleRa);
    lPosZ += sin(angleRb);
  }
  else if (pressed == 2) {
    lPosX -= sin(angleRa);
    lPosY -= cos(angleRa);
    lPosZ -= cos(angleRb);
  }
  if (pressed == 3) {
    alpha += rot;
    angleRa = radians(alpha);
  }
  else if (pressed == 4) {
    alpha -= rot;
    angleRa = radians(alpha);
  }
  if (pressed == 5) {
    beta += rot;
    angleRb = radians(beta);
  }
  else if (pressed == 6) {
    beta -= rot;
    angleRb = radians(beta);
  }
  if (lPosX < 0) lPosX = (M_W-1)*4;
  if (lPosX > (M_W-1)*4) lPosX = 0;
  if (lPosY < 0) lPosY = (M_H-1)*4;
  if (lPosY > (M_H-1)*4) lPosZ = 0;
  if (lPosZ < 0) lPosZ = (M_L-1)*4;
  if (lPosZ > (M_L-1)*4) lPosZ = 0;
  byte PosX = lPosX/4;
  byte PosY = lPosY/4;
  byte PosZ = lPosZ/4;
  byte dist = 0;
  for (byte pov = 0; pov < WIDTH; pov++) {
    int angle = radians(alpha + pov - PoV - 1);
    float a = sin(radians(angle));
    float b = cos(radians(angle));
    for (byte fov = 0; fov < HEIGHT; fov++) {
      float c = sin(radians(beta + fov - PooV - 1));
      for (byte i = 0; i < (RAY_L); i++) {
        byte ENDPosX = PosX + (i * a);
        byte ENDPosY = PosY + (i * b);
        byte ENDPosZ = PosZ + (i * c);
        if (mape[ENDPosX][ENDPosY][ENDPosZ] || ENDPosX < 0 || ENDPosX > M_W || ENDPosY < 0 || ENDPosY > M_H || ENDPosZ < 0 || ENDPosZ > M_L) {
          dist = map(i, 0,RAY_L, 255, 0);
          break;
        }
      } leds[XY(pov, fov)] = CHSV(50, 100, dist);
    }
  }
  delay(16);
}

uint16_t XY (uint8_t x, uint8_t y) {
  return (y * WIDTH + x);
}