#include "math.h"

float gPitch = 0;
float gRoll  = 0;
float gYaw   = 0;

/* Estimated values by using accelerometer and gyroscope data */
float estimatedX = 0.0;
float estimatedY = 0.0;
float estimatedZ = 0.0;

static bool gVeryFirstTime = true;
/* Vector compoments calculated using gyroscope data */
static float gyroscopeX = 0.0;
static float gyroscopeY = 0.0;
static float gyroscopeZ = 0.0;

static const float gkWeigth = 5;

static void normalize3(float& x, float& y, float& z);

void estimateOrientation(float& ax, float& ay, float& az,
                         const float& xz, const float& yz, const float& xy)
{
  /* Calculate vector component from accelerometer data */
  normalize3(ax, ay, az);

  if (gVeryFirstTime) {
    estimatedX = ax;
    estimatedY = ay;
    estimatedZ = az;
    gVeryFirstTime = false;
    return;
  }

  /* Evaluate vector from gyroscope data */
  if ( -0.1 < estimatedZ && estimatedZ < 0.1) {
    /* The z component is too small, better use previous estimation,
     * otherwise it will simply amplify the error for pitch and roll */
    gyroscopeX = estimatedX;
    gyroscopeY = estimatedY;
    gyroscopeZ = estimatedZ;
  } else {

    /* Get angle displacement from gyroscope
     * Datasheet says degree per seconds, but it appears they are
     * in radiants per second
     * Output Data Rate = 100Hz, thus T = 0.01s */
    float deltaPitch = (0.01 * xz) * 180 / M_PI;
    float deltaRoll = (0.01 * yz) * 180 / M_PI;
    float deltaYaw = (0.01 * xy) * 180 / M_PI;
    /* Get angle change in degree */
    gPitch = atan2(estimatedX, estimatedZ) * 180 / M_PI;
    gRoll = atan2(estimatedY, estimatedZ) * 180 / M_PI;
    gYaw = atan2(estimatedX, estimatedY) * 180 / M_PI;
    /* Update angle based on gyroscope readings */
    gPitch += deltaPitch;
    gRoll += deltaRoll;
    gYaw += deltaYaw;

    /* Estimate z component sign by looking in what quadrant pitch (xz) is */
    float zSign = (cos(gPitch * M_PI / 180) >= 0) ? 1 : -1;

    /* And now the fun part... Reverse calculate R components for gyroscope */
    gyroscopeX = sin(gPitch * M_PI / 180);
    gyroscopeX /= sqrt( 1 + pow(cos(gPitch * M_PI / 180), 2) * pow(tan(gRoll * M_PI / 180), 2));

    gyroscopeY = sin(gRoll * M_PI / 180);
    gyroscopeY /= sqrt( 1 + pow(cos(gRoll * M_PI / 180), 2) * pow(tan(gPitch * M_PI / 180), 2));

    gyroscopeZ = zSign * sqrt( 1 - gyroscopeX * gyroscopeX - gyroscopeY * gyroscopeY);
  }

  /* Combine accelerometer data with gyroscope, hopefully getting better result */
  estimatedX = (ax + gkWeigth * gyroscopeX) / (1 + gkWeigth);
  estimatedY = (ay + gkWeigth * gyroscopeY) / (1 + gkWeigth);
  estimatedZ = (az + gkWeigth * gyroscopeZ) / (1 + gkWeigth);

  /* Normalize vector */
  normalize3(estimatedX, estimatedY, estimatedZ);

  return ;
}

void normalize3(float& x, float& y, float& z)
{
  float r = sqrt(x*x + y*y + z*z);
  x /= r;
  y /= r;
  z /= r;
}