#include "./motor.h"
#include "include/config.h"
#include "./debug.h"
#include <Arduino.h>


void motorInit()
{
  pinMode(Config::MotorPinSleep, OUTPUT);
  digitalWrite(Config::MotorPinSleep, LOW);

  pinMode(Config::MotorPinPWM, OUTPUT);
  digitalWrite(Config::MotorPinPWM, LOW);

  pinMode(Config::MotorPinDirection, OUTPUT);
  pinMode(Config::MotorPinCurrentSensing, INPUT);

  // See motorIsOvercurrent
  analogReference(INTERNAL);
  delay(10);
}


void motorStart(MotorDirection direction)
{
  //dl("[ MOTOR    ] Starting in direction: "); dln((uint8_t)direction);

  digitalWrite(Config::MotorPinDirection, direction == MotorDirection::Up ? HIGH : LOW);
  digitalWrite(Config::MotorPinSleep, HIGH);
  digitalWrite(Config::MotorPinPWM, HIGH);
}


void motorStop()
{
  //dln("[ MOTOR    ] Stopping");

  digitalWrite(Config::MotorPinPWM, LOW);
  digitalWrite(Config::MotorPinSleep, LOW);
  digitalWrite(Config::MotorPinDirection, LOW);
}


bool motorIsOvercurrent()
{
  // The Polugu G2 module outputs 20 mV/A plus a 50 mV offset
  // The analog reference has been set to internal, which is 1.1v for the ATMega328P,
  // divided by 1024 units is 1.074mV per unit. This means we can measure up to 52.5A.
  // I sure hope we never reach that value :-)
  auto value = analogRead(Config::MotorPinCurrentSensing);
  float voltage = (float)value * (1100.0 / 1024.0);
  float current = (voltage - 50) / 20;

  dl("[ MOTOR    ] Measured current: value = "); dl(value); dl(", V = "); dl(voltage); dl(", A = "); dln(current);

  return current >= Config::MotorCurrentLimit;
}