#include <stdio.h>
#include <math.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

#define TFT_DC 2
#define TFT_CS 15
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
#define P1_PIN 35
#define P2_PIN 32
#define P3_PIN 33
#define P4_PIN 25

#define DEBUG 1

struct point {
  double x;
  double y;
};

struct cyrcle {
  point center;
  double radius;
};

point p1, p2;
point planeA, planeB, plane;

#define NN 4
cyrcle c[ NN ];
point pA[ NN * 3 ], pB[ NN * 3 ];
int cntA, cntB;

long time_calc, time_all;

void showStatus( int status );
void update_points();
int interception( cyrcle c1, cyrcle c2, point *p1, point *p2);
double distance(point a, point b);
void showResultPoints();

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  printf("\n");

  setup_graphics();

  c[0].center.x = 50;
  c[0].center.y = 10;
  c[0].radius = 30;

  c[1].center.x = 35;
  c[1].center.y = 9;
  c[1].radius = 30;

  c[2].center.x = -20;
  c[2].center.y = 10;
  c[2].radius = 70;

  c[3].center.x = 85;
  c[3].center.y = 5;
  c[3].radius = 60;

  cntA, cntB = 0;
  calculate();
}
/////////////////////////////////////////////////
void calculate(){
  time_all = micros();
  for (int i = 0; i < NN - 1; i++ ) {
    for (int m = i + 1; m < NN; m++) {
      time_calc = micros();
      int res = interception (c[i], c[m], &p1, &p2);
      time_calc = micros() - time_calc;
      if (res > 0) update_points( );
      showStatus( res );
    }
  }
  time_all = micros() - time_all;
  showResultPoints();
  showResultGraphics();
}

void update_points() {
  if ( cntA == 0 ) {
    pA[cntA++] = p1;
    pB[cntB++] = p2;
  }
  else {
    double d1 = distance(pA[0], p1);
    double d2 = distance(pB[0], p1);
    if (d1 < d2) {
      pA[cntA++] = p1;
    }
    else {
      pB[cntB++] = p1;
    }
    d1 = distance(pA[0], p2);
    d2 = distance(pB[0], p2);
    if (d1 < d2) {
      pA[cntA++] = p2;
    }
    else {
      pB[cntB++] = p2;
    }
  }
}

double distance(point a, point b) {
  double dx = b.x - a.x;
  double dy = b.y - a.y;
  return sqrt(dx * dx + dy * dy);
}

void showResultPoints() {
  double distA, distB, sumX, sumY = 0;

  printf("Точки в хмарі А:\n");
  for (int i = 0; i < cntA; i++) {
    printf("   %u - (%.6f, %.6f)\n", i, pA[i].x, pA[i].y );
  }
  printf("Точки в хмарі B:\n");
  for (int i = 0; i < cntB; i++) {
    printf("   %u - (%.6f, %.6f)\n", i, pB[i].x, pB[i].y );
  }
  sumX = pA[cntA - 1].x; sumY = pA[cntA - 1].y;
  for (int i = 0; i < cntA - 1; i++) {
    for (int k = i + 1; k < cntA; k++) {
      distA += distance(pA[i], pA[k]);
    }
    sumX += pA[i].x;
    sumY += pA[i].y;
  }
  planeA.x = sumX / cntA;
  planeA.y = sumY / cntA;

  sumX = pB[cntB - 1].x; sumY = pB[cntB - 1].y;
  for (int i = 0; i < cntB - 1; i++) {
    for (int k = i + 1; k < cntB; k++) {
      distB += distance(pB[i], pB[k]);
    }
    sumX += pB[i].x;
    sumY += pB[i].y;
  }
  planeB.x = sumX / cntB;
  planeB.y = sumY / cntB;

  printf("Загальна дистанція в хмарі А %.6f\n", distA);
  printf("Загальна дистанція в хмарі B %.6f\n", distB);
  if (distA < distB) {
    printf("  Точка літака A (%.6f, %.6f)\n", planeA.x, planeA.y );
    plane = planeA;
  }
  else {
    printf("  Точка літака B (%.6f, %.6f)\n", planeB.x, planeB.y );
    plane = planeB;
  }
  printf("Загальний час розрахунку %.3f ms\n", time_all / 1000.0);
}

void showStatus( int status ) {
  //printf("Result = %u\n", status);
  switch (status) {
    case -1:
      printf("Кола не перетинаються!\n");
      break;
    case 1:
      printf("Кола дотикаються в точці: (%.6f, %.6f)\n", p1.x, p1.y);
      break;
    case 2:
      printf("Кола перетинаються в точках: (%.6f, %.6f) та (%.6f, %.6f) \n", p1.x, p1.y, p2.x, p2.y);
  }
  //printf("Час розрахунку в мkс - %u\n", time_calc);
}

