/**
 * @file magnetometer.h
 * @brief Recupération orientation magnétique
 *
 * Fonctions  l'orientation magnétique en I2C depuis le magnétomètre hmc5883l
 * et fonctions mathèmatiques dévelopées à cause de l'abscence d'implementation de math.h sur le processeur.
 *
 *  @date 6 janv. 2017
 *  @author Dmitry Prokofyev (dmitry.prokofyev@free.fr)
 *  @version 1.0
 */

#ifndef MAGNETOMETER_H_
#define MAGNETOMETER_H_
#include  <math.h>
#include "i2c_custom.h"
//-----------------------------------------------------------------------------
/// @brief      Calcul x à la puissance y
///
/// @param      x base
/// @param      y puissance
///
/// @return     x^y
///
float power(float x, int y)
{
    float temp;
    if( y == 0)
       return 1;
    temp = power(x, y/2);
    if (y%2 == 0)
        return temp*temp;
    else
    {
        if(y > 0)
            return x*temp*temp;
        else
            return (temp*temp)/x;
    }
}


//-----------------------------------------------------------------------------
/// @brief      Calcul de arctangante de x par développement de taylor au degré 10
/// @param      x     double
///
/// @return     atan(x)
///@warning Uniquement pour x <=0
double atan_cst(double x)
{
    int k=0;
    int lim = 10;
    double result=0;
    for(k=0;k<lim;k++)
    {
        result+= (double) (   power( -1.0, k ) *  (power((float) x ,2*k+1))/(2*k+1) ) ;
    }
    return result;
}

//-----------------------------------------------------------------------------
/// @brief      Calcul de l'artan de y/x
///
/// @param      y
/// @param      x
///
/// @return     Atan(y/x)
///@warning Uniquement pour y<=x
double atan2_cst(double y, double x)
  {
          double absx, absy, val;

          if (x == 0 && y == 0) {
                  return 0;
          }
          absy = y < 0 ? -y : y;
          absx = x < 0 ? -x : x;
          if (absy - absx == absy) {
                  /* x negligible compared to y */
                  return y < 0 ? -M_PI_2 : M_PI_2;
          }
          if (absx - absy == absx) {
                  /* y negligible compared to x */
                  val = 0.0;
          }
          else    val = atan_cst(y/x);
          if (x > 0) {
                  /* first or fourth quadrant; already correct */
                  return val;
          }
          if (y < 0) {
                  /* third quadrant */
                  return val - M_PI;
          }
          return val + M_PI;
  }
//-----------------------------------------------------------------------------
/// @brief      Récupération de l'orientation magnétique depuis le hmc5883l
///
/// @return     Orientation magnétique en degrées.
float readHeading()
   {

   	float heading,tmpy,tmpx;
   	uint8_t xhi,xlo,zhi,zlo,yhi,ylo;
   	int16_t x,y,z;


   		writedataMAG(0x00,0x70);
   		writedataMAG(0x01,0xA0);
   		writedataMAG(0x02,0x00);

   		// recuperation des composantes x y et z
   		xhi = I2C_WriteRead(0x3D,0x03,1);
   				xlo = I2C_WriteRead(0x3D,0x04,1);
   				zhi = I2C_WriteRead(0x3D,0x05,1);
   				zlo = I2C_WriteRead(0x3D,0x06,1);
   				yhi = I2C_WriteRead(0x3D,0x07,1);
   				ylo = I2C_WriteRead(0x3D,0x08,1);
   				writedataMAG(0x3C,0x03); // clock back;
   				x = (int16_t)(xlo | (int16_t)(xhi << 8));
   				y = (int16_t)(ylo | (int16_t)(yhi << 8));
   				z = (int16_t)(zlo | (int16_t)(zhi << 8));
   				// comparaison de x et y pour déscider si l'on utilise x/y ou y/x dans l'actan
   				tmpy = y>0 ? y : -y;
   				tmpx = x>0 ? x : -x;
   				if ( (float)tmpy/(float)tmpx > 1)
   				{
   				  heading = atan2_cst((double)x,(double)y);
   				  heading = M_PI/4+M_PI/4-heading;
   				}
   				else
   				{
   				    heading = atan2_cst((double)y,(double)x);
   				}
   				if(heading < 0)
   				heading += 2*M_PI;
   				// Check for wrap due to addition of declination.
   				if(heading > 2*M_PI)
   				heading -= 2*M_PI;
   				heading *= 180/M_PI;

   				return heading;

   }


#endif /* MAGNETOMETER_H_ */