/*
  Forum: https://forum.arduino.cc/t/digital-pins-output-pins-not-being-written-to/1188669/13
  Wokwi: https://wokwi.com/projects/382382229268352001
*/

#define DEBUGPRINTS

//Port Motor Pins
constexpr int prtPWMPin = 9; //Port Motor PWM
constexpr int prtIn1Pin = 8; //Port Motor In1
constexpr int prtIn2Pin = 7; //Port Motor In2

//Starboard Motor Pins
constexpr int stbdPWMPin = 6; //Starboard Motor PWM
constexpr int stbdIn3Pin = 5; //Starboard Motor In3
constexpr int stbdIn4Pin = 4; //Starboard Motor In4

//Vertical Motor Pins
constexpr int vertPWMPin = 10; //Vertical Motor PWM Pin
constexpr int vertIn3Pin = 12; //Vertical Motor In3 Pin
constexpr int vertIn4Pin = 11; //Vertical Motor In4 Pin


constexpr byte FWD {0};
constexpr byte RVS {1};
constexpr byte STOP {2};

struct motorType {
  byte pwmPin;
  byte inPin1;
  byte inPin2;
  void init(byte pwm, byte in1, byte in2) {
    pwmPin = pwm;
    inPin1 = in1;
    inPin2 = in2;
    pinMode(inPin1, OUTPUT);
    pinMode(inPin2, OUTPUT);
    stop();
  }
  void setDirection(byte in1, byte in2) {
    digitalWrite(inPin1, in1);
    digitalWrite(inPin2, in2);
  }
  void stop() {
    digitalWrite(inPin1, LOW);
    digitalWrite(inPin2, LOW);
    speed(0);
  }
  void speed(int aSpeed) {
    if (aSpeed >= 0) {
      analogWrite(pwmPin, aSpeed);
    }
  }
  void forward() {
    digitalWrite(inPin1, HIGH);
    digitalWrite(inPin2, LOW);
  }
  void back() {
    digitalWrite(inPin1, LOW);
    digitalWrite(inPin2, HIGH);
  }
  void setDirSpeed(byte direction, int aSpeed) {
    switch (direction) {
      case FWD :
        forward();
        break;
      case RVS :
        back();
        break;
      case STOP :
        stop();
        aSpeed = 0;
        break;
    }
    speed(aSpeed);
  }
};

motorType prtMotor;
motorType stbdMotor;
motorType vertMotor;


// Array for decoding Z Axis Joystick values - Z_Voltage, In3, In4, PWM
constexpr int noOfRowsZ {5};
constexpr int noOfRColsZ {4};
int Z_Joystick_Array[noOfRowsZ][noOfRColsZ] = {
  {0, 0, 1, 127}, //Joystick full down - motor down at PWM 50%
  {1, 0, 1, 64}, //Joystick half down - motor down at PWM 25%
  {2, 0, 0, 0},  //Joystick in middle, motor stopped, PWM 0
  {3, 1, 0, 64}, //Joystick half up - motor up at PWM 25%
  {4, 1, 0, 127} //Joystick full up - motor up at 50%
};

//Array for decoded XY joystick - X_Voltage, Y_Voltage, In1, In2, PWM1, In3, In4, PWM2
constexpr int scaleXYMax = {3};
constexpr int noOfEntries {scaleXYMax * scaleXYMax};

int XY_MotorCtrl[noOfEntries][4] = {

  {RVS, 127, RVS, 64},
  {RVS, 127, RVS, 127},
  {RVS,  64, RVS, 127},

  {FWD,  127, STOP, 0},
  {STOP,   0, STOP, 0},
  {STOP,   0, RVS, 127},

  {FWD,  127, FWD, 64},
  {FWD,  127, FWD, 127},
  {FWD,   64, FWD, 127}

};

//Array for slew voltage on XY Joystick - Slew Voltage, In1, In2, PWM1, In3, In4,PWM2
constexpr int noOfRowsSL {5};
constexpr int noOfColsSL {7};

int SL_Array[noOfRowsSL][noOfColsSL] = {
  {0, 1, 0, 255, 0, 1, 255}, //Slew to left/port - starboard motor forward 50%, port motor reverse 50%
  {1, 1, 0, 127, 0, 1, 127}, //Slew to left/port 50%- starboard motor forward 25%, port motor reverse 25%
  {2, 0, 0, 0, 0, 0, 0},     //Stop both motors
  {3, 0, 1, 127, 1, 0, 127},//Slew to right/starboard 50% - port motor forward 25%, starboard motor reverse 25%
  {4, 0, 1, 255, 1, 0, 255}, //Slew to right/starboard - port motor forward 50%, starboard motor reverse 50%
};

constexpr byte X_Pin  {A0}; //Joystick input Port/Starboard pin
constexpr byte Y_Pin  {A1}; //Joystick input Forward/Reverse pin
constexpr byte Z_Pin  {A2}; //Joystick input Vertical up/down pin
constexpr byte SL_Pin {A3}; //XY Joystick slew pin

//Joystick input values
int X_Voltage = 0;  //Port/Starboard Joystick input value
int Y_Voltage = 0;  //Forward/Reverse Joystick input value
int Z_Voltage = 0;  //Vertical Joystick input value
int SL_Voltage = 0; // XY Joystick Slew value

int oldX  = 1;
int oldY  = 1;
int oldZ  = 2;
int oldSL = 2;

