diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp
index 12feee8..55b48f9 100755
--- a/MotionCore/Arm.cpp
+++ b/MotionCore/Arm.cpp
@@ -8,10 +8,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 		Vector3d::Y,
 		Vector3d::Up,
 		100.0, //mm
-		0.0,
-		125.0,
-		30.0,
-		180.0,
+		0,
+		186.0,
+		10.0,
+		270.0,
 		0.0,
 		nullptr
 	),
@@ -20,10 +20,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 		Vector3d::X,
 		Vector3d::Forward,
 		72.0, //mm
-		1.0,
-		48.0,
+		1,
+		71.0,
 		0.0,
-		120.0,
+		181.0,
 		0.0,
 		&j0
 	),
@@ -32,10 +32,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 		Vector3d::X,
 		Vector3d::Forward,
 		95.0, //mm
-		2.0,
-		115.0,
+		2,
+		171.0,
 		0.0,
-		180.0,
+		280.0,
 		0.0,
 		&j1
 	)
@@ -61,21 +61,6 @@ bool Arm::assumeHomePose()
 	return true;
 }
 
-bool Arm::moveJ0(double theta)
-{
-	return j0.moveTo(theta, std::nullopt);
-}
-
-bool Arm::moveJ1(double theta)
-{
-	return j1.moveTo(theta, std::nullopt);
-}
-
-bool Arm::moveJ2(double theta)
-{
-	return j2.moveTo(theta, std::nullopt);
-}
-
 Arm::~Arm()
 {
 	oSerialBus.close();
diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h
index 65db410..895a5b4 100755
--- a/MotionCore/Arm.h
+++ b/MotionCore/Arm.h
@@ -9,13 +9,18 @@ public:
 	Arm(const Arm& other) = delete;
 	Arm(Arm&& other) noexcept = default;
 	~Arm();
-	
+
 	bool assumeHomePoseImmediate();
 	bool assumeHomePose();
 
-	bool moveJ0(double theta);
-	bool moveJ1(double theta);
-	bool moveJ2(double theta);
+	ArmSegment& getJ0() { return j0; };
+	const ArmSegment& getJ0() const { return j0; };
+
+	ArmSegment& getJ1() { return j1; };
+	const ArmSegment& getJ1() const { return j1; };
+
+	ArmSegment& getJ2() { return j2; };
+	const ArmSegment& getJ2() const { return j2; };
 
 	void update(double frametime);
 
diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp
index 79453e7..3692fa8 100755
--- a/MotionCore/ArmSegment.cpp
+++ b/MotionCore/ArmSegment.cpp
@@ -24,7 +24,7 @@ ArmSegment::ArmSegment(
 		virtualToMechanicalAngle(homeAngle),
 		mechanicalMinAngle,
 		mechanicalMaxAngle,
-		0.5
+		1.0
 	),
 	parent(parent)
 {
@@ -104,12 +104,12 @@ void ArmSegment::update(const double frametime)
 	motor.update(frametime);
 }
 
-double ArmSegment::virtualToMechanicalAngle(const double virtualAngle)
+double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) const noexcept
 {
 	return virtualAngle + mechanicalToVirtualAngleOffset;
 }
 
-double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle)
+double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle) const noexcept
 {
 	return hardwareAngle - mechanicalToVirtualAngleOffset;
 }
diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h
index cf48afe..5a37949 100755
--- a/MotionCore/ArmSegment.h
+++ b/MotionCore/ArmSegment.h
@@ -46,16 +46,16 @@ public:
 	void update(const double frametime);
 
 	void setSpeed(const double speed) noexcept { motor.setSpeed(speed); };
-	double getMechanicalMinAngle() const { return motor.getMechanicalMinPosition(); };
-	double getMechanicalMaxAngle() const { return motor.getMechanicalMaxPosition(); };
-	double getHomeANgle() const noexcept { return homeAngle; };
+	double getMinAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMinPosition()); };
+	double getMaxAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMaxPosition()); };
+	double getHomeAngle() const noexcept { return homeAngle; };
 	double getSpeed() const noexcept { return motor.getSpeed(); };
 	ArmSegment* getParent() { return parent; };
 	ArmSegment const* getParent() const { return parent; };
 
 private:
-	double virtualToMechanicalAngle(const double virtualAngle);
-	double mechanicalToVirtualAngle(const double hardwareAngle);
+	double virtualToMechanicalAngle(const double virtualAngle) const noexcept;
+	double mechanicalToVirtualAngle(const double hardwareAngle) const noexcept;
 
 	Motor motor;
 	Vector3d rotationAxis;
diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp
index f39117f..3fa9972 100755
--- a/MotionCore/MainLoop.cpp
+++ b/MotionCore/MainLoop.cpp
@@ -3,6 +3,11 @@
 #include <thread>
 #include <chrono>
 #include <Windows.h>
+#include <algorithm>
+
+// Bullshit macros from windows lib
+#undef max
+#undef min
 
 MainLoop::MainLoop() noexcept:
     arm(L"COM4")
@@ -11,6 +16,8 @@ MainLoop::MainLoop() noexcept:
         std::cerr << "Unable to move arm to home!" << std::endl;
         exit(-1);
     }
+
+    SetProcessDPIAware();
 }
 
 MainLoop& MainLoop::getInstance()
@@ -23,18 +30,16 @@ MainLoop::~MainLoop() noexcept
 {
 }
 
+static std::chrono::steady_clock::time_point lastDisplay =
+std::chrono::high_resolution_clock::now();
+
 void MainLoop::run()
 {
     static std::chrono::steady_clock::time_point lastBegin =
         std::chrono::high_resolution_clock::now();
 
-    double j1Position = 0.0;     // start at 0
-    const double speed = 1000.0;  // units per second
-
-    std::cout << "Main loop started\n";
-    std::cout << "Use UP and DOWN arrows to move J1\n";
-    std::cout << "Press ESC to exit\n\n";
-
+    const Vector3d armPos(0, 500, 2560);
+    
     while (isRunning)
     {
         std::chrono::steady_clock::time_point begin =
@@ -43,37 +48,54 @@ void MainLoop::run()
         const double frametime =
             (begin - lastBegin).count() * 10e-7; // milliseconds
 
-        // -------- INPUT --------
-        if (GetAsyncKeyState(VK_UP) & 0x8000)
+        POINT p;
+        if (GetCursorPos(&p))
         {
-            j1Position += speed * frametime;
-            arm.moveJ1(j1Position);
+            Vector3d cursor(
+                p.x,
+                -p.y + 1440.0,
+                0.0
+            );
+
+            // Vector from arm to cursor
+            Vector3d toCursor = cursor - armPos;
+            double vx = std::max(toCursor.x, 0.0);
+            double vy = toCursor.y;
+            double vz = toCursor.z; // will be negative (0 - 2560)
+
+            // Horizontal distance (XZ plane)
+            double horiz = std::sqrt(vx * vx + vz * vz);
+
+            // Yaw (Y): rotation in XZ plane
+            double yawRad = std::atan2(vx, vz);
+            double yawDeg = (yawRad * 180.0 / 3.14159) - 150;
+
+            // Pitch (X): rotation in vertical plane
+            double pitchRad = std::atan2(vy, horiz) * 3 - 1;
+            double pitchDegTotal = (pitchRad * 180.0 / 3.14159);
+            double pitchDegj1 = 90 - (pitchDegTotal / 2.0) - 45;
+            double pitchDegj2 = -(pitchDegTotal / 2.0) + 45;
+
+            std::cout << "cursor: " << cursor
+                << " yaw: " << yawDeg
+                << " pitch j1: " << pitchDegj1 << std::endl
+                << " pitch j2: " << pitchDegj2 << std::endl;
+
+            // Send yaw/pitch to your arm here...
+            // arm.setYaw(yawRad);
+            // arm.setPitch(pitchRad);
+            arm.getJ0().moveTo(yawDeg, std::nullopt);
+            arm.getJ1().moveTo(pitchDegj1, std::nullopt);
+            arm.getJ2().moveTo(pitchDegj2, std::nullopt);
         }
 
-        if (GetAsyncKeyState(VK_DOWN) & 0x8000)
-        {
-            j1Position -= speed * frametime;
-            arm.moveJ1(j1Position);
-        }
-
-        if (GetAsyncKeyState(VK_ESCAPE) & 0x8000)
-        {
-            std::cout << "\nExit requested\n";
+        if (GetAsyncKeyState(VK_ESCAPE)) {
             isRunning = false;
         }
 
         arm.update(frametime);
 
-        // -------- STATUS OUTPUT --------
-        std::cout << "\rJ1: " << std::fixed << std::setprecision(3)
-            << j1Position
-            << " | Frame time (ms): "
-            << std::setprecision(3)
-            << frametime
-            << std::flush;
 
         lastBegin = std::chrono::high_resolution_clock::now();
     }
-
-    std::cout << "\nMain loop stopped\n";
 }
diff --git a/MotionCore/Motor.cpp b/MotionCore/Motor.cpp
index ed6e10f..6b1a20d 100755
--- a/MotionCore/Motor.cpp
+++ b/MotionCore/Motor.cpp
@@ -4,7 +4,8 @@
 
 constexpr double RAMPUP_REDUCTION_FAC = 0.1;
 constexpr double RAMPUP_RECOVERY_SPEED = 0.1;
-constexpr double SPEED_CORRECTION_FAC = 0.01; // Otherwhise speed=1.0 would be way too fast
+constexpr double SPEED_CORRECTION_FAC = 100000.0;
+constexpr double MOVEMENT_COMPARISON_EPSILON = 0.01;
 
 Motor::Motor(
 	const uint8_t index,
@@ -55,10 +56,10 @@ bool Motor::moveTo(const double theta, const std::optional<double> speed) noexce
 		return false;
 	}
 
-	moveStartPosition = currentPosition;
 	targetPosition = theta;
+	moveStartPosition = currentPosition;
 
-	return false;
+	return true;
 }
 
 bool Motor::moveToImmediate(const double theta) noexcept
