#include "math.h"
#include "turret.h"

double degToRad(double deg) { return deg * M_PI / 180.0; }

double radToDeg(double rad) { return rad * 180.0 / M_PI; }

void angleToStep(Turret::StepRatio stepRatio, vec2<double> angle, vec2<long> &step) {
  step.x = angle.x * stepRatio.x;
  step.y = angle.y * stepRatio.y;
}

void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset, vec2<long> zero, vec3<double> position, vec2<long> &step) {
  // x = -x; // natural axis direction
  position.y = -position.y; // natural axis direction
  double dX = position.x + offset.x;
  double dY = position.y + offset.y;
  double dZ = position.z + offset.z;

  vec2<double> angle(
    atan2(dZ, dY) - M_PI / 2,
    atan2(dZ, dX) - M_PI / 2
                    );

  angleToStep(stepRatio, angle, step);
}