#include <FastLED.h>
#include "figures.h"

#define WIDTH 16                            //Ширина матрицы
#define HEIGHT 16                           //Высота матрицы
#define NUM_LEDS (WIDTH * HEIGHT)           //Общее количество светодиодов
#define LED_PIN 13                          //Пин подключения матрицы
#define BRT_PIN A1                          //Пин подключения потенциометра
#define VERT_PIN A3                         //Пин подключения оси Y джойстика
#define HORZ_PIN A2                         //Пин подключения оси X джойстика
#define BUTTON_PIN 3                        //Пин подключения кнопки джойстика

float turn_speed = 2;                       //Скорость поворота фигуры

CRGB leds[NUM_LEDS];                        //Массив для светодиодов

char FIGURE = 1;                            //1 - Тэтраэдр, 2 - Куб, 3 - "Лестница"
char MODE = 1;                              //Режим
unsigned long startTime = 0;                //Время начала нажатия

unsigned char old_brightness = 0;           //Яркость на предыдущей итерации

Point camera = {60, 0, 0};                  //Камера
Plane cameraPlane({50, WIDTH / 2, HEIGHT / 2}, {50, -WIDTH / 2, -HEIGHT / 2}, {50, -WIDTH / 2, HEIGHT / 2});

char number_points;
char number_triangles;
class Point points[12];
char indexBuffer[20][3];
float distTriangle[20];

void setup() {
  FastLED.addLeds<WS2811, LED_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(analogRead(BRT_PIN) * 0.25);
  FastLED.clear();
  FastLED.show();

  pinMode(BUTTON_PIN, INPUT_PULLUP);

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);

  turn_speed = (turn_speed / 256.0) * (3.1415926 / 180.0);

  number_points = pgm_read_byte(&number_points_tetrahedron);
  number_triangles = pgm_read_byte(&number_triangles_tetrahedron);
  for (char i = 0; i < number_points; i++) {
    memcpy_P(&points[i], &points_tetrahedron[i], sizeof(Point));
  }
  for (char i = 0; i < number_triangles; i++) {
    for (char j = 0; j < 3; j++) {
      indexBuffer[i][j] = pgm_read_byte(&indexBuffer_tetrahedron[i][j]);
    }
  }
}

void shapeRotation() {                      //Вращение фигуры вокруг начала координат
  float angle_horz = (analogRead(HORZ_PIN) - 512.0) * turn_speed;
  float angle_vert = -(analogRead(VERT_PIN) - 512.0) * turn_speed;

  for (char i = 0; i < number_points; i++) {
    float angle_horz_point = atan2(points[i].y, points[i].x);         //Угол точки в плоскости XY
    float radius_horz = sqrt(points[i].x*points[i].x + points[i].y*points[i].y);//Расстояние от начала координат до точки
    points[i].x = radius_horz*cos(angle_horz + angle_horz_point);     //Новый x
    points[i].y = radius_horz*sin(angle_horz + angle_horz_point);     //Новый y

    float angle_vert_point = atan2(points[i].z, points[i].x);         //Угол точки в плоскости XZ
    float radius_vert = sqrt(points[i].x*points[i].x + points[i].z*points[i].z);//Расстояние от начала координат до точки
    points[i].x = radius_vert*cos(angle_vert + angle_vert_point);     //Новый x
    points[i].z = radius_vert*sin(angle_vert + angle_vert_point);     //Новый z
  }
}

Point projectionPoint(Point point) {        //Найти проекцию точки на матрицу
  Point vectorAB = {point.x - camera.x, point.y - camera.y, point.z - camera.z};//Вектор от камеры к точке
  float scalarProduct1 = vectorAB.x * cameraPlane.normal.x + vectorAB.y * cameraPlane.normal.y + vectorAB.z * cameraPlane.normal.z;
  if (scalarProduct1 == 0) {
    Point res = {0, 0, 0};
    return res;
  }
  Point vectorPointPlane {cameraPlane.point.x - camera.x, cameraPlane.point.y - camera.y, cameraPlane.point.z - camera.z};
  float scalarProduct2 = vectorPointPlane.x * cameraPlane.normal.x + vectorPointPlane.y * cameraPlane.normal.y + vectorPointPlane.z * cameraPlane.normal.z;
  float t = scalarProduct2 / scalarProduct1;
  if (t >= 0 && t <= 1) {                                             //Если плоскость между камерой и точкой
    Point res = {camera.x + t * vectorAB.x, camera.y + t * vectorAB.y, camera.z + t * vectorAB.z};
    return res;
  }
  else {
    Point res = {0, 0, 0};
    return res;
  }
}

void drawSegment(char x1, char y1, char x2, char y2) {   //Нарисовать отрезок на плоскости YZ
  char dx = abs(x2 - x1);
  char sx = x1 < x2 ? 1 : -1;
  char dy = -abs(y2 - y1);
  char sy = y1 < y2 ? 1 : -1;
  char err = dx + dy;

  while (true) {
    if (x1 < HEIGHT && y1 < WIDTH)
      leds[XY(x1, y1)] = CRGB::Green;

    if (x1 == x2 && y1 == y2) 
      break;

    char e2 = 2 * err;
    if (e2 >= dy) {
      err += dy;
      x1 += sx;
    }
    if (e2 <= dx) {
      err += dx;
      y1 += sy;
    }
  }
}

void drawHorLine(char x1, char x2, char y) {             //Нарисовать горизонтальный отрезок
  if (y >= HEIGHT)
    return;

  if (MODE == 2) {
    for (char i = x1; i < x2; i++) {
      if (i >= WIDTH)
        break;
      leds[XY(y, i)] = CRGB::Black;
    }
  }
}

void drawTriangle(Point a, Point b, Point c) {           //Нарисовать треугольник
  char x1 = char(round(a.z)) + WIDTH / 2;                //Преобразуем значения с математической плоскости YZ с отрицательными значенями на реальную матрицу (перенос начала координат в центр экрана)
  char y1 = char(round(a.y)) + HEIGHT / 2;
  char x2 = char(round(b.z)) + WIDTH / 2;
  char y2 = char(round(b.y)) + HEIGHT / 2;
  char x3 = char(round(c.z)) + WIDTH / 2;
  char y3 = char(round(c.y)) + HEIGHT / 2;

  if (MODE != 1) {
    char tmp;                                            //Выбираем верхнюю вершину
    if (y1 < y2) {
      tmp = y1;
      y1 = y2;
      y2 = tmp;
      tmp = x1;
      x1 = x2;
      x2 = tmp;
    }
    if (y1 < y3) {
      tmp = y1;
      y1 = y3;
      y3 = tmp;
      tmp = x1;
      x1 = x3;
      x3 = tmp;
    }
    if (y2 < y3) {                                       //Определяем среднюю и нижнюю вершины
      tmp = y2;
      y2 = y3;
      y3 = tmp;
      tmp = x2;
      x2 = x3;
      x3 = tmp;
    }

    float cross_x2;
    float cross_x3;
    float dx2 = x2 - x1;
    float dy2 = y2 - y1;
    float dx3 = x3 - x1;
    float dy3 = y3 - y1;
    float top_y = y1;
    while (top_y > y2) {                                 //Заполняем верхнюю половину треугольника
      cross_x2 = x1 + dx2 * (top_y - y1) / dy2;
      cross_x3 = x1 + dx3 * (top_y - y1) / dy3;
      if (cross_x2 > cross_x3)
        drawHorLine(round(cross_x3), round(cross_x2), round(top_y));
      else
        drawHorLine(round(cross_x2), round(cross_x3), round(top_y));
      top_y--;
    }

    dx2 = x3 - x2;
    dy2 = y3 - y2;
    while (top_y > y3) {                                 //Заполняем нижнюю половину треугольника
      cross_x2 = x2 + dx2 * (top_y - y2) / dy2;
      cross_x3 = x1 + dx3 * (top_y - y1) / dy3;
      if (cross_x2 > cross_x3)
        drawHorLine(round(cross_x3), round(cross_x2), round(top_y));
      else
        drawHorLine(round(cross_x2), round(cross_x3), round(top_y));
      top_y--;
    }
  }

  drawSegment(y1, x1, y2, x2);                           //Рисуем грани
  drawSegment(y1, x1, y3, x3);
  drawSegment(y2, x2, y3, x3);
}

