#include "motors.h"

bool removeArmMomentum = false;

Servo bucketServo;
Servo auxServo;

std::vector<MOTOR_PINS> motorPins =
    {
        {RMOTOR_IN1, RMOTOR_IN2}, // RIGHT_MOTOR Pins (IN1, IN2)
        {LMOTOR_IN1, LMOTOR_IN2}, // LEFT_MOTOR  Pins
        {ARM_IN1,    ARM_IN2}, //ARM_MOTOR pins
};

#define NUMMOTORS 3
motor motors[NUMMOTORS];

void setupmotors()
{

  for (int i = 0; i < NUMMOTORS; i++)
  {
    motors[i].init(motorPins[i].pinIN1, motorPins[i].pinIN2, i+2, false); // pinMode(motorPins[i].pinIN1, OUTPUT);
    motors[i].stop();
    motors[i].setindex(i);
    log_i("motor(%d) initialized", i);
    delay(50);
  }
  bucketServo.attach(bucketServoPin);
  bucketServo.write(default_bucketTilt);
  //auxServo.attach(auxServoPin);
  //auxControl(default_auxControl);
}

void drivemotor(uint8_t num, int dirspeed)
{
  if (num <= motorPins.size())
  {
    if(dirspeed == 0)
    {
      motors[num].stop();
      log_v("stop");
      return;
    }
    else
    {
      //uint8_t speed = map(dirspeed, -330, 330, -100, 100);
      log_i("drive motor %u, speed %d", num, dirspeed);
      motors[num].move(dirspeed);
      motors[num].prevsetpoint = dirspeed;
    }
  }
  else
  {
    log_e("invalid motor num %d", num);
  }
}

int prevBucketAngle = 0;
uint32_t lastbuckettimer = 0;
void bucketTilt(int bucketServoValue)
{
  if(millis()- lastbuckettimer > BUCKETTIMER)
  {
    log_i("bucket command received = %i", bucketServoValue);
    int servo = 0;
    if(bucketServoValue == 0)
    {
      log_v("bucket stop");
      return;
    }
    else if(bucketServoValue > 0)
    {
      servo = prevBucketAngle + SERVOSTEP;
    }
    else
    {
      servo = prevBucketAngle - SERVOSTEP;
    }

    if(servo > SERVOMAX) servo = SERVOMAX;
    if(servo < SERVOMIN) servo = SERVOMIN;

    bucketServo.write(servo);
    log_i("bucket (%i) angle=(%d)", bucketServoValue, servo);

    lastbuckettimer = millis();
    prevBucketAngle = servo;
  }
}

void AbsBucketTilt(int bucketServoValue)
{
  log_i("drive bucket servo, angle  %d", bucketServoValue);
  delay(5);
  bucketServo.write(bucketServoValue);
}

void auxControl(int auxServoValue)
{
  log_i("drive aux servo, angle  %d", auxServoValue);
  delay(5);
  auxServo.write(auxServoValue);
}
