/*
 * Copyright (C) 2011 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdio.h>

#include <utils/Log.h>

#include "Fusion.h"

namespace android {

// -----------------------------------------------------------------------

/*
 * gyroVAR gives the measured variance of the gyro's output per
 * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
 * which is independent of the sampling frequency.
 *
 * The variance of gyro's output at a given sampling period can be
 * calculated as:
 *      variance(T) = gyroVAR / T
 *
 * The variance of the INTEGRATED OUTPUT at a given sampling period can be
 * calculated as:
 *       variance_integrate_output(T) = gyroVAR * T
 *
 */
static const float gyroVAR = 1e-7;      // (rad/s)^2 / Hz
static const float biasVAR = 1e-8;      // (rad/s)^2 / s (guessed)

/*
 * Standard deviations of accelerometer and magnetometer
 */
static const float accSTDEV  = 0.05f;   // m/s^2 (measured 0.08 / CDD 0.05)
static const float magSTDEV  = 0.5f;    // uT    (measured 0.7  / CDD 0.5)

static const float SYMMETRY_TOLERANCE = 1e-10f;

/*
 * Accelerometer updates will not be performed near free fall to avoid
 * ill-conditioning and div by zeros.
 * Threshhold: 10% of g, in m/s^2
 */
static const float FREE_FALL_THRESHOLD = 0.981f;
static const float FREE_FALL_THRESHOLD_SQ =
        FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;

/*
 * The geomagnetic-field should be between 30uT and 60uT.
 * Fields strengths greater than this likely indicate a local magnetic
 * disturbance which we do not want to update into the fused frame.
 */
static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
static const float MAX_VALID_MAGNETIC_FIELD_SQ =
        MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;

/*
 * Values of the field smaller than this should be ignored in fusion to avoid
 * ill-conditioning. This state can happen with anomalous local magnetic
 * disturbances canceling the Earth field.
 */
static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
static const float MIN_VALID_MAGNETIC_FIELD_SQ =
        MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;

/*
 * If the cross product of two vectors has magnitude squared less than this,
 * we reject it as invalid due to alignment of the vectors.
 * This threshold is used to check for the case where the magnetic field sample
 * is parallel to the gravity field, which can happen in certain places due
 * to magnetic field disturbances.
 */
static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
    MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;

// -----------------------------------------------------------------------

template <typename TYPE, size_t C, size_t R>
static mat<TYPE, R, R> scaleCovariance(
        const mat<TYPE, C, R>& A,
        const mat<TYPE, C, C>& P) {
    // A*P*transpose(A);
    mat<TYPE, R, R> APAt;
    for (size_t r=0 ; r<R ; r++) {
        for (size_t j=r ; j<R ; j++) {
            double apat(0);
            for (size_t c=0 ; c<C ; c++) {
                double v(A[c][r]*P[c][c]*0.5);
                for (size_t k=c+1 ; k<C ; k++)
                    v += A[k][r] * P[c][k];
                apat += 2 * v * A[c][j];
            }
            APAt[j][r] = apat;
            APAt[r][j] = apat;
        }
    }
    return APAt;
}

template <typename TYPE, typename OTHER_TYPE>
static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
    mat<TYPE, 3, 3> r;
    r[0][0] = diag;
    r[1][1] = diag;
    r[2][2] = diag;
    r[0][1] = p.z;
    r[1][0] =-p.z;
    r[0][2] =-p.y;
    r[2][0] = p.y;
    r[1][2] = p.x;
    r[2][1] =-p.x;
    return r;
}


template<typename TYPE, size_t SIZE>
class Covariance {
    mat<TYPE, SIZE, SIZE> mSumXX;
    vec<TYPE, SIZE> mSumX;
    size_t mN;
public:
    Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
    void update(const vec<TYPE, SIZE>& x) {
        mSumXX += x*transpose(x);
        mSumX  += x;
        mN++;
    }
    mat<TYPE, SIZE, SIZE> operator()() const {
        const float N = 1.0f / mN;
        return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
    }
    void reset() {
        mN = 0;
        mSumXX = 0;
        mSumX = 0;
    }
    size_t getCount() const {
        return mN;
    }
};

// -----------------------------------------------------------------------

Fusion::Fusion() {
    Phi[0][1] = 0;
    Phi[1][1] = 1;

    Ba.x = 0;
    Ba.y = 0;
    Ba.z = 1;

    Bm.x = 0;
    Bm.y = 1;
    Bm.z = 0;

    x0 = 0;
    x1 = 0;

    init();
}

void Fusion::init() {
    mInitState = 0;

    mGyroRate = 0;

    mCount[0] = 0;
    mCount[1] = 0;
    mCount[2] = 0;

    mData = 0;
}