void drawFigure() {                                      //Нарисовать фигуру
  for (char i = 0; i < number_triangles; i++) {          //Находим расстояние от камеры до треугольников
    Point p0 = points[indexBuffer[i][0]];
    Point p1 = points[indexBuffer[i][1]];
    Point p2 = points[indexBuffer[i][2]];
    Point center = {(p0.x + p1.x + p2.x) / 3.0f, (p0.y + p1.y + p2.y) / 3.0f, (p0.z + p1.z + p2.z) / 3.0f};
    Point d = {camera.x - center.x, camera.y - center.y, camera.z - center.z};
    distTriangle[i] = sqrtf(d.x * d.x + d.y * d.y + d.z * d.z);
  }

  float dist = -1;                                       //Рисуем треугольники от самого дальнего
  char index = 0;
  for (char i = 0; i < number_triangles; i++) {          //Ищем самый дальний треугольник
    for (char q = 0; q < number_triangles; q++) {
      if (distTriangle[q] > dist) {
        dist = distTriangle[q];
        index = q;
      }
    }
    drawTriangle(projectionPoint(points[indexBuffer[index][0]]), projectionPoint(points[indexBuffer[index][1]]), projectionPoint(points[indexBuffer[index][2]]));
    distTriangle[index] = -1;
    dist = -1;
  }
}

int XY(char x, char y) {                                 //Функция преобразования координат
  //Для wokwi
  if (y < 16)
	  return ((16 - x) * 16  - (16 - y));
  else
    return ((16 - x) * 16  - (16 - y)) + 16 * 15;
  //Для реальной матрицы
  /*if (y % 2 == 0)
	  return (y * HEIGHT  + (HEIGHT - x - 1));
  return (y * HEIGHT  + x);*/
}

void newBrightness() {                                   //Изменение яркости
  unsigned char new_brightness = analogRead(BRT_PIN) * 0.25;
  if (abs(new_brightness - old_brightness) > 3) {
    FastLED.setBrightness(new_brightness);
    old_brightness = new_brightness;
  }
}

void setNextFigure() {                                   //Смена фигуры
  FastLED.clear();

  if (FIGURE == 1) {
    number_points = pgm_read_byte(&number_points_cube);
    number_triangles = pgm_read_byte(&number_triangles_cube);
    for (char i = 0; i < number_points; i++) {
      memcpy_P(&points[i], &points_cube[i], sizeof(Point));
    }
    for (char i = 0; i < number_triangles; i++) {
      for (char j = 0; j < 3; j++) {
        indexBuffer[i][j] = pgm_read_byte(&indexBuffer_cube[i][j]);
      }
    }
    FIGURE = 2;
  }
  else if (FIGURE == 2) {
    number_points = pgm_read_byte(&number_points_stairs);
    number_triangles = pgm_read_byte(&number_triangles_stairs);
    for (char i = 0; i < number_points; i++) {
      memcpy_P(&points[i], &points_stairs[i], sizeof(Point));
    }
    for (char i = 0; i < number_triangles; i++) {
      for (char j = 0; j < 3; j++) {
        indexBuffer[i][j] = pgm_read_byte(&indexBuffer_stairs[i][j]);
      }
    }
    FIGURE = 3;    
  }
  else if (FIGURE == 3) {
    number_points = pgm_read_byte(&number_points_tetrahedron);
    number_triangles = pgm_read_byte(&number_triangles_tetrahedron);
    for (char i = 0; i < number_points; i++) {
      memcpy_P(&points[i], &points_tetrahedron[i], sizeof(Point));
    }
    for (char i = 0; i < number_triangles; i++) {
      for (char j = 0; j < 3; j++) {
        indexBuffer[i][j] = pgm_read_byte(&indexBuffer_tetrahedron[i][j]);
      }
    }
    FIGURE = 1;
  }
}

void setNextMode() {                                     //Смена режима
  FastLED.clear();

  if (MODE == 1)
    MODE = 2;
  else if (MODE == 2)
    MODE = 1;
}

void loop() {
  if (digitalRead(BUTTON_PIN) == LOW && startTime == 0)
    startTime = millis();
  if (startTime != 0 && digitalRead(BUTTON_PIN) == HIGH) {
    if (millis() - startTime >= 1000) {
      setNextFigure();
      startTime = 0;
    }
    else {
      setNextMode();
      startTime = 0;
    }
  }
  
  newBrightness();

  shapeRotation();

  drawFigure();

  FastLED.show();

  FastLED.clear();
}