#include "MotorControl.h"
#include <Arduino.h>

constexpr int PORT_FORWARD = 4;
constexpr int PORT_BACKWARD = 16;
constexpr int PORT_SPEED = 5;

// PWM settings
constexpr int PWM_FREQ = 20000;
constexpr int PWM_CHANNEL = 0;
constexpr int PWM_RESOLUTION = 255;

void MotorControl::begin()
{
  pinMode(PORT_SPEED, OUTPUT);
  pinMode(PORT_FORWARD, OUTPUT);
  pinMode(PORT_BACKWARD, OUTPUT);

  analogWriteRange(PWM_RESOLUTION);
  analogWriteFreq(PWM_FREQ);
  startTime_ = millis();
}

void MotorControl::addCmd(float signedSpeed, unsigned long duration)
{
  if (numCmds_ < MAX_MOTOR_COMMANDS)
    motorCommands_[numCmds_++] = {int(signedSpeed * PWM_RESOLUTION), duration};
  else
    Serial.println("Too many commands");
}

float MotorControl::speed(int cmdIdx) const
{
  return float(motorCommands_[cmdIdx].directionAndSpeed) / float(PWM_RESOLUTION);
}

unsigned long MotorControl::duration(int cmdIdx) const
{
  return motorCommands_[cmdIdx].delayToNextCmdInMs;
}

void MotorControl::iteration()
{
  if (numCmds_ == 0)
  {
    analogWrite(PORT_SPEED, 0);
    return;
  }

  unsigned long timeElapsed = millis() - startTime_;
  while (timeElapsed > motorCommands_[currentCmd_].delayToNextCmdInMs)
  {
    const auto curCmdDuration = motorCommands_[currentCmd_].delayToNextCmdInMs;
    timeElapsed -= curCmdDuration;
    startTime_ += curCmdDuration;
    currentCmd_ = (currentCmd_ + 1) % numCmds_;
  }

  const auto &curCmd = motorCommands_[currentCmd_];
  const auto &nextCmd = motorCommands_[(currentCmd_ + 1) % numCmds_];
  const float fraction = float(timeElapsed) / float(curCmd.delayToNextCmdInMs);
  const int interpolated = int((1.f - fraction) * curCmd.directionAndSpeed + fraction * nextCmd.directionAndSpeed);

  if (interpolated < 0)
  {
    analogWrite(PORT_SPEED, -interpolated);
    digitalWrite(PORT_FORWARD, 0);
    digitalWrite(PORT_BACKWARD, 1);
  }
  else
  {
    analogWrite(PORT_SPEED, interpolated);
    digitalWrite(PORT_FORWARD, 1);
    digitalWrite(PORT_BACKWARD, 0);
  }
}