// https://forum.arduino.cc/t/project-help-arduino-brushless-motor-control/1155414
// https://wokwi.com/projects/372681056554643457

#define SERIAL_PORT_SPEED 115200
#define RC_NUM_CHANNELS 2

#define RC_CH1_INPUT 2 // RC Channel 1 Input Pin
#define RC_CH2_INPUT 3 // RC Channel 2 Input Pin


uint16_t RC_VALUES[RC_NUM_CHANNELS];

char buffer[64];

void analogWrite(byte thePin, int theValue, char *msg)
{
  sprintf(buffer, "%s pin %d value %d\n", msg, thePin, theValue);
  Serial.print(buffer);
  analogWrite(thePin, theValue);
}

void digitalWrite(byte thePin, int theValue, char *msg)
{
  sprintf(buffer, "%s pin %d value %s\n", msg, thePin, theValue ? "HIGH" : "LOW");
  Serial.print(buffer);
  digitalWrite(thePin, theValue);
}

uint32_t RC_START[RC_NUM_CHANNELS];
volatile uint16_t RC_SHARED[RC_NUM_CHANNELS];

uint16_t RC_LOW[RC_NUM_CHANNELS] = { 1013, 1013 };
uint16_t RC_MID[RC_NUM_CHANNELS] = { 1508, 1508 };
uint16_t RC_HIGH[RC_NUM_CHANNELS] = { 2005, 2005 };

uint16_t RC_CHANNEL_MODE[RC_NUM_CHANNELS] = { 0, 0 };
int RC_INVERT[RC_NUM_CHANNELS] = { 1, 0 };

float RC_TRANSLATED_VALUES[RC_NUM_CHANNELS];
float RC_TRANSLATED_LOW[RC_NUM_CHANNELS] = { -50, -50 };
float RC_TRANSLATED_MID[RC_NUM_CHANNELS] = { 0, 0 };
float RC_TRANSLATED_HIGH[RC_NUM_CHANNELS] = { 50, 50 };
uint16_t RC_DZPERCENT[RC_NUM_CHANNELS] = { 5, 5 };

// Motor Configuration
const int PIN_PWM_MOTOR1 = 9;    // Left motor speed control (PWM) pin
const int PIN_DIR_MOTOR1 = 4; // Left motor direction control pin

const int PIN_PWM_MOTOR2 = 10;     // Right motor speed control (PWM) pin
const int PIN_DIR_MOTOR2 = 7; // Right motor direction control pin

int _pwmCtrl_Motor1 = 0; // PWM control signal for Motor 1 (0-255)
int _dir_Motor1 = 0; // Direction of Motor 1

int _pwmCtrl_Motor2 = 0; // PWM control signal for Motor 2 (0-255)
int _dir_Motor2 = 0; // Direction of Motor 2

void setup() {
  Serial.begin(SERIAL_PORT_SPEED);
  pinMode(RC_CH1_INPUT, INPUT);
  pinMode(RC_CH2_INPUT, INPUT);
  attachInterrupt(digitalPinToInterrupt(RC_CH1_INPUT), READ_RC1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(RC_CH2_INPUT), READ_RC2, CHANGE);

  pinMode(PIN_PWM_MOTOR1, OUTPUT);
  pinMode(PIN_DIR_MOTOR1, OUTPUT);

  pinMode(PIN_PWM_MOTOR2, OUTPUT);
  pinMode(PIN_DIR_MOTOR2, OUTPUT);
}