void Fusion::initFusion(const vec4_t& q, float dT)
{
    // initial estimate: E{ x(t0) }
    x0 = q;
    x1 = 0;

    // process noise covariance matrix: G.Q.Gt, with
    //
    //  G = | -1 0 |        Q = | q00 q10 |
    //      |  0 1 |            | q01 q11 |
    //
    // q00 = sv^2.dt + 1/3.su^2.dt^3
    // q10 = q01 = 1/2.su^2.dt^2
    // q11 = su^2.dt
    //

    const float dT2 = dT*dT;
    const float dT3 = dT2*dT;

    // variance of integrated output at 1/dT Hz (random drift)
    const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;

    // variance of drift rate ramp
    const float q11 = biasVAR * dT;
    const float q10 = 0.5f * biasVAR * dT2;
    const float q01 = q10;

    GQGt[0][0] =  q00;      // rad^2
    GQGt[1][0] = -q10;
    GQGt[0][1] = -q01;
    GQGt[1][1] =  q11;      // (rad/s)^2

    // initial covariance: Var{ x(t0) }
    // TODO: initialize P correctly
    P = 0;

    // it is unclear how to set the initial covariance. It does affect
    // how quickly the fusion converges. Experimentally it would take
    // about 10 seconds at 200 Hz to estimate the gyro-drift with an
    // initial covariance of 0, and about a second with an initial covariance
    // of about 1 deg/s.
    const float covv = 0;
    const float covu = 0.5f * (float(M_PI) / 180);
    mat33_t& Pv = P[0][0];
    Pv[0][0] = covv;
    Pv[1][1] = covv;
    Pv[2][2] = covv;
    mat33_t& Pu = P[1][1];
    Pu[0][0] = covu;
    Pu[1][1] = covu;
    Pu[2][2] = covu;
}

bool Fusion::hasEstimate() const {
    return (mInitState == (MAG|ACC|GYRO));
}

bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
    if (hasEstimate())
        return true;

    if (what == ACC) {
        mData[0] += d * (1/length(d));
        mCount[0]++;
        mInitState |= ACC;
    } else if (what == MAG) {
        mData[1] += d * (1/length(d));
        mCount[1]++;
        mInitState |= MAG;
    } else if (what == GYRO) {
        mGyroRate = dT;
        mData[2] += d*dT;
        mCount[2]++;
        if (mCount[2] == 64) {
            // 64 samples is good enough to estimate the gyro drift and
            // doesn't take too much time.
            mInitState |= GYRO;
        }
    }

    if (mInitState == (MAG|ACC|GYRO)) {
        // Average all the values we collected so far
        mData[0] *= 1.0f/mCount[0];
        mData[1] *= 1.0f/mCount[1];
        mData[2] *= 1.0f/mCount[2];

        // calculate the MRPs from the data collection, this gives us
        // a rough estimate of our initial state
        mat33_t R;
        vec3_t up(mData[0]);
        vec3_t east(cross_product(mData[1], up));
        east *= 1/length(east);
        vec3_t north(cross_product(up, east));
        R << east << north << up;
        const vec4_t q = matrixToQuat(R);

        initFusion(q, mGyroRate);
    }

    return false;
}

void Fusion::handleGyro(const vec3_t& w, float dT) {
    if (!checkInitComplete(GYRO, w, dT))
        return;

    predict(w, dT);
}

status_t Fusion::handleAcc(const vec3_t& a) {
    // ignore acceleration data if we're close to free-fall
    if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
        return BAD_VALUE;
    }

    if (!checkInitComplete(ACC, a))
        return BAD_VALUE;

    const float l = 1/length(a);
    update(a*l, Ba, accSTDEV*l);
    return NO_ERROR;
}

status_t Fusion::handleMag(const vec3_t& m) {
    // the geomagnetic-field should be between 30uT and 60uT
    // reject if too large to avoid spurious magnetic sources
    const float magFieldSq = length_squared(m);
    if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
        return BAD_VALUE;
    } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
        // Also reject if too small since we will get ill-defined (zero mag)
        // cross-products below
        return BAD_VALUE;
    }

    if (!checkInitComplete(MAG, m))
        return BAD_VALUE;

    // Orthogonalize the magnetic field to the gravity field, mapping it into
    // tangent to Earth.
    const vec3_t up( getRotationMatrix() * Ba );
    const vec3_t east( cross_product(m, up) );

    // If the m and up vectors align, the cross product magnitude will
    // approach 0.
    // Reject this case as well to avoid div by zero problems and
    // ill-conditioning below.
    if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
        return BAD_VALUE;
    }

    // If we have created an orthogonal magnetic field successfully,
    // then pass it in as the update.
    vec3_t north( cross_product(up, east) );

    const float l = 1 / length(north);
    north *= l;

    update(north, Bm, magSTDEV*l);
    return NO_ERROR;
}

