/*
 * Created by Emon Feb 24, 2021
 * Model for CF2.1
 */
// #define DEBUG_MODULE "cfJacobian"
// #include "kalman_core.h"

#ifndef __CFJACOBIAN_H__
#define __CFJACOBIAN_H__

#include "kalman_core.h"
#include "stabilizer_types.h"
// #include <math.h>
// #include "arm_math.h"
// #include "cfassert.h"

#include "outlierFilter.h"
#include "physicalConstants.h"

#include "log.h"
#include "param.h"
#include "math3d.h"
#include "debug.h"
#include "static_mem.h"

#include "deck.h"

#include "FreeRTOS.h"
#include "task.h"

#include "power_distribution.h"

#endif // __CFJACOBIAN_H__

extern sensorData_t *pSensorData; // Both input and output for model
extern float *pCmd_thrust, *pCmd_roll, *pCmd_pitch, *pCmd_yaw;  // Input for model
extern motors_t *pMotors;  // Output for model
extern state_t *pState;  // Input for model
extern control_t *pControl;  // Input for model
// Output of modelCtrl is already defined as input of model (*pCmd_thrust, *pCmd_roll, *pCmd_pitch, *pCmd_yaw)

// The bounds on the covariance, these shouldn't be hit, but sometimes are... why?
#define MAX_COVARIANCE (100)
#define MIN_COVARIANCE (1e-6f)
#define ROLLPITCH_ZERO_REVERSION (0.001f)
#define GRAVITY_MAGNITUDE (9.81f)

// Initial variances, uncertain of position, but know we're stationary and roughly flat
static const float stdDevInitialPosition_xy = 100;
static const float stdDevInitialPosition_z = 1;
static const float stdDevInitialVelocity = 0.01;
static const float stdDevInitialAttitude_rollpitch = 0.01;
static const float stdDevInitialAttitude_yaw = 0.01;

static float procNoiseAcc_xy = 0.5f;
static float procNoiseAcc_z = 1.0f;
static float procNoiseVel = 0;
static float procNoisePos = 0;
static float procNoiseAtt = 0;
static float measNoiseBaro = 2.0f; // meters
static float measNoiseGyro_rollpitch = 0.1f; // radians per second
static float measNoiseGyro_yaw = 0.1f; // radians per second

static float initialX = 0.0;
static float initialY = 0.0;
static float initialZ = 0.0;

// Initial yaw of the Crazyflie in radians.
// 0 --- facing positive X
// PI / 2 --- facing positive Y
// PI --- facing negative X
// 3 * PI / 2 --- facing negative Y
static float initialYaw = 0.0;

// Quaternion used for initial yaw
static float initialQuaternion[4] = {0.0, 0.0, 0.0, 0.0};

// static uint32_t tdoaCount;

// static OutlierFilterLhState_t sweepOutlierFilterState;


// The data used by the kalman core implementation.
typedef struct {
  /**
   * Quadrocopter State
   *
   * The internally-estimated state is:
   * - X, Y, Z: the quad's position in the global frame
   * - PX, PY, PZ: the quad's velocity in its body frame
   * - D0, D1, D2: attitude error
   *
   * For more information, refer to the paper
   */
  float S[KC_STATE_DIM];

  // The quad's attitude as a quaternion (w,x,y,z)
  // We store as a quaternion to allow easy normalization (in comparison to a rotation matrix),
  // while also being robust against singularities (in comparison to euler angles)
  float q[4];

  // The quad's attitude as a rotation matrix (used by the prediction, updated by the finalization)
  float R[3][3];

  // The covariance matrix
  float P[KC_STATE_DIM][KC_STATE_DIM];
  arm_matrix_instance_f32 Pm;

  // ADDED BY EMON - The covariance matrix for KFChk
  float P_KF[KC_STATE_DIM][KC_STATE_DIM];
  arm_matrix_instance_f32 Pm_KF;
  float A_KF[KC_STATE_DIM][KC_STATE_DIM];
  arm_matrix_instance_f32 Am_KF;

  // Indicates that the internal state is corrupt and should be reset
  bool resetEstimation;

  float baroReferenceHeight;
} kalmanCoreData_Emon;

kalmanCoreData_Emon KFCoreEmonData;
Axis3f accEmon, gyroEmon;
float dtEmon, errorEmon, cmdThrustEmon;
// float stdMeasNoise = 0.25;
bool quadIsFlyingEmon;
flowMeasurement_t flowEmon;

void kalmanCoreInitEmon(kalmanCoreData_Emon* this);
void kalmanCorePredictEmon(kalmanCoreData_Emon* this, float cmdThrust, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying);
void scalarUpdateEmon(kalmanCoreData_Emon* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise);
void kalmanCoreUpdateWithFlowEmon(kalmanCoreData_Emon* this, const flowMeasurement_t *flow, const Axis3f *gyro);
void doOneSim(kalmanCoreData_Emon* this, float cmdThrust, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying);