/* ============================================
  code is placed under the MIT license
  Copyright (c) 2025 J-M-L
  For the Arduino Forum : https://forum.arduino.cc/u/j-m-l
  Discussion : https://forum.arduino.cc/t/help-with-making-if-statement-routine-more-efficient/1363808$0

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
  ===============================================
*/


#include "VarSpeedServo.h" // https://github.com/netlabtoolkit/VarSpeedServo$0

const byte servoXPin = 9;
const byte servoYPin = 10;
VarSpeedServo servoX;
VarSpeedServo servoY;

int targetX, targetY;

int maxDistanceToDestination(int targetX, int targetY) {
  int dX = servoX.read() - targetX; dX = abs(dX);
  int dY = servoY.read() - targetY; dY = abs(dY);
  return max(dX, dY);
}

void trackMovement(int targetX, int targetY) {
  static int previousDistance = -1;
  int currentDistance = maxDistanceToDestination(targetX, targetY);
  if (previousDistance != currentDistance) {
    previousDistance = currentDistance;
    Serial.print(F("Max Distance to go : ")); Serial.println(currentDistance);
    if (currentDistance == 0) {
      Serial.println(F("Both Servo reached their respective target position."));
    }
  }
}

void setup() {
  servoX.attach(servoXPin);
  servoY.attach(servoYPin);

  Serial.begin(115200);
  Serial.println(F("\nHOMING"));

  // set the intial position of the servo, as fast as possible, wait until done
  servoX.write( 0, 255, true);     // write(value, speed, wait)
  servoY.write(20, 255, true);     // write(value, speed, wait)

  delay(2000);                    // small pause to see them at their position

  Serial.println(F("setting new destination"));
  // move the servo to a target position, slow speed so that we can see what's happening, run in background
  targetX = 180;
  targetY = 150;
  servoX.write(targetX, 20, false);
  servoY.write(targetY, 40, false);
}

void loop() {
  trackMovement(targetX, targetY);
}



/* some of the VarSpeedServo API

  attach(pin )                - Attaches a servo motor to an i/o pin.
  attach(pin, min, max  )     - Attaches to a pin setting min and max values in microseconds, default min is 544, max is 2400
  attached()                  - Returns true if there is a servo attached.
  detach()                    - Stops an attached servos from pulsing its i/o pin.

  write(value)                - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
  write(value, speed)         - speed varies the speed of the move to new position 0=full speed, 1-255 slower to faster
  write(value, speed, wait)   - wait is a boolean that, if true, causes the function call to block until move is complete

  writeMicroseconds()         - Sets the servo pulse width in microseconds
  read()                      - Gets the last written servo pulse width as an angle between 0 and 180.
  readMicroseconds()          - Gets the last written servo pulse width in microseconds. (was read_us() in first release)

  stop()                      - stops the servo at the current position

  wait();                     - wait for movement to finish
  isMoving()                  - return true if servo is still moving

*/