#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 radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
               vec2<long> &step) {
  step.x = angle.x * stepRatio.x;
  step.y = angle.y * stepRatio.y;
}

void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
               vec2<double> &angle) {
  angle.x = step.x / stepRatio.x;
  angle.y = step.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);

  radToStep(stepRatio, angle, step);
}

void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
                      vec3<long> current, vec2<long> zero, vec2<long> step,
                      vec3<double> &position) {

  vec2<double> angle;
  stepToRad(stepRatio, step, angle);

  vec2<double> dXYZ(tan(angle.x) * current.z, tan(angle.y) * current.z);

  position.x = dXYZ.x - offset.x;
  position.y = -(dXYZ.y - offset.y); // natural axis direction
  position.z = current.z - offset.z;
}