// function to return a scaled value of 0-5V from the analogue pins
int scaled_value(int adc, int scale_factor = 5) {
  constexpr int max_adc = 1024;   // Changed from 1023 to 1024 so that the range
  // is from 0..4 instead of 0..5 !
  int scaled = scale_factor * adc / max_adc;
  return scaled;
}

void setup() {
  //Enable Serial Output for Displaying values
  Serial.begin(115200);
#ifdef DEBUGPRINTS
  Serial.println("Debug prints ON!");
#endif

  //Enable Joystick inputs
  pinMode(X_Pin, INPUT); //Joystick X Input Pin
  pinMode(Y_Pin, INPUT); //Joystick Y Input Pin
  pinMode(Z_Pin, INPUT); //Input Z Joystick Pin
  pinMode(SL_Pin, INPUT); // XY Joystick Slew input Pin

  //Set Motors to stop -> now done in init()
  //Enable Port motor outputs
  prtMotor.init(prtPWMPin, prtIn1Pin, prtIn2Pin);

  //Enable Starboard Motor Outputs
  stbdMotor.init(stbdPWMPin, stbdIn3Pin, stbdIn4Pin);

  //Enable Vertical Motor Outputs
  vertMotor.init(vertPWMPin, vertIn3Pin, vertIn4Pin);
}


void loop() {
  if (Vert_joystickDataHaveChanged()) {
    ControlVertMotor();
  }
  if (XY_joystickDataHaveChanged()) {
    ControlPortStarbdMotor();
  }
  if (SL_joystickDataHaveChanged()) {
    ControlPortStarbdMotors_Slew();
  }
}

boolean Vert_joystickDataHaveChanged() {
  Z_Voltage = scaled_value(analogRead(Z_Pin), 5);
  if (oldZ != Z_Voltage)
  {
    oldZ = Z_Voltage;
    return true;
  }
  return false;
}

boolean XY_joystickDataHaveChanged() {
  X_Voltage = scaled_value(analogRead(X_Pin), scaleXYMax);
  Y_Voltage = scaled_value(analogRead(Y_Pin), scaleXYMax);

  if (oldX != X_Voltage ||
      oldY != Y_Voltage )
  {
    oldX = X_Voltage;
    oldY = Y_Voltage;
    return true;
  }
  return false;
}

boolean SL_joystickDataHaveChanged() {
  SL_Voltage = scaled_value(analogRead(SL_Pin), 5);
  if (oldSL != SL_Voltage)
  {
    oldSL = SL_Voltage;
    return true;
  }
  return false;
}

void ControlVertMotor() {
  for (int Z_CountR = 0; Z_CountR < noOfRowsZ; Z_CountR++)
  {
    if (Z_Voltage == Z_Joystick_Array[Z_CountR][0])
    {
      int in1 = Z_Joystick_Array[Z_CountR][1];
      int in2 = Z_Joystick_Array[Z_CountR][2];
      int pwmValue = Z_Joystick_Array[Z_CountR][3];
      vertMotor.setDirection(in1, in2);
      vertMotor.speed(pwmValue);
#ifdef DEBUGPRINTS
      Serial.print("VertMotor found for voltage ");
      Serial.print(Z_Voltage);
      Serial.print("\t in row ");
      Serial.println(Z_CountR);
#endif
      return;
    }
  }
#ifdef DEBUGPRINTS
  Serial.print("VertMotor NOT found for voltage ");
  Serial.println(Z_Voltage);
#endif
}

void ControlPortStarbdMotor() {
  int row = X_Voltage + Y_Voltage * scaleXYMax;

#ifdef DEBUGPRINTS
  Serial.print("Starbd Ctrl found for voltage X,Y = ");
  Serial.print(X_Voltage);
  Serial.print(",");
  Serial.print(Y_Voltage);
  Serial.print("\tin row ");
  Serial.println(row);
#endif

  setPortStarbdMotor(row);
}


void setPortStarbdMotor(int row) {
  //XY_MotorCtrl[row];
  int dir1       = XY_MotorCtrl[row][0];
  int pwmValue1  = XY_MotorCtrl[row][1];
  int dir2       = XY_MotorCtrl[row][2];
  int pwmValue2  = XY_MotorCtrl[row][3];
  stbdMotor.setDirSpeed(dir1, pwmValue1);
  prtMotor.setDirSpeed(dir2, pwmValue2);
}

void ControlPortStarbdMotors_Slew() {
  for (int SL_Count = 0; SL_Count < noOfRowsSL; SL_Count++)
  {
    if (SL_Array[SL_Count][0] == SL_Voltage) {
#ifdef DEBUGPRINTS
      Serial.print("Starbd Slew found for voltage ");
      Serial.print(SL_Voltage);
      Serial.print("\t in row ");
      Serial.println(SL_Count);
#endif
      setPortStarbdMotors_Slew(SL_Count);
      return;
    }
  }
#ifdef DEBUGPRINTS
  Serial.print("Starbd Slew NOT found for voltage ");
  Serial.println(SL_Voltage);
#endif
}

void setPortStarbdMotors_Slew(int row) {
  int in1       = SL_Array[row][1];
  int in2       = SL_Array[row][2];
  int pwmValue1 = SL_Array[row][3];
  int in3       = SL_Array[row][4];
  int in4       = SL_Array[row][5];
  int pwmValue2 = SL_Array[row][6];
  stbdMotor.setDirection(in1, in2);
  stbdMotor.speed(pwmValue1);
  prtMotor.setDirection(in3, in4);
  prtMotor.speed(pwmValue2);
}