void Fusion::checkState() {
    // P needs to stay positive semidefinite or the fusion diverges. When we
    // detect divergence, we reset the fusion.
    // TODO(braun): Instead, find the reason for the divergence and fix it.

    if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) ||
        !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) {
        ALOGW("Sensor fusion diverged; resetting state.");
        P = 0;
    }
}

vec4_t Fusion::getAttitude() const {
    return x0;
}

vec3_t Fusion::getBias() const {
    return x1;
}

mat33_t Fusion::getRotationMatrix() const {
    return quatToMatrix(x0);
}

mat34_t Fusion::getF(const vec4_t& q) {
    mat34_t F;

    // This is used to compute the derivative of q
    // F = | [q.xyz]x |
    //     |  -q.xyz  |

    F[0].x = q.w;   F[1].x =-q.z;   F[2].x = q.y;
    F[0].y = q.z;   F[1].y = q.w;   F[2].y =-q.x;
    F[0].z =-q.y;   F[1].z = q.x;   F[2].z = q.w;
    F[0].w =-q.x;   F[1].w =-q.y;   F[2].w =-q.z;
    return F;
}

void Fusion::predict(const vec3_t& w, float dT) {
    const vec4_t q  = x0;
    const vec3_t b  = x1;
    const vec3_t we = w - b;

    // q(k+1) = O(we)*q(k)
    // --------------------
    //
    // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x                   psi |
    //        | -psi'                              cos(0.5*||w||*dT) |
    //
    // psi = sin(0.5*||w||*dT)*w / ||w||
    //
    //
    // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
    // ----------------------------------------
    //
    // G = | -I33    0 |
    //     |    0  I33 |
    //
    //  Phi = | Phi00 Phi10 |
    //        |   0     1   |
    //
    //  Phi00 =   I33
    //          - [w]x   * sin(||w||*dt)/||w||
    //          + [w]x^2 * (1-cos(||w||*dT))/||w||^2
    //
    //  Phi10 =   [w]x   * (1        - cos(||w||*dt))/||w||^2
    //          - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
    //          - I33*dT

    const mat33_t I33(1);
    const mat33_t I33dT(dT);
    const mat33_t wx(crossMatrix(we, 0));
    const mat33_t wx2(wx*wx);
    const float lwedT = length(we)*dT;
    const float hlwedT = 0.5f*lwedT;
    const float ilwe = 1/length(we);
    const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
    const float k1 = sinf(lwedT);
    const float k2 = cosf(hlwedT);
    const vec3_t psi(sinf(hlwedT)*ilwe*we);
    const mat33_t O33(crossMatrix(-psi, k2));
    mat44_t O;
    O[0].xyz = O33[0];  O[0].w = -psi.x;
    O[1].xyz = O33[1];  O[1].w = -psi.y;
    O[2].xyz = O33[2];  O[2].w = -psi.z;
    O[3].xyz = psi;     O[3].w = k2;

    Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
    Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);

    x0 = O*q;
    if (x0.w < 0)
        x0 = -x0;

    P = Phi*P*transpose(Phi) + GQGt;

    checkState();
}

void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
    vec4_t q(x0);
    // measured vector in body space: h(p) = A(p)*Bi
    const mat33_t A(quatToMatrix(q));
    const vec3_t Bb(A*Bi);

    // Sensitivity matrix H = dh(p)/dp
    // H = [ L 0 ]
    const mat33_t L(crossMatrix(Bb, 0));

    // gain...
    // K = P*Ht / [H*P*Ht + R]
    vec<mat33_t, 2> K;
    const mat33_t R(sigma*sigma);
    const mat33_t S(scaleCovariance(L, P[0][0]) + R);
    const mat33_t Si(invert(S));
    const mat33_t LtSi(transpose(L)*Si);
    K[0] = P[0][0] * LtSi;
    K[1] = transpose(P[1][0])*LtSi;

    // update...
    // P = (I-K*H) * P
    // P -= K*H*P
    // | K0 | * | L 0 | * P = | K0*L  0 | * | P00  P10 | = | K0*L*P00  K0*L*P10 |
    // | K1 |                 | K1*L  0 |   | P01  P11 |   | K1*L*P00  K1*L*P10 |
    // Note: the Joseph form is numerically more stable and given by:
    //     P = (I-KH) * P * (I-KH)' + K*R*R'
    const mat33_t K0L(K[0] * L);
    const mat33_t K1L(K[1] * L);
    P[0][0] -= K0L*P[0][0];
    P[1][1] -= K1L*P[1][0];
    P[1][0] -= K0L*P[1][0];
    P[0][1] = transpose(P[1][0]);

    const vec3_t e(z - Bb);
    const vec3_t dq(K[0]*e);
    const vec3_t db(K[1]*e);

    q += getF(q)*(0.5f*dq);
    x0 = normalize_quat(q);
    x1 += db;

    checkState();
}

// -----------------------------------------------------------------------

}; // namespace android