/*
  Robot_triangulation_using_circumcircles.ino by lingib
  https://www.instructables.com/member/lingib/instructables/
  Last update: 15 June 2025

  ----------
  ABOUT
  ----------
  This code is for an experimental laser positioning system.
   
  The matching Processing code is: 
  Robot_triangulation_using_circumcircles.pde by lingib
  
  ----------
  THEORY
  ----------
  Precise angle measurement is possible if you rotate a laser beam and apply the following formula:
      angle = time-interval_between-two-given-points/rotation_time*360

  In this project, we have four beacons B0, B1, B2, B3  
  Beacons B0, B2, B3 are equally spaced to form an equilateral triangle.
  Beacon B0 identifies itself by emitting a second pulse which I've called beacon B1

  Assume that we have four beacons B0, B1, B2, B3 and that the laser rotates at 60RPM (1000000 uS/rev).
  If the pulses start at B0, and we ignore B1:

    angle1=(B2-B0)*360/1000000
    angle2=(B3-B2)*360/1000000
    angle3=((B0+1000000)-B3)*360/100000

  Where:
    (B2-B0) is the time interval from beacon B0 to B2
    (B3-B2) is the time interval from beacon B2 to B3
    ((B0+1000000)-B3) is the time interval from beacon B3 to B0 after one revolution

  A "switch(){case ...}" function calculates the angles when the start point is other than B0

  The maths is such that the robot's XY coordinate can be calculated 
  from the observed angles and the beacon spacing. 
  
  This beacon positioning system is entirely interupt driven. 
  No code is required in the main loop().

  ----------
  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/>.  
*/

#define BEACON_INTERRUPT_PIN 2

bool DEBUG = true;

class BeaconDetector {
public:
  unsigned long beaconTimes[4];
  unsigned long intervals[4];
  volatile byte count = 0;
  volatile bool ready = false;

  unsigned long timePerRevolution = 1000000;  // default value for first rev
  unsigned long lastBeacon0Time = 0;

  float angle1 = 0, angle2 = 0, angle3 = 0;

  void init() {
    pinMode(BEACON_INTERRUPT_PIN, INPUT);
    attachInterrupt(digitalPinToInterrupt(BEACON_INTERRUPT_PIN), beaconISR, FALLING);
  }

  void recordBeaconTime(unsigned long t) {
    if (ready) return;  // Don't overwrite data while it's being processed

    if (count == 0) {
      // This is the first beacon of a new revolution
      if (lastBeacon0Time != 0) {
        timePerRevolution = t - lastBeacon0Time;
      }
      lastBeacon0Time = t;
    }

    if (count < 4) {
      beaconTimes[count++] = t;
      if (count == 4) {
        ready = true;
      }
    }
  }

