//
// hello.txtx2.RP2040.ino
//    RP2040 XIAO two-channel transmit-receive step-response hello-world
//    overclock at 250 MHz
// -> modified
//
// Neil Gershenfeld 7/10/23
// Miranda Li 10/13/25
//

#define digitalWriteFast(pin,val) (val ? sio_hw->gpio_set = (1 << pin) : sio_hw->gpio_clr = (1 << pin))
#define digitalReadFast(pin) ((1 << pin) & sio_hw->gpio_in)

#define UP_TX 4
#define UP_LED 2

#define LEFT_TX 0
#define LEFT_LED 7

#define RIGHT_TX 27
#define RIGHT_LED 28

#define DOWN_TX 29
#define DOWN_LED 6

#define RX 26

#define settle 80 // settle time (init: 20)
#define samples 200 // number of samples to accumulate (init: 2000)
#define CALIBRATION_LOOPS 5 // number of calibration loops to average

#define THRESHOLD 500

// Baseline values will be calibrated at startup
int32_t UP_BASELINE = 39240;
int32_t DOWN_BASELINE = 39240;
int32_t LEFT_BASELINE = 39240;
int32_t RIGHT_BASELINE = 39240;

bool calibrated = false;

void setup() {
   Serial.begin(115200);
   }

void loop() {
   }

void setup1() {
   pinMode(UP_TX,OUTPUT);
   pinMode(DOWN_TX,OUTPUT);
   pinMode(LEFT_TX,OUTPUT);
   pinMode(RIGHT_TX,OUTPUT);
   pinMode(UP_LED,OUTPUT);
   pinMode(DOWN_LED,OUTPUT);
   pinMode(LEFT_LED,OUTPUT);
   pinMode(RIGHT_LED,OUTPUT);
   }

void calibrate_baselines() {
   Serial.println("Calibrating baselines...");
   
   int32_t up_sum = 0, down_sum = 0, left_sum = 0, right_sum = 0;
   
   for (int loop = 0; loop < CALIBRATION_LOOPS; loop++) {
      int32_t up_value = 0, down_value = 0, left_value = 0, right_value = 0;
      
      // Run samples iterations just like in the main loop
      for (int i = 0; i < samples; ++i) {
         update_value(UP_TX, up_value);
         update_value(DOWN_TX, down_value);
         update_value(LEFT_TX, left_value);
         update_value(RIGHT_TX, right_value);
         }
      
      up_sum += up_value;
      down_sum += down_value;
      left_sum += left_value;
      right_sum += right_value;
      }
   
   // Calculate averages
   UP_BASELINE = up_sum / CALIBRATION_LOOPS;
   DOWN_BASELINE = down_sum / CALIBRATION_LOOPS;
   LEFT_BASELINE = left_sum / CALIBRATION_LOOPS;
   RIGHT_BASELINE = right_sum / CALIBRATION_LOOPS;
   
   Serial.print("Calibration complete. Loops: ");
   Serial.println(CALIBRATION_LOOPS);
   Serial.print("UP_BASELINE: ");
   Serial.println(UP_BASELINE);
   Serial.print("DOWN_BASELINE: ");
   Serial.println(DOWN_BASELINE);
   Serial.print("LEFT_BASELINE: ");
   Serial.println(LEFT_BASELINE);
   Serial.print("RIGHT_BASELINE: ");
   Serial.println(RIGHT_BASELINE);
   }

void update_value(int tx_pin, int32_t& value) {
   digitalWriteFast(tx_pin,HIGH); // charge up
   value += analogRead(RX); // read
   delayMicroseconds(settle); //settle

   digitalWriteFast(tx_pin,LOW); // charge down
   value -= analogRead(RX); // read
   delayMicroseconds(settle); // settle
   }

void update_led(int32_t value, int32_t baseline, int led_pin) {
   if (value > baseline + THRESHOLD) {
      digitalWriteFast(led_pin, HIGH);
   } else {
      digitalWriteFast(led_pin, LOW);
   }
   }

void loop1() {
   // Run calibration once at startup
   if (!calibrated) {
      calibrate_baselines();
      calibrated = true;
      }
   
   int32_t up_value, down_value, left_value, right_value;
   up_value = down_value = left_value = right_value = 0;
   
   for (int i = 0; i < samples; ++i) {
      update_value(UP_TX, up_value);
      update_value(DOWN_TX, down_value);
      update_value(LEFT_TX, left_value);
      update_value(RIGHT_TX, right_value);
      }
   
   // Control LEDs based on thresholds
   update_led(up_value, UP_BASELINE, UP_LED);
   update_led(down_value, DOWN_BASELINE, DOWN_LED);
   update_led(left_value, LEFT_BASELINE, LEFT_LED);
   update_led(right_value, RIGHT_BASELINE, RIGHT_LED);
   
   // Send data with names for serial plotter
   Serial.print("UP:");
   Serial.print(up_value);
   Serial.print(" DOWN:");
   Serial.print(down_value);
   Serial.print(" LEFT:");
   Serial.print(left_value);
   Serial.print(" RIGHT:");
   Serial.print(right_value);
   // Serial.print("UP_BASELINE: ");
   // Serial.println(UP_BASELINE);
   // Serial.print("DOWN_BASELINE: ");
   // Serial.println(DOWN_BASELINE);
   // Serial.print("LEFT_BASELINE: ");
   // Serial.println(LEFT_BASELINE);
   // Serial.print("RIGHT_BASELINE: ");
   // Serial.println(RIGHT_BASELINE);
   Serial.println("");
   Serial.flush(); // finish communicating before measuring
   }