From 1308944a0603c6ac0758bc87777f019c4f811cbe Mon Sep 17 00:00:00 2001
From: MmAaXx500 <viktor.balogh45@gmail.com>
Date: Wed, 19 Jun 2024 15:20:50 +0200
Subject: [PATCH] fix magnitude and phase calculation

---
 src/tmt300rs/hid-tmt300rs.c | 53 +++++++++++++++++++++++++++----------
 1 file changed, 39 insertions(+), 14 deletions(-)

diff --git a/src/tmt300rs/hid-tmt300rs.c b/src/tmt300rs/hid-tmt300rs.c
index a1efd7d..3306170 100644
--- a/src/tmt300rs/hid-tmt300rs.c
+++ b/src/tmt300rs/hid-tmt300rs.c
@@ -344,6 +344,27 @@ static u8 damper_values[] = {
 	0x7f, 0x07
 };
 
+static void t300rs_calculate_periodic_values(struct ff_effect *effect)
+{
+	struct ff_periodic_effect *periodic = &effect->u.periodic;
+
+	effect->replay.length -= 1;
+
+	periodic->magnitude = (periodic->magnitude * fixp_sin16(effect->direction * 360 / 0x10000)) / 0x7fff;
+
+	if (periodic->magnitude < 0){
+		/* the wheel does not handle negative magnitudes */
+		/* to give the expected result 180 deg is added to the phase */
+		periodic->phase = (periodic->phase + (0x10000 / 2)) % 0x10000;
+
+		/* the wheel handles positive magnitudes only */
+		periodic->magnitude = -periodic->magnitude;
+	}
+
+	/* the interval [0; 32677[ is used by the wheel for the [0; 360[ degree phase shift */
+	periodic->phase = periodic->phase * 32677 / 0x10000;
+}
+
 int t300rs_send_buf(struct t300rs_device_entry *t300rs, u8 *send_buffer, size_t len)
 {
 	int i;
@@ -828,26 +849,30 @@ static int t300rs_update_spring(struct t300rs_device_entry *t300rs,
 	return t300rs_update_damper(t300rs, state);
 }
 
-
 static int t300rs_update_periodic(struct t300rs_device_entry *t300rs,
 		struct tmff2_effect_state *state)
 {
 	struct ff_effect effect = state->effect;
 	struct ff_effect old = state->old;
-	struct ff_periodic_effect periodic = effect.u.periodic;
-	struct ff_periodic_effect periodic_old = old.u.periodic;
 	struct __packed t300rs_packet_mod_periodic {
 		struct t300rs_packet_header header;
 		uint8_t attribute;
 		uint16_t value;
 	} *packet_mod_periodic = (struct t300rs_packet_mod_periodic *)t300rs->send_buffer;
 
+	struct ff_periodic_effect periodic, periodic_old;
 	int ret;
-	int16_t magnitude, phase;
+	uint16_t phase;
+	int16_t magnitude;
 
-	magnitude = (periodic.magnitude * fixp_sin16(effect.direction * 360 / 0x10000)) / 0x7fff;
+	t300rs_calculate_periodic_values(&effect);
+	periodic = effect.u.periodic;
+	magnitude = periodic.magnitude;
 	phase = periodic.phase;
 
+	t300rs_calculate_periodic_values(&old);
+	periodic_old = effect.u.periodic;
+
 	if ((periodic.magnitude != periodic_old.magnitude)
 			|| (effect.direction != old.direction)) {
 
@@ -1125,7 +1150,6 @@ static int t300rs_upload_periodic(struct t300rs_device_entry *t300rs,
 		struct tmff2_effect_state *state)
 {
 	struct ff_effect effect = state->effect;
-	struct ff_periodic_effect periodic = state->effect.u.periodic;
 	struct __packed t300rs_packet_periodic {
 		struct t300rs_packet_header header;
 		uint16_t magnitude;
@@ -1138,19 +1162,20 @@ static int t300rs_upload_periodic(struct t300rs_device_entry *t300rs,
 		struct t300rs_packet_timing timing;
 	} *packet_periodic = (struct t300rs_packet_periodic *)t300rs->send_buffer;
 
+	struct ff_periodic_effect periodic;
 	int ret;
-	uint16_t duration, period, offset, periodic_offset;
-	int16_t phase, magnitude;
-
-	duration = effect.replay.length - 1;
-	magnitude = (periodic.magnitude * fixp_sin16(effect.direction * 360 / 0x10000)) / 0x7fff;
+	uint16_t duration, period, phase, offset, periodic_offset;
+	int16_t magnitude;
 
+	t300rs_calculate_periodic_values(&effect);
+	periodic = effect.u.periodic;
+	duration = effect.replay.length;
+	offset = effect.replay.delay;
+	magnitude = periodic.magnitude;
+	period = periodic.period;
 	phase = periodic.phase;
 	periodic_offset = periodic.offset;
 
-	period = periodic.period;
-	offset = effect.replay.delay;
-
 	t300rs_fill_header(&packet_periodic->header, effect.id, 0x6b);
 
 	packet_periodic->magnitude = cpu_to_le16(magnitude);