  void computeAngles() {
    if (!ready || timePerRevolution == 0) {
      Serial.println(F("Not ready or invalid revolution time."));
      return;
    }

    noInterrupts();  // Make a copy to avoid partial updates
    unsigned long times[4];
    for (int i = 0; i < 4; i++) times[i] = beaconTimes[i];
    interrupts();

    if (DEBUG) {
      for (int i = 0; i < 4; i++) {
        Serial.print("beaconTimes[");
        Serial.print(i);
        Serial.print("]: ");
        Serial.println(times[i]);
      }
    }

    // Compute time deltas between consecutive beacons
    intervals[0] = times[1] - times[0];
    intervals[1] = times[2] - times[1];
    intervals[2] = times[3] - times[2];
    intervals[3] = times[0] + timePerRevolution - times[3];  // Wrap-around

    if (DEBUG) {
      Serial.println(" ");
      for (int i = 0; i < 4; i++) {
        Serial.print("intervals[");
        Serial.print(i);
        Serial.print("]: ");
        Serial.println(intervals[i]);
      }
      Serial.print("Sum of intervals: ");
      Serial.println(intervals[0] + intervals[1] + intervals[2] + intervals[3]);
    }

    // Find smallest interval
    int minIndex = 0;
    unsigned long minInterval = intervals[0];
    for (int i = 1; i < 4; i++) {
      if (intervals[i] < minInterval) {
        minInterval = intervals[i];
        minIndex = i;
      }
    }

    if (DEBUG) {
      Serial.print("\nminInterval: ");
      Serial.println(minInterval);
      Serial.print("minIndex: ");
      Serial.println(minIndex);
    }

    switch (minIndex) {
      case 0:
        angle1 = intervals[0] + intervals[1];
        angle2 = intervals[2];
        angle3 = intervals[3];
        break;
      case 1:
        angle1 = intervals[1] + intervals[2];
        angle2 = intervals[3];
        angle3 = intervals[0];
        break;
      case 2:
        angle1 = intervals[2] + intervals[3];
        angle2 = intervals[0];
        angle3 = intervals[1];
        break;
      case 3:
        angle1 = intervals[0] + intervals[3];
        angle2 = intervals[1];
        angle3 = intervals[2];
        break;
    }

    angle1 = angle1 * 360.0 / timePerRevolution;
    angle2 = angle2 * 360.0 / timePerRevolution;
    angle3 = angle3 * 360.0 / timePerRevolution;

    if (DEBUG) {
      Serial.print("\nangle1: ");
      Serial.println(angle1);
      Serial.print("angle2: ");
      Serial.println(angle2);
      Serial.print("angle3: ");
      Serial.println(angle3);
      Serial.print("Sum: ");
      Serial.println(angle1 + angle2 + angle3);
    }

    if (abs(angle1 + angle2 + angle3 - 360.0) < 3.6)
      Serial.println(F("Angle sum check: OK"));
    else
      Serial.println(F("Angle sum check: FAILED"));

    // ---------- Begin Coordinate Calculations ----------
    // float chord1 = 2000.0;  // mm
    // float chord2 = 2000.0;  // mm
    float chord1 = 866.0254;  // calibrator radius is 500mm
    float chord2 = 866.0254;  // calibrator radius is 500mm

    float degToRad = 3.14159265 / 180.0;
    float sinA1 = sin(angle1 * degToRad);
    float sinA3 = sin(angle3 * degToRad);

    float r1 = chord1 / (2.0 * sinA1);
    float r2 = chord2 / (2.0 * sinA3);

    float angle60A1 = (60.0 - angle1) * degToRad;
    float angle60A3 = (60.0 - angle3) * degToRad;

    float x1 = r1 * sin(angle60A1);
    float y1 = r1 * cos(angle60A1);

    float x2 = -r2 * sin(angle60A3);
    float y2 = r2 * cos(angle60A3);

    float numeratorX = 2.0 * (x1 * y2 - x2 * y1) * (y2 - y1);
    float numeratorY = 2.0 * (x1 * y2 - x2 * y1) * (x1 - x2);
    float denominator = pow(y2 - y1, 2) + pow(x1 - x2, 2);

    float x = numeratorX / denominator;
    float y = numeratorY / denominator;

    Serial.println(F("\n--- Robot Coordinates ---"));
    Serial.print("r1: ");
    Serial.println(r1);
    Serial.print("r2: ");
    Serial.println(r2);
    Serial.print("x1: ");
    Serial.println(x1);
    Serial.print("y1: ");
    Serial.println(y1);
    Serial.print("x2: ");
    Serial.println(x2);
    Serial.print("y2: ");
    Serial.println(y2);
    Serial.print("Robot X: ");
    Serial.println(x);
    Serial.print("Robot Y: ");
    Serial.println(y);
    Serial.print("POS,"); // Output to Processing 4.4.4
    Serial.print(x, 1);   // 1 decimal place
    Serial.print(",");
    Serial.println(y, 1);
    Serial.println(F("-------------------------"));
    // ---------- End Coordinate Calculations ----------


    count = 0;
    ready = false;
  }

  static void beaconISR() {
    if (instance) instance->recordBeaconTime(micros());
  }

  static BeaconDetector* instance;
};

BeaconDetector* BeaconDetector::instance = nullptr;
BeaconDetector detector;

void setup() {
  Serial.begin(115200);
  BeaconDetector::instance = &detector;
  detector.init();
  detector.lastBeacon0Time = micros();  // Initialize to prevent invalid duration on first cycle
}

void loop() {
  if (detector.ready) {
    detector.computeAngles();
    delay(200);  // Avoid immediate re-triggering
  }
}