const int numPins = 3;
const int pwmPins[] = {A1, A2, A3};
const float minDutyCycle[numPins] = {5.98, 6.03, 6.03};
const float maxDutyCycle[numPins] = {12.08, 12.02, 11.94};
const float deadzone = 0.1;
volatile float mappedDutyCycles[numPins] = {0.0, 0.0, 0.0};

void setup() {
  Serial.begin(9600);
  delay(5000);

  for (int i = 0; i < numPins; i++) {
    pinMode(pwmPins[i], INPUT);
  }
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void loop() {
  for (int i = 0; i < numPins; i++) {
    unsigned long highDuration = pulseIn(pwmPins[i], HIGH);
    unsigned long lowDuration = pulseIn(pwmPins[i], LOW);
    unsigned long period = highDuration + lowDuration;
    float dutyCycle = (float)highDuration / (float)period * 100.0;

    // Map the duty cycle value to the range of -1.0 to 1.0
    mappedDutyCycles[i] = mapFloat(dutyCycle, minDutyCycle[i], maxDutyCycle[i], -1.0, 1.0);

    // Apply deadzone
    if (mappedDutyCycles[i] > -deadzone && mappedDutyCycles[i] < deadzone) {
      mappedDutyCycles[i] = 0.0;
    }
  }

  delay(10); // Wait for 10ms before the next reading
}

// Running on core1
void setup1() {
  delay(5000);
}

void loop1() {
  // Print the pin name and the mapped duty cycle in a format suitable for the Serial Plotter
  for (int i = 0; i < numPins; i++) {
    Serial.print("Pin ");
    Serial.print(pwmPins[i]);
    Serial.print(": ");
    Serial.print(mappedDutyCycles[i]);

    if (i < numPins - 1) {
      Serial.print(", ");
    } else {
      Serial.println();
    }
  }

  delay(10); // Wait for 10ms before the next transmission
}