//https://forum.arduino.cc/t/creation-of-pwm-signal-for-transmitter-based-on-digital-and-analog-input-for-an-rc-car/1105063

int forwardPin = 12;
int backwardPin = 13;
int commandPin = 10;

// Pulse Widths
const int centerPW = 750;             // Standard Pulse Width for no turning
const int minPW = 500;                 // Minimum Pulse Width for turning
const int maxPW = 1000;              // Maximum Pulse Width for turning
const int syncPW = 1500;              // Synchronization Segment Pulse Width
const float maxSig = 1023.0;        // Joystick minimum and maximum serial outputs
const float minSig = 0.0;
const float restSig = 501.0;           // Resting joystick serial output
const float shiftSig = 30.0;            // Serial output shift of joystick to apply deadzone

// Pulse Counts for each mode of operation
const int forwardPC = 15;
const int backwardPC = 40;
const int leftPC = 25;
const int rightPC = 30;
const int forwardLeftPC = 10;
const int backwardLeftPC = 20;
const int forwardRightPC = 35;
const int backwardRightPC = 45;

const float period = 1 / 490;
const float conversion = 255; // Number gotten to convert Duty Cycle percentages to analogWrite() output
float w_min = 0.12495 * (((maxPW - minPW) / (maxSig - minSig)) * minSig + minPW);
float w_max = 0.12495 * (((maxPW - minPW) / (maxSig - minSig)) * maxSig + minPW);
float w_rest = 0.12495 * (((maxPW - minPW) / (maxSig - minSig)) * restSig + minPW);

const int syncDutyCycle = syncPW / period;
const int centerCommandDutyCycle = centerPW / period;
const int syncInterval = conversion * syncDutyCycle;
const int centerCommandInterval = conversion * centerCommandDutyCycle;

void setup() {
  pinMode(forwardPin, INPUT);
  pinMode(backwardPin, INPUT);
  pinMode(commandPin, OUTPUT);
}

void loop() {
  int F = digitalRead(forwardPin);
  int B = digitalRead(backwardPin);
  float Turn = analogRead(A0);
  int L = 0;
  int R = 0;
  int command = 0;

  // Joystick center serial output is 500 to 501, so +/- 30 to
  // include an artificial dead zone to not create sudden turns
  if (Turn > restSig + shiftSig) {
    L = 0;
    R = 1;
  }
  else if (Turn < restSig - shiftSig) {
    L = 1;
    R = 0;
  }
  else {
    L = 0;
    R = 0;
  }

  command = 1000 * F + 100 * B + 10 * L + R;

  // commandSignal(a, b, c, d)
  // If b == 0, no back/forth; if b == 1, forward; if b == -1, backward
  // If c == 0, no turn; if c == 1, wheels angle right; if c == -1, wheels angle left
  switch (command) {
    case 1000:
      commandSignal(forwardPC, 1, 0, Turn);
      break;
    case 0100:
      commandSignal(backwardPC, -1, 0, Turn);
      break;
    case 0010:
      commandSignal(leftPC, 0, -1, Turn);
      break;
    case 0001:
      commandSignal(rightPC, 0, 1, Turn);
      break;
    case 1010:
      commandSignal(forwardLeftPC, 1, -1, Turn);
      break;
    case 0110:
      commandSignal(backwardLeftPC, -1, -1, Turn);
      break;
    case 1001:
      commandSignal(forwardRightPC, 1, 1, Turn);
      break;
    case 0101:
      commandSignal(backwardRightPC, -1, 1, Turn);
      break;
    default:
      command = 0;
  }
  delay(1000); // Temporary
}

void sync() {
  int i;
  for (i = 0; i <= 3; i++) {
    analogWrite(commandPin, syncInterval);
  }
}

void commandSignal(int pulseCount, int motorDir, int turnDir, float turn) {
  int i;
  float anglePWM = 0;
  sync();
  if (motorDir != 0 && turnDir == 0) {
    for (i = 0; i <= pulseCount - 1; i++) {
      analogWrite(commandPin, centerCommandInterval);
    }
  }
  else if (turnDir == -1) {
    anglePWM = ((w_rest - w_min) / ((restSig - shiftSig) - minSig)) * turn + w_min;
    for (i = 0; i <= pulseCount - 1; i++) {
      analogWrite(commandPin, anglePWM);
    }
  }
  else {
    anglePWM = ((w_max - w_rest) / (maxSig - (restSig + shiftSig))) * turn + w_rest;
    for (i = 0; i <= pulseCount - 1; i++) {
      analogWrite(commandPin, anglePWM);
    }
  }
}