From 93ee4716cc673e13465a92be16c41c5499686b90 Mon Sep 17 00:00:00 2001
From: Blaz Kristan <blaz@kristan-sp.si>
Date: Thu, 23 Sep 2021 20:45:53 +0200
Subject: [PATCH] Wled math bugfix.

---
 wled00/wled_math.h | 60 ++++++++++------------------------------------
 1 file changed, 12 insertions(+), 48 deletions(-)

diff --git a/wled00/wled_math.h b/wled00/wled_math.h
index 3b091433..b25014bf 100644
--- a/wled00/wled_math.h
+++ b/wled00/wled_math.h
@@ -15,10 +15,10 @@
 
 #define modd(x, y) ((x) - (int)((x) / (y)) * (y))
 
-float cos_t(float x)
+float cos_t(float phi)
 {
-  x = modd(x, TWO_PI);
-  char sign = 1;
+  float x = modd(phi, TWO_PI);
+  int8_t sign = 1;
   if (x > PI)
   {
       x -= PI;
@@ -28,7 +28,7 @@ float cos_t(float x)
 
   float res = sign * (1 - ((xx) / (2)) + ((xx * xx) / (24)) - ((xx * xx * xx) / (720)) + ((xx * xx * xx * xx) / (40320)) - ((xx * xx * xx * xx * xx) / (3628800)) + ((xx * xx * xx * xx * xx * xx) / (479001600)));
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("cos: %f,%f\n",res,cos(x));
+  Serial.printf("cos: %f,%f,%f,(%f)\n",phi,res,cos(x),res-cos(x));
   #endif
   return res;
 }
@@ -36,7 +36,7 @@ float cos_t(float x)
 float sin_t(float x) {
   float res =  cos_t(HALF_PI - x);
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("sin: %f,%f\n",res,sin(x));
+  Serial.printf("sin: %f,%f,%f,(%f)\n",x,res,sin(x),res-sin(x));
   #endif
   return res;
 }
@@ -46,7 +46,7 @@ float tan_t(float x) {
   if (c==0.0) return 0;
   float res = sin_t(x) / c;
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("tan: %f,%f\n",res,tan(x));
+  Serial.printf("tan: %f,%f,%f,(%f)\n",x,res,tan(x),res-tan(x));
   #endif
   return res;
 }
@@ -67,7 +67,7 @@ float acos_t(float x) {
   ret = ret - 2 * negate * ret;
   float res = negate * PI + ret;
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("acos,%f,%f,%f\n",x,res,acos(x));
+  Serial.printf("acos: %f,%f,%f,(%f)\n",x,res,acos(x),res-acos(x));
   #endif
   return res;
 }
@@ -75,12 +75,12 @@ float acos_t(float x) {
 float asin_t(float x) {
   float res = HALF_PI - acos_t(x);
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("asin,%f,%f,%f\n",x,res,asin(x));
+  Serial.printf("asin: %f,%f,%f,(%f)\n",x,res,asin(x),res-asin(x));
   #endif
   return res;
 }
 
-/*
+
 //https://stackoverflow.com/a/42542593
 #define A 0.0776509570923569
 #define B -0.287434475393028
@@ -112,43 +112,7 @@ float atan_t(float x) {
   }
   if (neg) res = -res;
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("atan,%f,%f,%f\n",xinput,res,atan(xinput));
-  #endif
-  return res;
-}
-*/
-//https://stackoverflow.com/a/42542593
-#define A 0.0776509570923569
-#define B -0.287434475393028
-#define C ((HALF_PI/2) - A - B)
-
-//polynominal factors for approximation between 1 and 5
-#define C0 0.089494f
-#define C1 0.974207f
-#define C2 -0.326175f
-#define C3 0.05375f
-#define C4 -0.003445f
-
-float atan_t(float x) {
-  bool neg = (x < 0);
-  #ifdef WLED_DEBUG_MATH
-  float xinput = x;
-  #endif
-  x = std::abs(x);
-  float res;
-  if (x > 5.0f) { //atan(x) converges to pi/2 - (1/x) for large values
-    res = HALF_PI - (1.0f/x);
-  }
-  else if (x > 1.0f) { //1 < x < 5
-    float xx = x * x;
-    res = (C4*xx*xx)+(C3*xx*x)+(C2*xx)+(C1*x)+C0;
-  } else { //this approximation is only for x <= 1
-    float xx = x * x;
-    res = ((A*xx + B)*xx + C)*x;
-  }
-  if (neg) res = -res;
-  #ifdef WLED_DEBUG_MATH
-  Serial.printf("atan,%f,%f,%f\n",xinput,res,atan(xinput));
+  Serial.printf("atan: %f,%f,%f,(%f)\n",xinput,res,atan(xinput),res-atan(xinput));
   #endif
   return res;
 }
@@ -158,7 +122,7 @@ float floor_t(float x) {
   int val = x;
   if (neg) val--;
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("floor: %f,%f\n",val,floor(x));
+  Serial.printf("floor: %f,%f,%f\n",x,(float)val,floor(x));
   #endif
   return val;
 }
@@ -167,7 +131,7 @@ float fmod_t(float num, float denom) {
   int tquot = num / denom;
   float res = num - tquot * denom;
   #ifdef WLED_DEBUG_MATH
-  Serial.printf("fmod: %f,%f\n",res,fmod(num,denom));
+  Serial.printf("fmod: %f,%f,(%f)\n",res,fmod(num,denom),res-fmod(num,denom));
   #endif
   return res;
 }