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

//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

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(inPin1, LOW);
    speed(0);
  }
  void speed(int aSpeed) {
    if (aSpeed >= 0) {
      analogWrite(pwmPin, aSpeed);
    }
  }
  // The following functions are not used here
  // but could be useful in future
  void forward() {
    digitalWrite(inPin1, HIGH);
    digitalWrite(inPin1, LOW);
  }
  void back() {
    digitalWrite(inPin1, LOW);
    digitalWrite(inPin1, HIGH);
  }
};

motorType prtMotor;
motorType stbdMotor;
motorType vertMotor;



//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

// Centre Joystick Values
//int x_zero = 0; //Centre X value
//int y_zero = 0; //Centre Y Value
//int z_zero = 0; //Centre Z Value

//Max Joystick Values
//int Max_X_Voltage = 4;
//int Max_Y_Voltage = 4;
//int Max_Z_Voltage = 4;

//Min Joystick Values
int Min_X_Voltage = 0;
int Min_Y_Voltage = 0;
int Min_Z_Voltage = 0;

//int Z_CountR = 0;// Used to access Rows data in Z_Joystick_Array
//int Z_CountC = 0;// Used to access Columns data in Z_Joystick_Array
int Z_Rows = 5; //Max Number of Rows in Z Joystick Array
int Z_Columns = 4; //Max Number of Columns in Z Joystick Array

int X_Count = 0; // Used to access X_Voltage data in XY_Joystick_Array
int Y_Count = 8; // Used to access Y_Voltage date in XY_Joystick_Array
int XY_Columns = 7; //Used to access Columns in XY_Joystick_Array
int XY_Rows = 9; // Number of Rows in XY Joystick Array

// Array for decoding Z Axis Joystick values - Z_Voltage, In3, In4, PWM
int Z_Joystick_Array[5][4] = {
  {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
int XY_Joystick_Array [9][8] {
  {2, 2, 0, 0, 0, 0, 0, 0}, // Both Motors stopped
  {2, 4, 1, 0, 50, 1, 0, 127}, // Both Motors forward
  {4, 2, 1, 0, 50, 0, 0, 0}, // Right/starboard - Port Motor forward PWM 50%, Starboard Motor stopped
  {0, 2, 0, 0, 0, 1, 0, 127}, // Left/Port - Starboard Motor forward PWM 50%, Port Motor stopped
  {2, 0, 0, 1, 50, 0, 1, 127}, //Both Motors Reverse
  {4, 0, 1, 0, 25, 1, 0, 127}, //Left/Port Forwards - Starboard Motor Forward PWM 50%,Port Motor Forward PWM 25%
  {0, 4, 0, 1, 50, 0, 1, 64}, //Right/Starboard Reverse - Port Motor Reverse PWM 50%, Starboard Motor Forward PWM 25%
  {4, 4, 1, 0, 50, 1, 0, 64}, //Right/Starboard Forward - Port Motor Forward PWM 50%, Starboard Motor Reverse PWM 25%
  {0, 0, 0, 1, 25, 0, 1, 127} //Left/Port Reverse - Port Motor Reverse PWM 25%, Starboard Motor Reverse PWM 50%
};

#define X_Pin A0 //Joystick input Port/Starboard pin
#define Y_Pin A1 //Joystick input Forward/Reverse pin
#define Z_Pin A2 //Joystick input up/down

int scaled_value(int adc, int scale_factor = 5) {
  constexpr int max_adc = 1023;
  int scaled = scale_factor * adc / max_adc;
  return scaled;
}
bool inRange(int val, int minimum, int maximum)
{
  return ((minimum <= val) && (val <= maximum));
}

void setup() {

  //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

  //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);

  //Read zero joystick values

  //x_zero = scaled_value(analogRead(X_Pin), 5);
  //y_zero = scaled_value(analogRead(Y_Pin), 5);
  //z_zero = scaled_value(analogRead(Z_Pin), 5);

  //Enable Serial Output for Displaying values
  Serial.begin(9600);

}

int oldX, oldY, oldZ;

void loop() {
  //Read average Joystick Values
    if (joystickDataHaveChanged()) {
      ControlVertMotor();
    }
}

boolean joystickDataHaveChanged() {
  X_Voltage = scaled_value(analogRead(X_Pin), 5);
  Y_Voltage = scaled_value(analogRead(Y_Pin), 5);
  Z_Voltage = scaled_value(analogRead(Z_Pin), 5);
  if (oldX != X_Voltage ||
      oldY != Y_Voltage ||
      oldZ != Z_Voltage    )
  {
    Serial.print("X Voltage\t");
    Serial.print(X_Voltage);
    Serial.print("\tY Voltage\t");
    Serial.print(Y_Voltage);
    Serial.print("\tZ Voltage\t");
    Serial.println(Z_Voltage);
    oldX = X_Voltage;
    oldY = Y_Voltage;
    oldZ = Z_Voltage;
    return true;
  }
  return false;
}

void ControlVertMotor() {
  for (int Z_CountR = 0; Z_CountR < Z_Rows; Z_CountR++)
  {
    int Array_Voltage = 0;
    Array_Voltage = Z_Joystick_Array[Z_CountR][0];
    if (Z_Voltage == Array_Voltage)
    {
      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);

      Serial.print("Z Array Row: ");
      Serial.print(Z_CountR);
      Serial.print("\tArray Voltage: ");
      Serial.print(Array_Voltage);
      Serial.print("\tVert In3: ");
      Serial.print(in1);
      Serial.print("\tVert In4: ");
      Serial.print(in2);
      Serial.print("\tVert PWM: ");
      Serial.println(pwmValue);
      return;
    }
  }
}