@@ -99,10 +100,10 @@ bool Motor::moveBy(const double theta, const std::optional<double> speed) noexce
 		return false;
 	}
 
-	moveStartPosition = moveStartPosition;
 	targetPosition = acuteTarget;
+	moveStartPosition = currentPosition;
 
-	return false;
+	return true;
 }
 
 bool Motor::moveByImmediate(const double theta) noexcept
@@ -200,16 +201,27 @@ bool Motor::isPositionInMechanicalRange(const double pos) const noexcept
 
 void Motor::handleMovement(const double frametime)
 {
-	constexpr double epsilon = 0.1;
-	if (std::abs(currentPosition - targetPosition) > epsilon) {
-		const double nextPositionStep = currentPosition + (targetPosition - moveStartPosition) * calculateCurrentSpeed() * SPEED_CORRECTION_FAC;
-		if (!isPositionInMechanicalRange(nextPositionStep)) {
-			return;
-		}
-		const bool result = sendMACommand(nextPositionStep);
-		if (result) {
-			currentPosition = nextPositionStep;
-		}
+	const double remaining = targetPosition - currentPosition;
+	if (std::abs(remaining) <= MOVEMENT_COMPARISON_EPSILON) {
+		currentPosition = targetPosition;
+		return;
+	}
+
+	const double speed = calculateCurrentSpeed();
+	const double maxStep = speed * frametime * SPEED_CORRECTION_FAC;
+	double step;
+
+	if (std::abs(remaining) <= std::abs(maxStep)) {
+		step = remaining;
+	}
+	else {
+		step = (remaining > 0 ? 1.0 : -1.0) * std::abs(maxStep);
+	}
+
+	const double nextPositionStep = std::clamp(currentPosition + step, mechanicalMinPosition, mechanicalMaxPosition);
+
+	if (sendMACommand(nextPositionStep)) {
+		currentPosition = nextPositionStep;
 	}
 }
 
@@ -223,14 +235,14 @@ bool Motor::sendMACommand(const double targetPos) noexcept
 double Motor::calculateCurrentSpeed() const noexcept
 {
 	if (targetPosition == moveStartPosition) {
-		return 0.0;
+		return speedLow;
 	}
 
 	auto smoothstep = [](double x) noexcept {
 		return x * x * (3.0 - 2.0 * x);
 	};
 
-	const double t = ((currentPosition - moveStartPosition) / (targetPosition - moveStartPosition));
+	const double t = getCurrentMovementProgress();
 	
 	// Accelleration
 	if (t < RAMPUP_RECOVERY_SPEED) {
@@ -244,3 +256,13 @@ double Motor::calculateCurrentSpeed() const noexcept
 		return targetSpeed;
 	}
 }
+
+double Motor::getCurrentMovementProgress() const noexcept
+{
+	return std::clamp((currentPosition - moveStartPosition) / (targetPosition - moveStartPosition), 0.0, 1.0);
+}
+
+bool Motor::isCurrentlyMoving() const noexcept
+{
+	return std::abs(currentPosition - targetPosition) > MOVEMENT_COMPARISON_EPSILON;
+}
diff --git a/MotionCore/Motor.h b/MotionCore/Motor.h
index 4b4838d..c7ff2f7 100755
--- a/MotionCore/Motor.h
+++ b/MotionCore/Motor.h
@@ -33,14 +33,14 @@ public:
 
 	void update(const double frametime);
 
-	std::size_t getIndex() const { return index; };
-	double getCurrentPosition() const { return currentPosition; };
-	double getTargetPosition() const { return targetPosition; };
-	double getSpeed() const { return targetSpeed; };
+	std::size_t getIndex() const noexcept { return index; };
+	double getCurrentPosition() const noexcept { return currentPosition; };
+	double getTargetPosition() const noexcept { return targetPosition; };
+	double getSpeed() const noexcept { return targetSpeed; };
 	void setSpeed(const double speedTarget) noexcept;
-	double getMechanicalMinPosition() const { return mechanicalMinPosition; };
-	double getMechanicalMaxPosition() const { return mechanicalMaxPosition; };
-	double getPaused() const { return paused; };
+	double getMechanicalMinPosition() const noexcept { return mechanicalMinPosition; };
+	double getMechanicalMaxPosition() const noexcept { return mechanicalMaxPosition; };
+	double getPaused() const noexcept { return paused; };
 	bool isPositionInMechanicalRange(const double pos) const noexcept;
 
 private:
@@ -50,6 +50,8 @@ private:
 	bool sendMACommand(const double targetPos) noexcept;
 	// Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition
 	double calculateCurrentSpeed() const noexcept;
+	double getCurrentMovementProgress() const noexcept;
+	bool isCurrentlyMoving() const noexcept;
 
 	const uint8_t index;
 	OSerialBus& serialBus;
diff --git a/MotionCore/Vector3d.cpp b/MotionCore/Vector3d.cpp
index 80df308..b1b60fd 100755
--- a/MotionCore/Vector3d.cpp
+++ b/MotionCore/Vector3d.cpp
@@ -13,17 +13,17 @@ const Vector3d Vector3d::X          { 1.0,  0.0,  0.0 };
 const Vector3d Vector3d::Y          { 0.0,  1.0,  0.0 };
 const Vector3d Vector3d::Z          { 0.0,  0.0,  1.0 };
 
-Vector3d::Vector3d()
+Vector3d::Vector3d() noexcept
     : x(0.0), y(0.0), z(0.0)
 {
 }
 
-Vector3d::Vector3d(double x, double y, double z)
+Vector3d::Vector3d(double x, double y, double z) noexcept
     : x(x), y(y), z(z)
 {
 }
 
-Vector3d& Vector3d::operator=(const Vector3d& other)
+Vector3d& Vector3d::operator=(const Vector3d& other) noexcept
 {
     if (this != &other)
     {
@@ -45,27 +45,27 @@ Vector3d& Vector3d::operator=(Vector3d&& other) noexcept
     return *this;
 }
 
-Vector3d Vector3d::operator+(const Vector3d& rhs) const
+Vector3d Vector3d::operator+(const Vector3d& rhs) const noexcept
 {
     return { x + rhs.x, y + rhs.y, z + rhs.z };
 }
 
-Vector3d Vector3d::operator-(const Vector3d& rhs) const
+Vector3d Vector3d::operator-(const Vector3d& rhs) const noexcept
 {
     return { x - rhs.x, y - rhs.y, z - rhs.z };
 }
 
-Vector3d Vector3d::operator*(double scalar) const
+Vector3d Vector3d::operator*(double scalar) const noexcept
 {
     return { x * scalar, y * scalar, z * scalar };
 }
 
-Vector3d Vector3d::operator/(double scalar) const
+Vector3d Vector3d::operator/(double scalar) const noexcept
 {
     return { x / scalar, y / scalar, z / scalar };
 }
 
-Vector3d& Vector3d::operator+=(const Vector3d& rhs)
+Vector3d& Vector3d::operator+=(const Vector3d& rhs) noexcept
 {
     x += rhs.x;
     y += rhs.y;
@@ -73,7 +73,7 @@ Vector3d& Vector3d::operator+=(const Vector3d& rhs)
     return *this;
 }
 
-Vector3d& Vector3d::operator-=(const Vector3d& rhs)
+Vector3d& Vector3d::operator-=(const Vector3d& rhs) noexcept
 {
     x -= rhs.x;
     y -= rhs.y;
@@ -81,7 +81,7 @@ Vector3d& Vector3d::operator-=(const Vector3d& rhs)
     return *this;
 }
 
-Vector3d& Vector3d::operator*=(double scalar)
+Vector3d& Vector3d::operator*=(double scalar) noexcept
 {
     x *= scalar;
     y *= scalar;
@@ -89,7 +89,7 @@ Vector3d& Vector3d::operator*=(double scalar)
     return *this;
 }
 
-Vector3d& Vector3d::operator/=(double scalar)
+Vector3d& Vector3d::operator/=(double scalar) noexcept
 {
     x /= scalar;
     y /= scalar;
@@ -97,12 +97,12 @@ Vector3d& Vector3d::operator/=(double scalar)
     return *this;
 }
 
-bool Vector3d::operator==(const Vector3d& rhs) const
+bool Vector3d::operator==(const Vector3d& rhs) const noexcept
 {
     return x == rhs.x && y == rhs.y && z == rhs.z;
 }
 
-bool Vector3d::operator!=(const Vector3d& rhs) const
+bool Vector3d::operator!=(const Vector3d& rhs) const noexcept
 {
     return !(*this == rhs);
 }
@@ -113,22 +113,22 @@ std::ostream& operator<<(std::ostream& os, const Vector3d& v)
     return os;
 }
 
-double Vector3d::dot(const Vector3d& rhs) const
+double Vector3d::dot(const Vector3d& rhs) const noexcept
 {
     return x * rhs.x + y * rhs.y + z * rhs.z;
 }
 
-double Vector3d::magnitude() const
+double Vector3d::magnitude() const noexcept
 {
     return std::sqrt(x * x + y * y + z * z);
 }
 
-double Vector3d::magnitudeSquared() const
+double Vector3d::magnitudeSquared() const noexcept
 {
     return x * x + y * y + z * z;
 }
 
-Vector3d Vector3d::cross(const Vector3d& rhs) const
+Vector3d Vector3d::cross(const Vector3d& rhs) const noexcept
 {
     return {
         y * rhs.z - z * rhs.y,
@@ -137,7 +137,7 @@ Vector3d Vector3d::cross(const Vector3d& rhs) const
     };
 }
 
-Vector3d Vector3d::normalized() const
+Vector3d Vector3d::normalized() const noexcept
 {
     double m = magnitude();
     if (m == 0.0)
@@ -147,7 +147,7 @@ Vector3d Vector3d::normalized() const
     return { x * inv, y * inv, z * inv };
 }
 
-void Vector3d::normalize()
+void Vector3d::normalize() noexcept
 {
     double m = magnitude();
     if (m == 0.0)
diff --git a/MotionCore/Vector3d.h b/MotionCore/Vector3d.h
index ce7e713..84ae078 100755
--- a/MotionCore/Vector3d.h
+++ b/MotionCore/Vector3d.h
@@ -10,37 +10,37 @@ public:
     double z;
 
 public:
-    Vector3d();
-    Vector3d(double x, double y, double z);
+    Vector3d() noexcept;
+    Vector3d(double x, double y, double z) noexcept;
 
     Vector3d(const Vector3d& other) = default;
     Vector3d(Vector3d&& other) noexcept = default;
 
 
-    double dot(const Vector3d& rhs) const;
-    double magnitude() const;
-    double magnitudeSquared() const;
-    Vector3d cross(const Vector3d& rhs) const;
-    Vector3d normalized() const;
-    void normalize();
+    double dot(const Vector3d& rhs) const noexcept;
+    double magnitude() const noexcept;
+    double magnitudeSquared() const noexcept;
+    Vector3d cross(const Vector3d& rhs) const noexcept;
+    Vector3d normalized() const noexcept;
+    void normalize() noexcept;
 
-    Vector3d& operator=(const Vector3d& other);
+    Vector3d& operator=(const Vector3d& other) noexcept;
     Vector3d& operator=(Vector3d&& other) noexcept;
 
-    Vector3d operator+(const Vector3d& rhs) const;
-    Vector3d operator-(const Vector3d& rhs) const;
+    Vector3d operator+(const Vector3d& rhs) const noexcept;
+    Vector3d operator-(const Vector3d& rhs) const noexcept;
 
-    Vector3d operator*(double scalar) const;
-    Vector3d operator/(double scalar) const;
+    Vector3d operator*(double scalar) const noexcept;
+    Vector3d operator/(double scalar) const noexcept;
 
-    Vector3d& operator+=(const Vector3d& rhs);
-    Vector3d& operator-=(const Vector3d& rhs);
+    Vector3d& operator+=(const Vector3d& rhs) noexcept;
+    Vector3d& operator-=(const Vector3d& rhs) noexcept;
 
-    Vector3d& operator*=(double scalar);
-    Vector3d& operator/=(double scalar);
+    Vector3d& operator*=(double scalar) noexcept;
+    Vector3d& operator/=(double scalar) noexcept;
 
-    bool operator==(const Vector3d& rhs) const;
-    bool operator!=(const Vector3d& rhs) const;
+    bool operator==(const Vector3d& rhs) const noexcept;
+    bool operator!=(const Vector3d& rhs) const noexcept;
 
 public:
     static const Vector3d Zero;