// https://wokwi.com/projects/355651819818651649
// https://forum.arduino.cc/t/help-sending-ascii-commands-to-pan-tilt/1065163

# include <SoftwareSerial.h>

// Defines new serial Port not to interfeer with USB Digital Pin 4 = RX, Digital Pin 5 = TX
SoftwareSerial mySerial(4, 5);  // RX, TX

int Xpin = A0;             // Joystick X (horizontal)
int Ypin = A1;             // Joystick Y (vertical)
int Xval;
int Yval;
int restX = 512;           // Joystick X rest position value
int restY = 512;           // Joystick Y rest position value
int deadZoneX = 200;        // Dead zone for X (without movement)
int deadZoneY = 200;        // Dead zone for Y (without movement)
int dt1 = 100;              // Delay 1 time for program loop, This works fine with 100 (ms)
int dt2 = 40;              // Delay 2 time for Y axis, This works fine with 40 (ms)
int currentJoystickY;
int currentJoystickX;
int currentYPosition;
int currentXPosition;
int previousYPosition;
int previousXPosition;

void setup() {

  // open serial comunication
  Serial.begin(9600);

  // Set the baud rate for the SoftwareSerial object
  mySerial.begin(9600);

  // Define pin modes for Xpin and Ypin
  pinMode(Xpin, INPUT);
  pinMode(Ypin, INPUT);

  // Define pin modes for TX and RX
  pinMode(4, INPUT);
  pinMode(5, OUTPUT);

}

void loop() {
  doXJoyStick();
}

void doXJoyStick()
{
  enum {IDLE, LEFT, RIGHT, STOP};
  static unsigned char state = IDLE;

  int joyX = analogRead(A0) - 512; // normalize to -511..511 range

  switch (state) {
  case IDLE :
    if (joyX < -200) {
      Serial.println("PAN Right");
      Serial.println(joyX);
      mySerial.print("#AMMF0000W\r\n");                 // PAN Up
      mySerial.flush();

      state = LEFT;
    }
    else if (joyX > 200) {
      Serial.println("PAN Left");
      Serial.println(joyX);
      mySerial.print("#AMMB0000W\r\n");                  // PAN Down
      mySerial.flush();

      state = RIGHT;
    }
    break;

  case LEFT :
    if (joyX > -100)
      state = STOP;

    break;

  case RIGHT :
    if (joyX < 100)
      state = STOP;

    break;

  case STOP :
    Serial.println("PAN Stop");
    Serial.println(Xval);
    mySerial.print("#AMST0000W\r\n");                  // PAN Stop
    mySerial.flush();
    
    state = IDLE;

    break;
  }
}

void loopOriginal() {
  Yval = analogRead(Ypin);
  Xval = analogRead(Xpin);
  currentYPosition = Yval;
  currentXPosition = Xval;
  delay(dt1);

  if (getYJoystickUpdateReady() == true) {              // Check if joystick Y axis has change
    int currentJoystickY = Yval;
// if joystick Y axis has change command acording to the Yval value
    if (currentJoystickY > (restY + deadZoneY)) {       // Yval > 512 + 200 = 712
      Serial.println("TILT Up");                        // Show Command on the Serial Monitor
      Serial.println(Yval);                             // Show Value of Y axis on the Serial Monitor
      mySerial.print("$AMMF0000W\r\n");                 // TILT Up
      mySerial.flush();                                 // Wait to entire message to be sent
    }
    else if (currentJoystickY < (restY - deadZoneY)) {  // Yval < 512 - 200 = 312
      Serial.println("TILT Down");  //... down?
      Serial.println(Yval);
      mySerial.print("$AMMB0000W\r\n");                 // TILT Down
      mySerial.flush();
    }
    else if ( (restY - deadZoneY) < currentJoystickY < (restY + deadZoneY) ) {
      Serial.println("TILT Stop");                      // 312 < Yval < 712
      Serial.println(Yval);
      mySerial.print("$AMST0000W\r\n");                 // TILT Motor Stop
      mySerial.flush();
    }
    delay(dt2);                                         // This delay has proved to be needed to avoid X axis miss a command
  }
  if (getXJoystickUpdateReady() == true) {              // Check if joystick Y axis has change
    int currentJoystickX = Xval;
// if joystick X axis has change command acording to the Xval value
    if (currentJoystickX > (restX + deadZoneX)) {       // Xval > 512 + 200 = 712
      Serial.println("PAN Right");
      Serial.println(Xval);
      mySerial.print("#AMMF0000W\r\n");                 // PAN Up
      mySerial.flush();
    }
    else if (currentJoystickX < (restX - deadZoneX)) {  // Xval < 512 - 200 = 312
      Serial.println("PAN Left");
      Serial.println(Xval);
      mySerial.print("#AMMB0000W\r\n");                  // PAN Down
      mySerial.flush();
    }
    else if ( (restX - deadZoneX) < currentJoystickX < (restX + deadZoneX) ) {
      Serial.println("PAN Stop");                       // 312 < Xval < 712
      Serial.println(Xval);
      mySerial.print("#AMST0000W\r\n");                 // PAN Motor Stop
      mySerial.flush();
      delay(dt2);
      // This delay and the repeted "stop" command had to be added as some times the X axis will not receive this command.
      // This seams to insure the X axis do stop when needed
      mySerial.print("#AMST0000W\r\n");                 // PAN Motor Stop
      mySerial.flush();
    }
  }
}

bool getYJoystickUpdateReady() {
  // Determines if we are ready for another joystick update on the Y axis

  if (abs( currentYPosition - previousYPosition ) > 300 ) {
    previousYPosition = currentYPosition;
    return true;
  }
  return false;
}

bool getXJoystickUpdateReady() {
  // Determines if we are ready for another joystick update on the X axis

  if (abs( currentXPosition - previousXPosition ) > 300 ) {
    previousXPosition = currentXPosition;
    return true;
  }

  return false;
}