/*
  Robot_triangulation_using_circumcircles.pde by lingib
  https://www.instructables.com/member/lingib/instructables/
  Last update: 11 July 2025

  ----------
  ABOUT
  ----------
  
  This code is for displaying the output of my experimental laser positioning system.  
   
  The project comprises three equally spaced beacons that remain silent until struck 
  by a rotating laser beam. The maths is such that the robot's XY coordinate can be calculated 
  from the observed angles and the beacon spacing. 
  
  The matching Arduino code is 
  Robot_triangulation_using_circumcircles.pde by lingib
  https://www.instructables.com/member/lingib/instructables/
  
  ----------
  COPYRIGHT
  ----------
  This code is free software: you can redistribute it and/or
  modify it under the terms of the GNU General Public License as published
  by the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This software is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License. If
  not, see <http://www.gnu.org/licenses/>.  
*/


import processing.serial.*;

Serial myPort;
float robotX = 0;
float robotY = 0;

void setup() {
  size(600, 600);  // 1000mm × 1000mm grid (1mm = 0.6 px)
  String portName = Serial.list()[0];  // Change index if needed
  myPort = new Serial(this, portName, 115200);
  myPort.bufferUntil('\n');
}

void draw() {
  background(255);
  drawGrid();
  drawBeaconTriangle();
  drawRobot(robotX, robotY);
}

void drawGrid() {
  stroke(220);
  fill(0);
  textAlign(CENTER, CENTER);
  textSize(10);

  // Vertical gridlines every 100 mm (-500 to +500)
  for (int i = -5; i <= 5; i++) {
    float x = map(i * 100, -500, 500, 0, width);
    line(x, 0, x, height);
    if (i != 0) text(i * 10 + " cm", x, height - 10);
  }

  // Horizontal gridlines every 100 mm (0 to 1000)
  for (int i = 0; i <= 10; i++) {
    float y = map(i * 100, 0, 1000, height, 0);
    line(0, y, width, y);
    if (i != 0) text(i * 10 + " cm", width / 2 + 30, y);
  }

  // Origin marker
  fill(0);
  ellipse(width / 2, height, 6, 6);
  text("(0, 0)", width / 2 + 25, height - 10);
}

void drawBeaconTriangle() {
  float centerX = 0;
  float centerY = 500;
  float radius = 500;

  // Change this:
  float[] angles = { -90, 150, 30 };  // Clockwise order: top, left, right

  float[] px = new float[3];
  float[] py = new float[3];

  for (int i = 0; i < 3; i++) {
    float rad = radians(angles[i]);
    float x_mm = centerX + radius * cos(rad);
    float y_mm = centerY + radius * sin(rad);

    px[i] = map(x_mm, -500, 500, 0, width);
    py[i] = map(y_mm, 0, 1000, height, 0);
  }

  stroke(0, 128, 255);
  strokeWeight(2);
  noFill();
  triangle(px[0], py[0], px[1], py[1], px[2], py[2]);

  fill(0, 128, 255);
  for (int i = 0; i < 3; i++) {
    ellipse(px[i], py[i], 10, 10);
    text("Beacon " + (i + 1), px[i], py[i] - 15);
  }
}

void drawRobot(float x_mm, float y_mm) {
  float px = map(x_mm, -500, 500, 0, width);
  float py = map(y_mm, 0, 1000, height, 0);

  fill(255, 0, 0);
  ellipse(px, py, 20, 20);

  fill(0);
  textAlign(CENTER, TOP);
  text(nf(x_mm / 10, 0, 1) + " cm", px, py + 10);
  text(nf(y_mm / 10, 0, 1) + " cm", px, py + 25);
}

void serialEvent(Serial p) {
  String line = p.readStringUntil('\n');
  if (line == null) return;

  line = trim(line);
  if (line.startsWith("POS,")) {
    String[] parts = split(line.substring(4), ",");
    if (parts.length == 2) {
      robotX = float(parts[0]);  // mm
      robotY = float(parts[1]);  // mm
    }
  }
}