#include "math.h"

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

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

void angleToStep(double stepRatioX, double stepRatioY, long &stepX, long &stepY,
                 double angleX, double angleY) {
  stepX = angleX * stepRatioX;
  stepY = angleY * stepRatioY;
}

void cartesianToPolar(double stepRatioX, double stepRatioY, double offsetX,
                      double offsetY, double offsetZ, long &stepX, long &stepY,
                      double zeroX, double zeroY, double x, double y,
                      double z) {
  // x = -x; // natural axis direction
  y = -y; // natural axis direction
  double dX = x + offsetX;
  double dY = y + offsetY;
  double dZ = z + offsetZ;

  double angleY = atan2(dZ, dY) - M_PI / 2;
  double angleX = atan2(dZ, dX) - M_PI / 2;

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