 #include "DW1000Device.h"
 #include <MatrixMath.h>
 #include <math.h>
-//A3: 1
-//A2: 3
-//A1: 2
+
+#define CALIBRATE_OFFSET false
+#define CALIBRATE_MULTIPLIER false
+#define CALIBRATION_DISTANCE 1.0
+
+#define BEACON_1_ADDRESS 0x1A1A
+#define BEACON_2_ADDRESS 0x2A1A
+#define BEACON_3_ADDRESS 0x1A3A
+
+#define BEACON_1_X 10.0f
+#define BEACON_1_Y 5.0f
+
+#define BEACON_2_X 10.0f
+#define BEACON_2_Y 0.0f
+
+#define BEACON_3_X 0.0f
+#define BEACON_3_Y 0.0f
+
+#define BEACON_1_OFFSET -0.09f
+#define BEACON_1_MULTIPLIER 1.2f
+
+#define BEACON_2_OFFSET -0.09f
+#define BEACON_2_MULTIPLIER 0.7257f
+
+#define BEACON_3_OFFSET -0.09f
+#define BEACON_3_MULTIPLIER 1.55f
+
 // connection pins
 const uint8_t PIN_RST = 3; // reset pin
 const uint8_t PIN_IRQ = 7; // irq pin

}

void newRange() {
  address = DW1000Ranging.getDistantDevice()->getShortAddress();

  range = DW1000Ranging.getDistantDevice()->getRange();
  range -= 0.2;
  range *= 0.7257;
  dist[0] = address==0x1A1A?range:dist[0];
  dist[1] = address==0x2A1A?range:dist[1];
  dist[2] = address==0x1A3A?range:dist[2];
  Serial.print("1: ");Serial.print(dist[0]);Serial.print(" 2: ");Serial.print(dist[1]);Serial.print(" 3: ");Serial.print(dist[2]);
  
  // ITERATIVE STEPS BELOW


volatile int address = 0;

volatile float dist[3] = {655.35, 655.35, 655.35};

float beacons[3][2] = {{BEACON_1_X, BEACON_1_Y},
                       {BEACON_2_X, BEACON_2_Y},
                       {BEACON_3_X, BEACON_3_Y}};

float shortestDistanceBeacon1 = 900.0f;
float shortestDistanceBeacon2 = 900.0f;
float shortestDistanceBeacon3 = 900.0f;

float A[3][3];
void setup() {
