// #define IN1 18#define IN2 19   #define ENA 23   #define IN3 25    #define IN4 26#define ENB 22

//pin motor driver......./*IN1,IN2,IN3,IN4*/
const uint8_t INpin[4] = { 18, 19, 25, 26 };
const uint8_t ENpin[2] = { 23, 22 };
uint8_t previousPower = 0;

// Definisikan array dua dimensi untuk nilai HIGH/LOW untuk setiap arah gerakan
const int HighLow[5][4] = {
  { LOW, LOW, LOW, LOW },    // Stop
  { HIGH, LOW, HIGH, LOW },  // forward
  { LOW, HIGH, LOW, HIGH },  // backward
  { HIGH, LOW, LOW, HIGH },  // right
  { LOW, HIGH, HIGH, LOW }   // left
};

uint8_t arah(String perintah) {
  if (perintah == "stop") {
    return 0;
  } else if (perintah == "maju") {
    return 1;
  } else if (perintah == "mundur") {
    return 2;
  } else if (perintah == "kanan") {
    return 3;
  } else if (perintah == "kiri") {
    return 4;
  }
}

uint8_t power(int powerPercentage) {
  // Berfungsi untuk mengubah persentase daya menjadi nilai PWM
  return map(powerPercentage, 0, 100, 0, 255);
}

void move(String arahgerak, uint8_t powerPercen, uint16_t durasi_ms) {
  for (uint8_t i = 0; i < 4; i++) {
    digitalWrite(INpin[i], HighLow[arah(arahgerak)][i]);
  }
  
  int8_t step = (previousPower < powerPercen) ? 1 : -1;
  uint8_t iterations = abs(powerPercen - previousPower) + 1;
  uint8_t delayPerIteration = durasi_ms / iterations;
  Serial.printf("%s %d%% %d>%d\t{", arahgerak, powerPercen, durasi_ms, delayPerIteration);

  for (int8_t p = previousPower; p != powerPercen + step; p += step) {
    analogWrite(ENpin[0], power(p));
    analogWrite(ENpin[1], power(p));
    Serial.print("-");
    Serial.print(p);
    delay(delayPerIteration);  // Adjusted delay to meet totalTimeMs
  }
  Serial.print("}\n\n");
  previousPower = powerPercen;
}

void motordriverSetup() {
  // Set pin modes for Motor 1
  for (uint8_t i = 0; i < 4; i++) {
    pinMode(INpin[i], OUTPUT);
  }
  pinMode(ENpin[0], OUTPUT);
  pinMode(ENpin[1], OUTPUT);
  // Initialize motors to stop
  move("stop", 0, 0);
}
//-------------------------------------------------------------//
void setup() {
  // Initialize Serial Monitor
  Serial.begin(115200);
  motordriverSetup();
}
//------------------------------------------------------------//

void loop() {
  move("maju", random(10,100), 1000);
  delay(2000);
  move("stop", 0, 0);
  delay(1000);

  move("mundur", 50, 1500);
  delay(2000);
  move("stop", 0, 10);
  delay(1000);

  move("kanan", 50, 1000);
  delay(2000);
  move("stop", 0, 100);
  delay(1000);

  move("kiri", 50, 1000);
  delay(2000);
  move("stop", 0, 100);
  delay(1000);
}