//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 int syncPC = 4;

const float period_us = 1000000 / 490; 
const float conversion = 255; // Number gotten to convert Duty Cycle percentages to analogWrite() output

const float pw_min = ((maxPW - minPW) / (maxSig - minSig)) * (minSig - minSig) + minPW;
const float pw_max = ((maxPW - minPW) / (maxSig - minSig)) * (maxSig - minSig) + minPW;
const float pw_rest = ((maxPW - minPW) / (maxSig - minSig)) * (restSig - minSig) + minPW;

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

void setup() {
  pinMode(forwardPin, INPUT);
  pinMode(backwardPin, INPUT);
  pinMode(commandPin, OUTPUT);  
  Serial.begin(9600);
}

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;
  Serial.print(command);
  Serial.print(", ");

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

  Serial.println("");

  delayMicroseconds(4*period_us); // Temporary
}

void commandSignal(int pulseCount, int motorDir, int turnDir, float turn) {
  int mode = 10*motorDir + turnDir; // motorDir: 0 - Back, 1 - None, 2 - Forward
                                    // turnDir:  0 - Left, 1 - None, 2 - Right

  Serial.println(mode);

  float anglePWM_L = ((pw_rest - pw_min) / ((restSig - shiftSig) - minSig)) * (turn - minSig) + pw_min;
  float anglePWM_R = ((pw_max - pw_rest) / (maxSig - (restSig + shiftSig))) * (turn - (restSig + shiftSig)) + pw_rest;

  // Synchronization Signal
  signalPWM(syncPC, syncPW);

  switch (mode) {
    case 21:
      signalPWM(forwardPC, centerPW);
      break;
    case 01:
      signalPWM(backwardPC, centerPW);
      break;
    case 10:
      signalPWM(leftPC, anglePWM_L);
      break;
    case 12:
      signalPWM(rightPC, anglePWM_R);
      break;
    case 20:
      signalPWM(forwardLeftPC, anglePWM_L);
      break;
    case 00:
      signalPWM(backwardLeftPC, anglePWM_L);
      break;
    case 22:
      signalPWM(forwardRightPC, anglePWM_R);
      break;
    case 02:
      signalPWM(backwardRightPC, anglePWM_R);
      break;
    default:
      digitalWrite(commandPin, LOW);
  }
}

void signalPWM(int PC, float PW) {
  float remain = period_us - PW;
  for (int i = 0; i <= PC - 1; i++) {
    digitalWrite(commandPin, HIGH);
    delayMicroseconds(PW);
    digitalWrite(commandPin, LOW);
    delayMicroseconds(remain);
  }
}