#include "DRV8833.h"

#include <Arduino.h>

namespace RoboidControl {
namespace Arduino {

DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
    : Thing(participant) {
  this->pinIn1 = pinIn1;
  this->pinIn2 = pinIn2;

#if (ESP32)
  in1Ch = nextAvailablePwmChannel++;
  ledcSetup(in1Ch, 500, 8);
  ledcAttachPin(pinIn1, in1Ch);
  in2Ch = 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::SetAngularVelocity(Spherical velocity) {
  Thing::SetAngularVelocity(velocity);
  // ignoring rotation axis for now.
  // Spherical angularVelocity = this->GetAngularVelocity();
  float angularSpeed = velocity.distance;  // in degrees/sec

  float rpm = angularSpeed / 360 * 60;
  float motorSpeed = rpm / this->maxRpm;

  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
  // if (direction == RotationDirection::CounterClockwise)
  //   speed = -speed;
  // Determine the rotation direction
  if (velocity.direction.horizontal.InDegrees() < 0)
    motorSpeed = -motorSpeed;
  if (this->reverse)
    motorSpeed = -motorSpeed;

  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
  //           << ", 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
}

DRV8833::DRV8833(Participant* participant,
                 unsigned char pinAIn1,
                 unsigned char pinAIn2,
                 unsigned char pinBIn1,
                 unsigned char pinBIn2,
                 unsigned char pinStandby,
                 bool reverseA,
                 bool reverseB)
    : Thing(participant) {
  this->pinStandby = pinStandby;
  if (pinStandby != 255)
    pinMode(pinStandby, OUTPUT);

  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
  this->motorA->name = "Motor A";
  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
  this->motorB->name = "Motor B";
}

}  // namespace Arduino
}  // namespace RoboidControl