#include "math.h"

// rollH atan((22.5 - 18.5) / 236) ~0.97°
// rollV ~1°

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

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

void angleToStep(long &stepX, long &stepY, double angleX, double angleY) {
  // TODO remove magic numbers
  double stepRatioX = M_PI / ((51.2 / 15.0) * (100.0 * 64.0));
  double stepRatioY = M_PI / ((48.0 / 15.0) * (100.0 * 64.0));

  stepX = angleX / stepRatioX;
  stepY = angleY / stepRatioY;
}

double rollCorrection(double x, double max, double width) {
  double correction = max * (1.0 - cos((10.0 * x) / (width * M_PI))) / 2.0;
  return x - correction;
}

void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
                      double x, double y, double z) {
  // TODO remove magic numbers
  double distance = 166.0; // turret to screen
  double offsetY = 21.0;   // laser to stand
  double offsetX = 0.0;    // unused

  x = -x; // natural axis direction
  double dX = x + offsetX;
  double dY = rollCorrection(y + offsetY, 0.2, 400.0);
  double dZ = z + distance;

  double roll = atan2(4.0, 236.0);
  double rollY = dX * tan(roll);
  double rollX = dY * tan(roll);

  double dXr = dX + rollX;
  double dYr = dY + rollY;

  double rho = sqrt(pow(dXr, 2.0) + pow(dYr, 2.0) + pow(dZ, 2.0));
  double angleY = M_PI / 2.0 - acos(dYr / rho);
  double angleX = atan2(dXr, dZ);

  angleToStep(stepX, stepY, angleX, angleY);
}