#include "ServoDriver.h"
#include "Config.h"

// 18 servos on hardware PWM, the way the old robot sketch drove them (the
// Teensy 3.6 Servo library stops at 12). 16-bit duty at 50 Hz is ~0.3 us per
// step. Every servo pin must be PWM-capable - on the Teensy 3.6 all of the
// SERVOS[] pins are.
static constexpr uint8_t PWM_BITS = 16;
static constexpr float PERIOD_US = 1e6f / SERVO_PWM_HZ;

static void writePulse(uint8_t pin, float us) {
  analogWrite(pin, (int)(us / PERIOD_US * (1 << PWM_BITS) + 0.5f));
}

void ServoDriver::begin() {
  analogWriteResolution(PWM_BITS);
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++) {
      analogWriteFrequency(SERVOS[i][j].pin, SERVO_PWM_HZ);
      pinMode(SERVOS[i][j].pin, OUTPUT);
      analogWrite(SERVOS[i][j].pin, 0);   // no pulses until the first writeLeg()
    }
}

void ServoDriver::relaxAll() {
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++)
      analogWrite(SERVOS[i][j].pin, 0);   // no pulses: the servos go limp
}

int ServoDriver::degToMicros(int leg, int joint, float deg) {
  const ServoCal& c = SERVOS[leg][joint];
  float us = c.us_at_0deg + c.us_per_deg * deg;
  return (int)constrain(us, (float)c.us_min, (float)c.us_max);
}

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