void loop() {
  cntA = 0;
  cntB = 0;
  c[0].radius = analogRead(P1_PIN)/16;
  c[1].radius = analogRead(P2_PIN)/16;
  c[2].radius = analogRead(P3_PIN)/16;
  c[3].radius = analogRead(P4_PIN)/16;
  calculate();
  delay(5000); // this speeds up the simulation
}



int interception( cyrcle c1, cyrcle c2, point *p1, point *p2) {
  double x1, y1, r1;
  double x2, y2, r2;

  x1 = c1.center.x;
  y1 = c1.center.y;
  r1 = c1.radius;

  x2 = c2.center.x;
  y2 = c2.center.y;
  r2 = c2.radius;

  p1->x = 0;
  p1->y = 0;
  p2->x = 0;
  p2->y = 0;

  double dx = x2 - x1;
  double dy = y2 - y1;
  double d = sqrt(dx * dx + dy * dy);

  if (d > r1 + r2) {
    printf("Кола не перетинаються (занадто далеко один від одного).\n");
    return -1;
  } else if (d < fabs(r1 - r2)) {
    printf("Кола не перетинаються (одне коло всередині іншого).\n");
    return -1;
  } else if (d == 0 && r1 == r2) {
    printf("Кола співпадають (безліч точок перетину).\n");
    return -1;
  } else {
    // Обчислимо точку пересічення лінії центрів і точок перетину
    double a = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
    double h = sqrt(r1 * r1 - a * a);

    // Координати точки P на лінії між центрами
    double x3 = x1 + a * (dx) / d;
    double y3 = y1 + a * (dy) / d;

    // Координати точок перетину
    double rx = -dy * (h / d);
    double ry = dx * (h / d);

    double xi1 = x3 + rx;
    double yi1 = y3 + ry;

    double xi2 = x3 - rx;
    double yi2 = y3 - ry;

    if (h == 0) {
      p1->x = x3;
      p1->y = y3;
      return 1;
    } else {
      p1->x = xi1;
      p1->y = yi1;
      p2->x = xi2;
      p2->y = yi2;
      return 2;
    }
  }
  return 0;
}

void setup_graphics(){
  tft.begin();
  tft.setRotation(1);
  tft.setTextColor(ILI9341_WHITE);
}

void showResultGraphics(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(0, 0);
  tft.print("(0,0)");

  double koef=1, kx, ky, xmin=1e8, xmax=0, ymin=1e8, ymax=0, dx=1e8, dy=1e8;
  printf("\n  Before scale\n");
  for (int i=0; i<NN; i++) {
    printf("Circle %u (%.1f, %.1f, %.1f)\n", i, c[i].center.x, c[i].center.y, c[i].radius);
    if ( c[i].center.x - c[i].radius < xmin ) xmin = c[i].center.x - c[i].radius;
    if ( c[i].center.x + c[i].radius > xmax ) xmax = c[i].center.x + c[i].radius;
    if ( c[i].center.y - c[i].radius < ymin ) ymin = c[i].center.y - c[i].radius;
    if ( c[i].center.y + c[i].radius > ymax ) ymax = c[i].center.y + c[i].radius;
    if (dx > c[i].center.x - c[i].radius ) dx = c[i].center.x - c[i].radius;
    if (dy > c[i].center.y - c[i].radius ) dy = c[i].center.y - c[i].radius;
  }
  kx = 320 / (xmax - xmin);
  ky = 240 / (ymax - ymin);
  //koef = min(kx, ky);
  printf("dx=%.1f dy=%.1f koef=%.2f xmin=%.1f xmax=%.1f ymin=%.1f ymax=%.1f\n", dx, dy, koef, xmin, xmax, ymin, ymax);
  printf("  After scale\n");
  for (int i=0; i<NN; i++) {
    double x = (c[i].center.x-dx)*koef;
    double y = (c[i].center.y-dy)*koef;
    double r = c[i].radius*koef;
    printf("Circle %u (%.1f, %.1f, %.1f)\n", i, x, y, r);
    tft.drawCircle( x, y, r, ILI9341_WHITE);
  }
  tft.fillCircle( (plane.x-dx)*koef, (plane.y-dy)*koef, 2, ILI9341_RED);
  for (int i=0; i<cntA; i++) tft.drawCircle((pA[i].x-dx)*koef, (pA[i].y-dy)*koef, 4, ILI9341_YELLOW);
  for (int i=0; i<cntB; i++) tft.drawCircle((pB[i].x-dx)*koef, (pB[i].y-dy)*koef, 4, ILI9341_BLUE);
  printf("\n\n\n\n");
}