#include <Arduino.h>
#include "config.h"
#include "actuators.h"

bool actuatorsInit() {
  pinMode(PIN_LB, OUTPUT);
  pinMode(PIN_LF, OUTPUT);
  pinMode(PIN_RB, OUTPUT);
  pinMode(PIN_RF, OUTPUT);

  ledcSetup(PWM_CH_L, PWM_FREQ, PWM_RES);
  ledcSetup(PWM_CH_R, PWM_FREQ, PWM_RES);

  ledcAttachPin(L_PWM_PIN, PWM_CH_L);
  ledcAttachPin(R_PWM_PIN, PWM_CH_R);

  Serial.println("Actuators initialized");
  Serial.printf("  L_PWM → GPIO%d\n", L_PWM_PIN);
  Serial.printf("  R_PWM → GPIO%d\n", R_PWM_PIN);
  Serial.printf("  L_DIR → GPIO%d, %d\n", PIN_LB, PIN_LF);
  Serial.printf("  R_DIR → GPIO%d, %d\n", PIN_RB, PIN_RF);
  
  actuatorsSetSpeed(150, 150);
  actuatorsStop();
  return true;
}

void actuatorsStop() {
  digitalWrite(PIN_LB, HIGH);
  digitalWrite(PIN_LF, HIGH);
  digitalWrite(PIN_RB, HIGH);
  digitalWrite(PIN_RF, HIGH);
}

void actuatorsForward() {
  digitalWrite(PIN_LB, LOW);
  digitalWrite(PIN_LF, HIGH);
  digitalWrite(PIN_RB, LOW);
  digitalWrite(PIN_RF, HIGH);
}

void actuatorsBack() {
  digitalWrite(PIN_LB, HIGH);
  digitalWrite(PIN_LF, LOW);
  digitalWrite(PIN_RB, HIGH);
  digitalWrite(PIN_RF, LOW);
}

void actuatorsLeft() {
  digitalWrite(PIN_LB, HIGH);
  digitalWrite(PIN_LF, LOW);
  digitalWrite(PIN_RB, LOW);
  digitalWrite(PIN_RF, HIGH);
}

void actuatorsRight() {
  digitalWrite(PIN_LB, LOW);
  digitalWrite(PIN_LF, HIGH);
  digitalWrite(PIN_RB, HIGH);
  digitalWrite(PIN_RF, LOW);
}

void actuatorsSetSpeed(int l, int r) {
  ledcWrite(PWM_CH_L, constrain(l, 0, 255));
  ledcWrite(PWM_CH_R, constrain(r, 0, 255));
}