#include <TFT_eSPI.h>
#include "position.h"

TFT_eSPI tft = TFT_eSPI(); // Inicjalizacja obiektu TFT_eSPI

struct Point {
    float x;
    float y;
};

struct yPoint {
    float y1;
    float y2;
};

Point rotatePoint(const Point& A, float angle) {
    // Konwersja kąta na radiany
    float radians = angle * 3.14159 / 180.0;

    float Sx = A.x - 120;
    float Sy = A.y - 120;

    // Obliczenie współrzędnych po obrocie
    float newX = Sx * cos(radians) - Sy * sin(radians);
    float newY = Sx * sin(radians) + Sy * cos(radians);

    newX += 120;
    newY += 120;

    // Zwrócenie nowych współrzędnych
    return {newX, newY};
}

void setup() {
  tft.init(); // Inicjalizacja ekranu
  tft.setRotation(0); // Obróć ekran o 90 stopni (opcjonalnie)
  Serial.begin(9600);
  pinMode(13, INPUT);
  pinMode(12, INPUT);


  int theta = 20;
  int pitchAngle = -15;
  Point A = {0, 120};
  Point B = {240, 120};
  Point rA = rotatePoint(A, theta);
  Point rB = rotatePoint(B, theta);

  /*Point pA = pitchPoint(rA, pitchAngle, theta);
  Point pB = pitchPoint(rB, pitchAngle, theta);

  rA = pA;
  rB = pB;*/

  tft.drawLine(rA.x, rA.y, rB.x, rB.y, 0x9afa);

  int i = round(rA.x);
  Serial.println(rA.x);
  Serial.println(rA.y);
  Serial.println(rB.x);
  Serial.println(rB.y);
  while(i < rB.x)
  {
    yPoint temp = findY(120, i, 120, 120);
    //Serial.println(temp.y1);
    int yF = findYF(i, rA.x, rA.y, rB.x, rB.y);
    int h = abs(round(temp.y2) - yF);

    tft.drawFastVLine(i, temp.y2, h, 0x423f);

    int h2 = abs(round(temp.y1) - yF);
    tft.drawFastVLine(i, yF, h2, 0xb4a8);
    i++;
  }
  i = 0;

  if(rA.y > 120)
  {
    while(i < rA.x)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0x423f);
      i++;
    }
  }
  else if(rA.y < 120)
  {
    while(i < rA.x)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0xb4a8);
      i++;
    }
  }
  else
  {
    return;
  }

  i = round(rB.x);
  if(rB.y < 120)
  {
    while(i < 240)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0xb4a8);
      i++;
    }
  }
  else if(rB.y > 120)
  {
    while(i < 240)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0x423f);
      i++;
    }
  }
  else
  {
    return;
  }

}

void drawPlane()
{
  tft.fillRect(39, 119, 52, 7, 0x0000);
  tft.fillRect(89, 119, 7, 17, 0x0000);
  tft.fillRect(109, 119, 8, 8, 0x0000);
  tft.fillRect(129, 119, 57, 7, 0x0000);
  tft.fillRect(129, 119, 7, 17, 0x0000);

  tft.fillRect(40, 120, 50, 5, 0xffc0);
  tft.fillRect(90, 120, 5, 15, 0xffc0);

  tft.fillRect(110, 120, 6, 6, 0xffc0);
  tft.fillRect(130, 120, 55, 5, 0xffc0);
  tft.fillRect(130, 120, 5, 15, 0xffc0);
}

yPoint findY(float r, int x, float a, float b)
{
  float y1 = sqrt(r * r - x * x + 2 * x * a - a * a) + b;
  float y2 = -sqrt(r * r - x * x + 2 * x * a - a * a) + b;

  return {y1, y2};
}

float findX(float y, float r, float a, float b, bool plus)
{
  float x;
  if(!plus)
  {
    x = -sqrt(r * r - y * y + 2 * y * b - b * b) + a;
  }
  else
  {
    x = sqrt(r * r - y * y + 2 * y * b - b * b) + a;
  }
  return x;
}

int findYF(int x, float aX, float aY, float bX, float bY)
{
  float a = (bY - aY) / (bX - aX);
  float b = aY - a * aX;

  int yF = a * x + b;
  return round(yF);
}

