/*
DESCRIPTION
This demo uses a servo and a button to mimic a BLTouch module.
BLTouch modules are used in 3D printing for mesh bed leveling.
They create a mapping of the build plate and adjust for the offset while printing.

---

HOW THE LEVELING WORKS
The print head with the module moves to spots on the bed and extends it's probe (in this case the servo rotates).
When the probe touches the bed a signal is sent and the probe retracts. (In this case manually simulated with the button)
This process is repeated for all points.

---

INSTRUCTON
The cycles when the start button (green) is pressed.

Press the stop button (red) to mimic touching the bed.
The servo will then stop and move to the pos "0".
The LED will turn on to visualize it.
This process is repeated until the cycle is finished.
In that case numbers will be printed in the output.

The next cycle starts when the start button is pressed again.
Be aware that the next cycle will only start when the previous cycle is completed.
*/

#include <Servo.h>

// Define the pins of the components in the simulation
#define SERVO_PIN 14
#define BTN_START_PIN 13
#define BTN_STOP_PIN 15
#define LED_PIN 12

// Number of probing points per axis
// Total points = X * Y
#define X 3
#define Y 3

Servo bl_servo;
int pos = 0;        // Current servo position

int mapping[X][Y];  // Simulated offsets

void setup()
{
  bl_servo.attach(SERVO_PIN);
  pinMode(BTN_START_PIN, INPUT);
  pinMode(BTN_STOP_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);

  reset();
}

void loop()
{
  if (digitalRead(BTN_START_PIN) == HIGH)
  {
    // Start the cycle
    for (int x = 0; x < X; ++x)
    {
      for (int y = 0; y < Y; ++y)
      {
        // Measure and save
        mapping[x][y] = probe();
      }
    }

    // Show the measurements
    display();
  }

  delay(1);
}

void reset()
{
  // Move back to "0"
  for (pos = pos; pos >= 0; --pos)
  {
    bl_servo.write(pos);

    delay(5);
  }

  // Turn the LED off
  digitalWrite(LED_PIN, LOW);
}

int probe()
{
  // Slowly extend
  for (pos = 0; pos <= 180; ++pos)
  {
    bl_servo.write(pos);

    // If touching bed
    if (digitalRead(BTN_STOP_PIN) == HIGH)
    {
      // Stop the extension
      break;
    }

    delay(15);
  }

  // Turn the LED on
  digitalWrite(LED_PIN, HIGH);

  // Calculate the offset, where 90° is 0; 180° is -90; 0° is 90
  int offset = (pos - 90) * -1;

  // Wait hald a second
  delay(500);

  // Retract the probe
  reset();

  return offset;
}

void display()
{
  // display the measuements as a grid
  for (int x = 0; x < X; ++x)
  {
    for (int y = 0; y < Y; ++y)
    {
      String cell = String(mapping[x][y]) + " ";

      if (mapping[x][y] >= 0)
      {
        cell = "+" + cell;
      }
      
      Serial.print(cell);
    }
    Serial.print("\n");
  }

  Serial.print("\n");
}