#include "ServoDriver.h"
#include "Config.h"
#include <Servo.h>

static Servo servos[NUM_LEGS][3];

void ServoDriver::begin() {
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++)
      servos[i][j].attach(SERVO_PINS[i][j]);
}

void ServoDriver::relaxAll() {
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++)
      servos[i][j].detach();
}

static int degToMicros(float deg) {
  return SERVO_US_AT_0DEG + (int)(deg * SERVO_US_PER_DEG);
}

void ServoDriver::writeLeg(int i, const JointAngles& a) {
  if (!a.valid) return;   // never write an out-of-range or unreachable solution
  servos[i][0].writeMicroseconds(degToMicros(a.coxa_deg));
  servos[i][1].writeMicroseconds(degToMicros(a.femur_deg));
  servos[i][2].writeMicroseconds(degToMicros(a.tibia_deg));
}