/*
 * skvt.c
 *
 *  Created on: 11 окт. 2023 г.
 *      Author: seklyuts
 */
#include "f28x_project.h"
#include "skvt.h"

/*
1) t = arctan(sin/cos)
2) t = PI/2-arctan(cos/sin)
3) t = PI/2+arctan(-cos/sin)
4) t = PI-arctan(sin/-cos)
5) t = -PI+arctan(-sin/-cos)
6) t = -PI/2-arctan(-cos/-sin)
7) t = -PI/2+arctan(cos/-sin)
8) t = -arctan(-sin/cos)

y= -0.0915 x^2 + 0.3434x - 0.0015
*/
#define A_p -FRAC16(0.0915)
#define B_p FRAC16(0.3434)
#define C_p -FRAC16(0.0015)

int16_t AngleSector;

int16_t mult_r(int16_t x, int16_t y) { return (int16_t) (((int32_t) (x) * (y)) >> 15); }

int16_t Atan_approximation(int16_t x)
{
  int16_t xx,y;

  if (x<0) y = FRAC16(0.25); //x=32767;//it will be so if (sin == cos), because (x<<15)/x = (x * 2^8)/x = 2^8 = 0x8000 = -32768;
  else {
      if(x <= C_p) y = 0;
      else {
        xx=mult_r(x,x);
        y = mult_r(xx,A_p) + mult_r(x,B_p) + C_p;
      }
  }
  return (y);
}


int16_t Atan(int16_t rSinA, int16_t rCosA)
{
int16_t theta_actual_el_1 = 0;
int16_t MrCosA, MrSinA;

  if(rSinA >= 0) {
    if(rCosA >= 0) { //0 < 0.5
      if(rSinA < rCosA) { AngleSector=1;
        theta_actual_el_1 = Atan_approximation(((int32_t)rSinA<<15)/rCosA);
      } else { AngleSector = 2;
        theta_actual_el_1 = FRAC16(0.5)-Atan_approximation(((int32_t)rCosA<<15)/rSinA);
      }
    } else { //0.5 < 1
      MrCosA = abs(rCosA);
      if(rSinA < MrCosA) { AngleSector = 4;
        theta_actual_el_1 = FRAC16(1)-Atan_approximation(((int32_t)rSinA<<15)/MrCosA);
      } else { AngleSector = 3;
        theta_actual_el_1 = FRAC16(0.5)+Atan_approximation(((int32_t)MrCosA<<15)/rSinA);
      }
    }
  } else {
    MrSinA = abs(rSinA);
    if(rCosA >= 0) { //0 > -0.5
      if(MrSinA < rCosA) { AngleSector = 8;
        theta_actual_el_1 = -Atan_approximation(((int32_t)MrSinA<<15)/rCosA);
      } else { AngleSector = 7;
        theta_actual_el_1 = -FRAC16(0.5)+Atan_approximation(((int32_t)rCosA<<15)/MrSinA);
      }
    } else { //-0.5 > -1
      MrCosA = abs(rCosA);
      if(MrSinA < MrCosA) { AngleSector = 5;
        theta_actual_el_1 = -FRAC16(1)+Atan_approximation(((int32_t)MrSinA<<15)/MrCosA);
      } else { AngleSector = 6;
        theta_actual_el_1 = -FRAC16(0.5)-Atan_approximation(((int32_t)MrCosA<<15)/MrSinA);
      }
    }
  }

  return(theta_actual_el_1);
}