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

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

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

CRGB leds[NUM_LEDS];                        //Массив для светодиодов
unsigned long startTime = 0;                //Время начала нажатия
unsigned char old_brightness = 0;           //Яркость

Point camera(60, 0, 0);
Point pointPlane1(50, WIDTH / 2, HEIGHT / 2);
Point pointPlane2(50, -WIDTH / 2, -HEIGHT / 2);
Point pointPlane3(50, -WIDTH / 2, HEIGHT / 2);
Plane cameraPlane(pointPlane1, pointPlane2, pointPlane3);
/*
char const number_points = 8;
class Point figure[number_points] = {Point(20, 20, 20), Point(-20, 20, 20), Point(20, -20, 20), Point(-20, -20, 20),
  Point(20, 20, -20), Point(-20, 20, -20), Point(20, -20, -20), Point(-20, -20, -20)};
char indexBuffer[12][3] = {
    {0, 1, 2}, {1, 3, 2},
    {4, 6, 5}, {5, 6, 7},
    {0, 4, 1}, {1, 4, 5},
    {2, 3, 6}, {3, 7, 6},
    {0, 2, 4}, {2, 6, 4},
    {1, 5, 3}, {5, 7, 3}
};*/

char const number_points = 4;
class Point figure[number_points] = {Point(20, 20, 20), Point(20, -20, -20), Point(-20, 20, -20), Point(-20, -20, 20)};
char indexBuffer[number_points][4] = {{0, 1, 2}, {1, 2, 3}, {0, 2, 3}, {0, 1, 3}};

void setup() {
  FastLED.addLeds<WS2811, LED_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(analogRead(BRT_PIN) * 0.25);
  for (char i = 0; i < HEIGHT; i++) {       //Очистка матрицы
    for (char j = 0; j < WIDTH; j++) {
      leds[XY(i, j)] = CRGB(0, 0, 0);
    }
  }
  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);
}

void shapeRotation() {
  float angle_horz = (analogRead(HORZ_PIN) - 512.0) * turn_speed;     //Угол от -10 до 10 градусов
  float angle_vert = (analogRead(VERT_PIN) - 512.0) * turn_speed;

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

    float angle_vert_point = atan2(figure[i].z, figure[i].x);
    float radius_vert = sqrt(figure[i].x*figure[i].x + figure[i].z*figure[i].z);
    figure[i].x = radius_vert*cos(angle_vert + angle_vert_point);
    figure[i].z = radius_vert*sin(angle_vert + angle_vert_point);
  }
}

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;
  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(Point point1, Point point2) {         //Нарисовать отрезок
    char x1 = char(round(point1.y)) + HEIGHT / 2;
    char y1 = char(round(point1.z)) + WIDTH / 2;
    char x2 = char(round(point2.y)) + HEIGHT / 2;
    char y2 = char(round(point2.z)) + WIDTH / 2;
    const char deltaX = abs(x2 - x1);
    const char deltaY = abs(y2 - y1);
    const char signX = x1 < x2 ? 1 : -1;
    const char signY = y1 < y2 ? 1 : -1;
    char error = deltaX - deltaY;
    while(x1 != x2 || y1 != y2) {
      leds[XY(x1, y1)] = CRGB::Green;
      char error2 = error * 2;
      if(error2 > -deltaY) {
        error -= deltaY;
        x1 += signX;
      }
      if(error2 < deltaX) {
        error += deltaX;
        y1 += signY;
      }
    }
}

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;
    //FastLED.show();
  }
}

void loop() {
  if (digitalRead(BUTTON_PIN) == LOW && startTime == 0)
    startTime = millis();
  if (startTime != 0 && digitalRead(BUTTON_PIN) == HIGH) {
    if (millis() - startTime >= 1000) {
      //SetNextAlgorithm();
      startTime = 0;
    }
    else {
      //SetNextMode();
      startTime = 0;
    }
  }
  
  NewBrightness();

  shapeRotation();

  for (char i = 0; i < number_points; i++) {
    Point point = projectionPoint(figure[i]);
    leds[XY(round(point.y) + HEIGHT / 2, round(point.z) + WIDTH / 2)] = CRGB::Green;
  }

  for (char i = 0; i < 4; i++) {
    drawSegment(projectionPoint(figure[indexBuffer[i][0]]), projectionPoint(figure[indexBuffer[i][1]]));
    drawSegment(projectionPoint(figure[indexBuffer[i][1]]), projectionPoint(figure[indexBuffer[i][2]]));
    drawSegment(projectionPoint(figure[indexBuffer[i][2]]), projectionPoint(figure[indexBuffer[i][0]]));
  }
  /*
  for (char i = 0; i < 8; i++) {
    Point point = projectionPoint(figure[i]);
    leds[XY(round(point.y) + HEIGHT / 2, round(point.z) + WIDTH / 2)] = CRGB::Green;
  }
  drawSegment(projectionPoint(figure[0]), projectionPoint(figure[1]));
  drawSegment(projectionPoint(figure[0]), projectionPoint(figure[2]));
  drawSegment(projectionPoint(figure[1]), projectionPoint(figure[3]));
  drawSegment(projectionPoint(figure[2]), projectionPoint(figure[3]));

  drawSegment(projectionPoint(figure[4]), projectionPoint(figure[5]));
  drawSegment(projectionPoint(figure[4]), projectionPoint(figure[6]));
  drawSegment(projectionPoint(figure[5]), projectionPoint(figure[7]));
  drawSegment(projectionPoint(figure[6]), projectionPoint(figure[7]));

  drawSegment(projectionPoint(figure[0]), projectionPoint(figure[4]));
  drawSegment(projectionPoint(figure[1]), projectionPoint(figure[5]));
  drawSegment(projectionPoint(figure[2]), projectionPoint(figure[6]));
  drawSegment(projectionPoint(figure[3]), projectionPoint(figure[7]));

  drawSegment(figure[0], figure[1]);
  drawSegment(figure[0], figure[2]);
  drawSegment(figure[1], figure[3]);
  drawSegment(figure[2], figure[3]);

  // нижняя грань (z = -)
  drawSegment(figure[4], figure[5]);
  drawSegment(figure[4], figure[6]);
  drawSegment(figure[5], figure[7]);
  drawSegment(figure[6], figure[7]);

  // вертикальные рёбра (соединяют верх/низ)
  drawSegment(figure[0], figure[4]);
  drawSegment(figure[1], figure[5]);
  drawSegment(figure[2], figure[6]);
  drawSegment(figure[3], figure[7]);*/

  FastLED.show();
  //delay(50);
  for (char i = 0; i < HEIGHT; i++) {       //Очистка матрицы
    for (char j = 0; j < WIDTH; j++) {
      leds[XY(i, j)] = CRGB(0, 0, 0);
    }
  }
}