void loop() {
//  Serial.println(millis());

  delay(500);  // crude loop throttle
  Serial.println();

  rc_read_values();
  rc_invert_values();
  rc_deadzone_adjust();
  rc_translate_values();

  // Motor Control
  int motorSpeed = map(RC_VALUES[0], RC_LOW[0], RC_HIGH[0], -127, 127);
  motorSpeed = constrain(motorSpeed, -150, 150);

  int turnValue = map(RC_VALUES[1], RC_LOW[1], RC_HIGH[1], -127, 127);
  turnValue = constrain(turnValue, -150, 150);

  int leftMotorSpeed = motorSpeed + turnValue;
  int rightMotorSpeed = motorSpeed - turnValue;

  // Determine motor directions
  int leftMotorForward = leftMotorSpeed >= 0;
  int rightMotorForward = rightMotorSpeed >= 0;

  sprintf(buffer, "%s left @ %3d  %s right @ %3d\n",
    leftMotorForward ? "fwd" : "rev", leftMotorSpeed,
    rightMotorForward ? "fwd" : "rev", rightMotorSpeed
  );
  Serial.print(buffer);

  // If motor direction changed, gradually decrease PWM to 0 for coasting
  if (leftMotorForward != _dir_Motor1) {
Serial.println("              left decrease gradually");
    for (int pwm = abs(_pwmCtrl_Motor1); pwm >= 0; pwm--) {
      analogWrite(PIN_PWM_MOTOR1, pwm, "left grade");
      delay(50); // Small delay to gradually reduce PWM
    }
  }

  if (rightMotorForward != _dir_Motor2) {
Serial.println("              right decrease gradually");
    for (int pwm = abs(_pwmCtrl_Motor2); pwm >= 0; pwm--) {
      analogWrite(PIN_PWM_MOTOR2, pwm, "right grade");
      delay(50); // Small delay to gradually reduce PWM
    }
  }

 // Update motor direction variables
  _dir_Motor1 = leftMotorForward;
  _dir_Motor2 = rightMotorForward;
  // Set motor directions
  digitalWrite(PIN_DIR_MOTOR1, leftMotorForward ? HIGH : LOW, "left dir ");
  digitalWrite(PIN_DIR_MOTOR2, rightMotorForward ? LOW : HIGH, "right dir ");

  // Set motor speeds
  analogWrite(PIN_PWM_MOTOR1, abs(leftMotorSpeed), "left speed");
  analogWrite(PIN_PWM_MOTOR2, abs(rightMotorSpeed), "right speed");
}

void READ_RC1() {
  Read_Input(0, RC_CH1_INPUT);
}

void READ_RC2() {
  Read_Input(1, RC_CH2_INPUT);
}

void Read_Input(uint8_t channel, uint8_t input_pin) {
  if (digitalRead(input_pin) == HIGH) {
    RC_START[channel] = micros();
  } else {
    uint16_t rc_compare = (uint16_t)(micros() - RC_START[channel]);
    RC_SHARED[channel] = rc_compare;
  }
}


//...
void rc_read_values() {

int reading;
reading = analogRead(A0);
reading = map(reading, 0, 1023, 1050, 1950);

RC_VALUES[0] = reading;

reading = analogRead(A1);
reading = map(reading, 0, 1023, 1050, 1950);

RC_VALUES[1] = reading;

}
/*


  noInterrupts();
  memcpy(RC_VALUES, (const void *)RC_SHARED, sizeof(RC_SHARED));
  interrupts();
}
*/

void rc_invert_values() {
  for (int i = 0; i < RC_NUM_CHANNELS; i++) {
    if (RC_INVERT[i] == 1) {
      RC_VALUES[i] = (RC_HIGH[i] + RC_LOW[i]) - RC_VALUES[i];
    }
    if (RC_VALUES[i] > RC_HIGH[i]) {
      RC_VALUES[i] = RC_HIGH[i];
    }
    if (RC_VALUES[i] < RC_LOW[i]) {
      RC_VALUES[i] = RC_LOW[i];
    }
  }
}

void rc_translate_values() {
  for (int i = 0; i < RC_NUM_CHANNELS; i++) {
    RC_TRANSLATED_VALUES[i] = translateValueIntoNewRange((float)RC_VALUES[i], (float)RC_HIGH[i], (float)RC_LOW[i], RC_TRANSLATED_LOW[i], RC_TRANSLATED_HIGH[i]);
  }
}

void rc_deadzone_adjust() {
  for (int i = 0; i < RC_NUM_CHANNELS; i++) {
    float newval = 0;
    if (RC_CHANNEL_MODE[i] == 0) {
      newval = translateValueIntoNewRange((float)RC_VALUES[i], (float)RC_HIGH[i], (float)RC_LOW[i], 50.0, -50.0);
      if (abs(newval) < RC_DZPERCENT[i]) {
        RC_VALUES[i] = RC_MID[i];
      }
    } else if (RC_CHANNEL_MODE[i] == 1) {
      newval = translateValueIntoNewRange((float)RC_VALUES[i], (float)RC_HIGH[i], (float)RC_LOW[i], 50.0, 0.0);
      if (abs(newval) < RC_DZPERCENT[i]) {
        RC_VALUES[i] = RC_LOW[i];
      }
    }
  }
}

float translateValueIntoNewRange(float currentvalue, float currentmax, float currentmin, float newmax, float newmin) {
  return (((currentvalue - currentmin) * (newmax - newmin)) / (currentmax - currentmin)) + newmin;
}