#include "DRV8833.h"

#include <Arduino.h>

namespace RoboidControl {
namespace Arduino {

#pragma region DRV8833

DRV8833::DRV8833(Configuration config, Participant* participant)
    : Thing(participant) {
  this->pinStandby = pinStandby;
  if (pinStandby != 255)
    pinMode(pinStandby, OUTPUT);

  this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
  this->motorA->SetName("Motor A");
  this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
  this->motorB->SetName("Motor B");
}

DRV8833::DRV8833(Configuration config, Thing* parent)
    : DRV8833(config, parent->owner) {
  this->SetParent(parent);
}

#pragma endregion DRV8833

#pragma region Differential drive

DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
                                              Participant* owner)
    : RoboidControl::DifferentialDrive(owner), drv8833(config, owner) {
  this->leftWheel = this->drv8833.motorA;
  this->rightWheel = this->drv8833.motorB;
}

DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
                                              Thing& parent)
    : RoboidControl::DifferentialDrive(parent), drv8833(config) {}

void DRV8833::DifferentialDrive::Update(bool recurse) {
  RoboidControl::DifferentialDrive::Update(recurse);
  this->drv8833.Update(recurse);
}

#pragma endregion Differential drive

#pragma region Motor

#if (ESP32)
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
#endif

DRV8833Motor::DRV8833Motor(DRV8833* driver,
                           unsigned char pinIn1,
                           unsigned char pinIn2,
                           bool reverse)
    : Motor(driver->owner) {
  this->SetParent(driver);

  this->pinIn1 = pinIn1;
  this->pinIn2 = pinIn2;

#if (ESP32)
  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
  ledcSetup(in1Ch, 500, 8);
  ledcAttachPin(pinIn1, in1Ch);

  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
  ledcSetup(in2Ch, 500, 8);
  ledcAttachPin(pinIn2, in2Ch);

#else
  pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
  pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
#endif

  // this->reverse = reverse;
}

// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
//   this->maxRpm = rpm;
// }

void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
  Motor::SetTargetVelocity(motorSpeed);

  uint8_t motorSignal =
      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);

  // std::cout << "moto speed " << this->name << " = " << motorSpeed
  //           << ", motor signal = " << (int)motorSignal << "\n";

#if (ESP32)
  if (motorSpeed == 0) {  // stop
    ledcWrite(in1Ch, 0);
    ledcWrite(in2Ch, 0);

  } else if (motorSpeed > 0) {  // forward
#if FAST_DECAY
    ledcWrite(in1Ch, motorSignal);
    ledcWrite(in2Ch, 0);
#else
    ledcWrite(in1Ch, 255);
    ledcWrite(in2Ch, 255 - motorSignal);
#endif
  } else {  // (motorSpeed < 0) reverse
#if FAST_DECAY
    ledcWrite(in1Ch, 0);
    ledcWrite(in2Ch, motorSignal);
#else
    ledcWrite(in1Ch, 255 - motorSignal);
    ledcWrite(in2Ch, 255);
#endif
  }
#else  // not ESP32
  if (motorSpeed == 0) {  // stop
    analogWrite(pinIn1, 0);
    analogWrite(pinIn2, 0);

  } else if (motorSpeed > 0) {  // forward
#if FAST_DECAY
    analogWrite(pinIn1, motorSignal);
    analogWrite(pinIn2, 0);
#else
    analogWrite(pinIn1, 255);
    analogWrite(pinIn2, 255 - motorSignal);
#endif
  } else {  // (motorSpeed < 0) reverse
#if FAST_DECAY
    analogWrite(pinIn1, 0);
    analogWrite(pinIn2, motorSignal);
#else
    analogWrite(pinIn1, 255 - motorSignal);
    analogWrite(pinIn2, 255);
#endif
  }
#endif
}

#pragma endregion Motor

}  // namespace Arduino
}  // namespace RoboidControl