Point pitchPoint(Point p, float pitch, float roll)
{
  float rollAngle = 3.14 / 2 - roll * 3.14 / 180;
  float newX;
  float pitchAngle = pitch * cos(rollAngle);

  Serial.println(pitchAngle);

  if(p.x > 0 & p.y < 120)
  {
    newX = p.x + pitchAngle;
  }
  else if(p.x > 120 & p.y < 120)
  {
    newX = p.x - pitchAngle;
  }
  else if(p.x > 120 & p.y > 120)
  {
    newX = p.x + pitchAngle;
  }
  else if(p.x > 0 & p.y > 120)
  {
    newX = p.x - pitchAngle;
  }
  else
  {
    newX = p.x;
  }
  bool invert = false;
  if(newX > 240)
  {
    float delta = abs(newX - 240);
    newX = 240 - delta;
    invert = true; 
  }
  else if(newX < 0)
  {
    float delta = abs(newX);
    newX = delta;
    invert = true;
  }


  float newY;
  if(p.y > 120)
  {
    yPoint newPointY = findY(120, round(newX), 120, 120);
    if(invert)
    {
      newY = newPointY.y1;
    }
    else
    {
      newY = newPointY.y2;
    }  
  }
  else if(p.y < 120)
  {
    yPoint newPointY = findY(120, round(newX), 120, 120);
    if(invert)
    {
      newY = newPointY.y2;
    }
    else
    {
      newY = newPointY.y1;
    }  
  }

  return {newX, newY};
}

/*Point pitchPoint(Point p1, Point p2, float pitch, float roll, bool plus)
{
  float a = (p2.y - p1.y) / (p2.x - p1.x);
  float b = p1.y - a * p1.x;

  float sx = (p1.x + p2.x) / 2;
  float sy = (p1.y + p2.y) / 2;

  //pitch od -120 do 120
  //float xS = pitch * cos((90 - roll) * 0.017453);
  float deltaY = pitch * sin((90 - roll) * 0.17453);
  float yL = a * (sx + pitch) + b;

  float xC = findX(yL, 120, 120, 120, plus);
  yPoint YCP = findY(120, xC, 120, 120);
  float yC;

  if(p1.y + deltaY < 120)
  {
    yC = YCP.y2;
  }
  else
  {
    yC = YCP.y1;
  }

  return {xC, yC};
}*/

void loop() {
  int value = analogRead(13);
  int value2 = analogRead(12);
  int roll = map(value2, 0,4098, -80, 80);
  int intensity = map(value,0,4098,-90,90);
  
  //Serial.println(intensity);


  int theta = roll;
  int pitchAngle = intensity;
  Point A = {0, 120};
  Point B = {240, 120};
  Point rA = rotatePoint(A, theta);
  Point rB = rotatePoint(B, theta);
  /*Serial.println(rB.x);
  Serial.println(rB.y);
  Serial.println(rA.x);
  Serial.println(rA.y);*/

  Point pA = pitchPoint(rA, pitchAngle, theta);
  Point pB = pitchPoint(rB, pitchAngle, theta);

  rA = pA;
  rB = pB;

  tft.drawLine(rA.x, rA.y, rB.x, rB.y, 0x9afa);

  int i = round(rA.x);
  Serial.println(pitchAngle);
  Serial.println("/////////////////////////");
  while(i < rB.x)
  {
    yPoint temp = findY(120, i, 120, 120);
    //Serial.println(temp.y1);
    int yF = findYF(i, rA.x, rA.y, rB.x, rB.y);
    int h = abs(round(temp.y2) - yF);

    tft.drawFastVLine(i, temp.y2, h, 0x423f);

    int h2 = abs(round(temp.y1) - yF);
    tft.drawFastVLine(i, yF, h2, 0xb4a8);
    i++;
  }
  i = 0;

  if(rA.y > 120)
  {
    while(i < rA.x)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0x423f);
      i++;
    }
  }
  else if(rA.y < 120)
  {
    while(i < rA.x)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0xb4a8);
      i++;
    }
  }
  else
  {
    return;
  }

  i = round(rB.x);
  if(rB.y < 120)
  {
    while(i < 240)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0xb4a8);
      i++;
    }
  }
  else if(rB.y > 120)
  {
    while(i < 240)
    {
      yPoint temp = findY(120, i, 120, 120);

      int h = abs(round(temp.y1) - round(temp.y2));
      tft.drawFastVLine(i, temp.y2, h, 0x423f);
      i++;
    }
  }
  else
  {
    return;
  }

  drawPlane();

  delay(1000);
}