From cbb17e279fcabe56b763662bb4670ddf0c3c442b Mon Sep 17 00:00:00 2001
From: Julien Oculi <xdrz.jot.sr@gmail.com>
Date: Thu, 12 Jun 2025 16:54:28 +0200
Subject: [PATCH] feat: implement getter for turret `home` `zero` and
 `position`

---
 maths.cpp  |  20 +++++++++
 maths.h    |   5 +++
 turret.cpp | 117 ++++++++++++++++++++++++++++++++++++++++++++++++++---
 turret.h   |   4 +-
 4 files changed, 139 insertions(+), 7 deletions(-)

diff --git a/maths.cpp b/maths.cpp
index 6214c64..2a2159b 100644
--- a/maths.cpp
+++ b/maths.cpp
@@ -11,6 +11,12 @@ void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
   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) {
@@ -24,3 +30,17 @@ void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
 
   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;
+}
diff --git a/maths.h b/maths.h
index 4724c71..2b3c72a 100644
--- a/maths.h
+++ b/maths.h
@@ -7,7 +7,12 @@ double degToRad(double deg);
 double radToDeg(double rad);
 void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
                vec2<long> &step);
+void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
+               vec2<double> &angle);
 void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
                       vec2<long> zero, vec3<double> position, vec2<long> &step);
+void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
+                      vec3<long> current, vec2<long> zero, vec2<long> step,
+                      vec3<double> &position);
 
 #endif
diff --git a/turret.cpp b/turret.cpp
index 6c71b01..0c6881c 100644
--- a/turret.cpp
+++ b/turret.cpp
@@ -210,17 +210,124 @@ Turret &Turret::moveByY(double y, Unit unit) { return moveTo(0, y, 0, unit); }
 Turret &Turret::moveByZ(double z, Unit unit) { return moveTo(0, 0, z, unit); }
 
 Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) {
-  // TODO implement
+  if (unit == Unit::MM) {
+    vec3<double> position;
+    vec2<long> xy(_current.x, _current.y);
+    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
+    x = position.x / 10;
+    y = position.y / 10;
+    z = position.z / 10;
+  }
+
+  if (unit == Unit::CM) {
+    vec3<double> position;
+    vec2<long> xy(_current.x, _current.y);
+    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
+    x = position.x;
+    y = position.y;
+    z = position.z;
+  }
+
+  if (unit == Unit::M) {
+    vec3<double> position;
+    vec2<long> xy(_current.x, _current.y);
+    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
+    x = position.x * 100;
+    y = position.y * 100;
+    z = position.z * 100;
+  }
+
+  if (unit == Unit::STEP) {
+    x = _home.x;
+    y = _home.y;
+  }
+
+  if (unit == Unit::RAD) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = angle.x;
+    y = angle.y;
+  }
+
+  if (unit == Unit::DEG) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = radToDeg(angle.x);
+    y = radToDeg(angle.y);
+  }
+
   return *this;
 }
 
-Turret &Turret::getHome(double &x, double &y, double &z, Unit unit) {
-  // TODO implement
+Turret &Turret::getHome(double &x, double &y, Unit unit) {
+  if (unit == Unit::MM) {
+    vec3<double> position;
+    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
+    x = position.x / 10;
+    y = position.y / 10;
+  }
+
+  if (unit == Unit::CM) {
+    vec3<double> position;
+    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
+    x = position.x;
+    y = position.y;
+  }
+
+  if (unit == Unit::M) {
+    vec3<double> position;
+    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
+    x = position.x * 100;
+    y = position.y * 100;
+  }
+
+  if (unit == Unit::STEP) {
+    x = _home.x;
+    y = _home.y;
+  }
+
+  if (unit == Unit::RAD) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = angle.x;
+    y = angle.y;
+  }
+
+  if (unit == Unit::DEG) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = radToDeg(angle.x);
+    y = radToDeg(angle.y);
+  }
+
   return *this;
 }
 
-Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) {
-  // TODO implement
+Turret &Turret::getZero(double &x, double &y, Unit unit) {
+  if ((unit == Unit::MM) || (unit == Unit::CM) || (unit == Unit::MM)) {
+    x = 0;
+    y = 0;
+  }
+
+  if (unit == Unit::STEP) {
+    x = _zero.x;
+    y = _zero.y;
+  }
+
+  if (unit == Unit::RAD) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _zero, angle);
+    x = angle.x;
+    y = angle.y;
+  }
+
+  if (unit == Unit::DEG) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _zero, angle);
+    x = radToDeg(angle.x);
+    y = radToDeg(angle.y);
+  }
+
   return *this;
 }
 
diff --git a/turret.h b/turret.h
index fed92cb..c73489e 100644
--- a/turret.h
+++ b/turret.h
@@ -60,8 +60,8 @@ public:
   Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
 
   Turret &getPosition(double &x, double &y, double &z, Unit unit);
-  Turret &getHome(double &x, double &y, double &z, Unit unit);
-  Turret &getZero(double &x, double &y, double &z, Unit unit);
+  Turret &getHome(double &x, double &y, Unit unit);
+  Turret &getZero(double &x, double &y, Unit unit);
 
   Turret &calibrate();