#pragma once

#include <AP_Logger/LogStructure.h>

#define LOG_IDS_FROM_NAVEKF3 \
    LOG_XKF0_MSG, \
    LOG_XKF1_MSG, \
    LOG_XKF2_MSG, \
    LOG_XKF3_MSG, \
    LOG_XKF4_MSG, \
    LOG_XKF5_MSG, \
    LOG_XKFD_MSG, \
    LOG_XKFM_MSG, \
    LOG_XKFS_MSG, \
    LOG_XKQ_MSG,  \
    LOG_XKT_MSG,  \
    LOG_XKTV_MSG, \
    LOG_XKV1_MSG, \
    LOG_XKV2_MSG, \
    LOG_XKY0_MSG, \
    LOG_XKY1_MSG

// @LoggerMessage: XKF0
// @Description: EKF3 beacon sensor diagnostics
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: ID: Beacon sensor ID
// @Field: rng: Beacon range
// @Field: innov: Beacon range innovation
// @Field: SIV: sqrt of beacon range innovation variance
// @Field: TR: Beacon range innovation consistency test ratio
// @Field: BPN: Beacon north position
// @Field: BPE: Beacon east position
// @Field: BPD: Beacon down position
// @Field: OFH: High estimate of vertical position offset of beacons rel to EKF origin
// @Field: OFL: Low estimate of vertical position offset of beacons rel to EKF origin
// @Field: OFN: North position of receiver rel to EKF origin
// @Field: OFE: East position of receiver rel to EKF origin
// @Field: OFD: Down position of receiver rel to EKF origin
struct PACKED log_XKF0 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    uint8_t ID;             // beacon identifier
    int16_t rng;            // beacon range (cm)
    int16_t innov;          // beacon range innovation (cm)
    uint16_t sqrtInnovVar;  // sqrt of beacon range innovation variance (cm)
    uint16_t testRatio;     // beacon range innovation consistency test ratio *100
    int16_t beaconPosN;     // beacon north position (cm)
    int16_t beaconPosE;     // beacon east position (cm)
    int16_t beaconPosD;     // beacon down position (cm)
    int16_t offsetHigh;     // high estimate of vertical position offset of beacons rel to EKF origin (cm)
    int16_t offsetLow;      // low estimate of vertical position offset of beacons rel to EKF origin (cm)
    int16_t posN;           // North position of receiver rel to EKF origin (cm)
    int16_t posE;           // East position of receiver rel to EKF origin (cm)
    int16_t posD;           // Down position of receiver rel to EKF origin (cm)
};


// @LoggerMessage: XKF1
// @Description: EKF3 estimator outputs
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: Roll: Estimated roll
// @Field: Pitch: Estimated pitch
// @Field: Yaw: Estimated yaw
// @Field: VN: Estimated velocity (North component)
// @Field: VE: Estimated velocity (East component)
// @Field: VD: Estimated velocity (Down component)
// @Field: dPD: Filtered derivative of vertical position (down)
// @Field: PN: Estimated distance from origin (North component)
// @Field: PE: Estimated distance from origin (East component)
// @Field: PD: Estimated distance from origin (Down component)
// @Field: GX: Estimated gyro bias, X axis
// @Field: GY: Estimated gyro bias, Y axis
// @Field: GZ: Estimated gyro bias, Z axis
// @Field: OH: Height of origin above WGS-84
struct PACKED log_XKF1 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    int16_t roll;
    int16_t pitch;
    uint16_t yaw;
    float velN;
    float velE;
    float velD;
    float posD_dot;
    float posN;
    float posE;
    float posD;
    int16_t gyrX;
    int16_t gyrY;
    int16_t gyrZ;
    int32_t originHgt;
};


// @LoggerMessage: XKF2
// @Description: EKF3 estimator secondary outputs
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: AX: Estimated accelerometer X bias
// @Field: AY: Estimated accelerometer Y bias
// @Field: AZ: Estimated accelerometer Z bias
// @Field: VWN: Estimated wind velocity (North component)
// @Field: VWE: Estimated wind velocity (East component)
// @Field: MN: Magnetic field strength (North component)
// @Field: ME: Magnetic field strength (East component)
// @Field: MD: Magnetic field strength (Down component)
// @Field: MX: Magnetic field strength (body X-axis)
// @Field: MY: Magnetic field strength (body Y-axis)
// @Field: MZ: Magnetic field strength (body Z-axis)
// @Field: IDX: Innovation in vehicle drag acceleration (X-axis component)
// @Field: IDY: Innovation in vehicle drag acceleration (Y-axis component)
// @Field: IS: Innovation in vehicle sideslip
struct PACKED log_XKF2 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    int16_t accBiasX;
    int16_t accBiasY;
    int16_t accBiasZ;
    int16_t windN;
    int16_t windE;
    int16_t magN;
    int16_t magE;
    int16_t magD;
    int16_t magX;
    int16_t magY;
    int16_t magZ;
    float innovDragX;
    float innovDragY;
    float innovSideslip;
};


// @LoggerMessage: XKF3
// @Description: EKF3 innovations
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: IVN: Innovation in velocity (North component)
// @Field: IVE: Innovation in velocity (East component)
// @Field: IVD: Innovation in velocity (Down component)
// @Field: IPN: Innovation in position (North component)
// @Field: IPE: Innovation in position (East component)
// @Field: IPD: Innovation in position (Down component)
// @Field: IMX: Innovation in magnetic field strength (X-axis component)
// @Field: IMY: Innovation in magnetic field strength (Y-axis component)
// @Field: IMZ: Innovation in magnetic field strength (Z-axis component)
// @Field: IYAW: Innovation in vehicle yaw
// @Field: IVT: Innovation in true-airspeed
// @Field: RErr: Accumulated relative error of this core with respect to active primary core
// @Field: ErSc: A consolidated error score where higher numbers are less healthy
struct PACKED log_XKF3 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    int16_t innovVN;
    int16_t innovVE;
    int16_t innovVD;
    int16_t innovPN;
    int16_t innovPE;
    int16_t innovPD;
    int16_t innovMX;
    int16_t innovMY;
    int16_t innovMZ;
    int16_t innovYaw;
    int16_t innovVT;
    float rerr;
    float errorScore;
};


// @LoggerMessage: XKF4
// @Description: EKF3 variances
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: SV: Square root of the velocity variance
// @Field: SP: Square root of the position variance
// @Field: SH: Square root of the height variance
// @Field: SM: Magnetic field variance
// @Field: SVT: Square root of the total airspeed variance
// @Field: errRP: Filtered error in roll/pitch estimate
// @Field: OFN: Most recent position recent magnitude (North component)
// @Field: OFE: Most recent position recent magnitude (East component)
// @Field: FS: Filter fault status
// @Field: TS: Filter timeout status
// @Field: SS: Filter solution status
// @Field: GPS: Filter GPS status
// @Field: PI: Primary core index
struct PACKED log_XKF4 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    int16_t sqrtvarV;
    int16_t sqrtvarP;
    int16_t sqrtvarH;
    int16_t sqrtvarM;
    int16_t sqrtvarVT;
    float   tiltErr;
    int8_t  offsetNorth;
    int8_t  offsetEast;
    uint16_t faults;
    uint8_t timeouts;
    uint32_t solution;
    uint16_t gps;
    int8_t primary;
};


// @LoggerMessage: XKF5
// @Description: EKF3 Sensor innovations (primary core) and general dumping ground
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: NI: Normalised flow variance
// @Field: FIX: Optical flow LOS rate vector innovations from the main nav filter (X-axis)
// @Field: FIY: Optical flow LOS rate vector innovations from the main nav filter (Y-axis)
// @Field: AFI: Optical flow LOS rate innovation from terrain offset estimator
// @Field: HAGL: Height above ground level
// @Field: offset: Estimated vertical position of the terrain relative to the nav filter zero datum
// @Field: RI: Range finder innovations
// @Field: rng: Measured range
// @Field: Herr: Filter ground offset state error
// @Field: eAng: Magnitude of angular error
// @Field: eVel: Magnitude of velocity error
// @Field: ePos: Magnitude of position error
struct PACKED log_XKF5 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    uint8_t normInnov;
    int16_t FIX;
    int16_t FIY;
    int16_t AFI;
    int16_t HAGL;
    int16_t offset;
    int16_t RI;
    uint16_t meaRng;
    uint16_t errHAGL;
    float angErr;
    float velErr;
    float posErr;
};


// @LoggerMessage: XKFD
// @Description: EKF3 Body Frame Odometry errors
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: IX: Innovation in velocity (X-axis)
// @Field: IY: Innovation in velocity (Y-axis)
// @Field: IZ: Innovation in velocity (Z-axis)
// @Field: IVX: Variance in velocity (X-axis)
// @Field: IVY: Variance in velocity (Y-axis)
// @Field: IVZ: Variance in velocity (Z-axis)
struct PACKED log_XKFD {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    float velInnovX;
    float velInnovY;
    float velInnovZ;
    float velInnovVarX;
    float velInnovVarY;
    float velInnovVarZ;
};

// @LoggerMessage: XKT
// @Description: EKF3 timing information
// @Field: TimeUS: Time since system startup
// @Field: C: EKF core this message instance applies to
// @Field: Cnt: count of samples used to create this message
// @Field: IMUMin: smallest IMU sample interval
// @Field: IMUMax: largest IMU sample interval
// @Field: EKFMin: low-passed achieved average time step rate for the EKF (minimum)
// @Field: EKFMax: low-passed achieved average time step rate for the EKF (maximum)
// @Field: AngMin: accumulated measurement time interval for the delta angle (minimum)
// @Field: AngMax: accumulated measurement time interval for the delta angle (maximum)
// @Field: VMin: accumulated measurement time interval for the delta velocity (minimum)
// @Field: VMax: accumulated measurement time interval for the delta velocity (maximum)
struct PACKED log_XKT {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    uint32_t timing_count;
    float dtIMUavg_min;
    float dtIMUavg_max;
    float dtEKFavg_min;
    float dtEKFavg_max;
    float delAngDT_min;
    float delAngDT_max;
    float delVelDT_min;
    float delVelDT_max;
};


// @LoggerMessage: XKFM
// @Description: EKF3 diagnostic data for on-ground-and-not-moving check
// @Field: TimeUS: Time since system startup
// @Field: C: EKF core this message instance applies to
// @Field: OGNM: True of on ground and not moving
// @Field: GLR: Gyroscope length ratio
// @Field: ALR: Accelerometer length ratio
// @Field: GDR: Gyroscope rate of change ratio
// @Field: ADR: Accelerometer rate of change ratio
struct PACKED log_XKFM {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    uint8_t ongroundnotmoving;
    float gyro_length_ratio;
    float accel_length_ratio;
    float gyro_diff_ratio;
    float accel_diff_ratio;
};


// @LoggerMessage: XKQ
// @Description: EKF3 quaternion defining the rotation from NED to XYZ (autopilot) axes
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: Q1: Quaternion a term
// @Field: Q2: Quaternion b term
// @Field: Q3: Quaternion c term
// @Field: Q4: Quaternion d term
struct PACKED log_XKQ {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    float q1;
    float q2;
    float q3;
    float q4;
};


// @LoggerMessage: XKFS
// @Description: EKF3 sensor selection
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: MI: compass selection index
// @Field: BI: barometer selection index
// @Field: GI: GPS selection index
// @Field: AI: airspeed selection index
struct PACKED log_XKFS {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    uint8_t mag_index;
    uint8_t baro_index;
    uint8_t gps_index;
    uint8_t airspeed_index;
};

// @LoggerMessage: XKTV
// @Description: EKF3 Yaw Estimator States
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: TVS: Tilt Error Variance from symbolic equations (rad^2)
// @Field: TVD: Tilt Error Variance from difference method (rad^2)
struct PACKED log_XKTV {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    float tvs;
    float tvd;
};

// @LoggerMessage: XKV1
// @Description: EKF3 State variances (primary core)
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: V00: Variance for state 0
// @Field: V01: Variance for state 1
// @Field: V02: Variance for state 2
// @Field: V03: Variance for state 3
// @Field: V04: Variance for state 4
// @Field: V05: Variance for state 5
// @Field: V06: Variance for state 6
// @Field: V07: Variance for state 7
// @Field: V08: Variance for state 8
// @Field: V09: Variance for state 9
// @Field: V10: Variance for state 10
// @Field: V11: Variance for state 11

// @LoggerMessage: XKV2
// @Description: more EKF3 State Variances (primary core)
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: V12: Variance for state 12
// @Field: V13: Variance for state 13
// @Field: V14: Variance for state 14
// @Field: V15: Variance for state 15
// @Field: V16: Variance for state 16
// @Field: V17: Variance for state 17
// @Field: V18: Variance for state 18
// @Field: V19: Variance for state 19
// @Field: V20: Variance for state 20
// @Field: V21: Variance for state 21
// @Field: V22: Variance for state 22
// @Field: V23: Variance for state 23
struct PACKED log_XKV {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    float v00;
    float v01;
    float v02;
    float v03;
    float v04;
    float v05;
    float v06;
    float v07;
    float v08;
    float v09;
    float v10;
    float v11;
};


// @LoggerMessage: XKY0
// @Description: EKF3 Yaw Estimator States
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: YC: GSF yaw estimate (rad)
// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad)
// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad)
// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad)
// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad)
// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad)
// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad)
// @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0
// @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1
// @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2
// @Field: W3: Weighting applied to yaw estimate from individual EKF filter 3
// @Field: W4: Weighting applied to yaw estimate from individual EKF filter 4
struct PACKED log_XKY0 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    float yaw_composite;
    float yaw_composite_variance;
    float yaw0;
    float yaw1;
    float yaw2;
    float yaw3;
    float yaw4;
    float wgt0;
    float wgt1;
    float wgt2;
    float wgt3;
    float wgt4;
};


// @LoggerMessage: XKY1
// @Description: EKF3 Yaw Estimator Innovations
// @Field: TimeUS: Time since system startup
// @Field: C: EKF3 core this data is for
// @Field: IVN0: North velocity innovation from individual EKF filter 0 (m/s)
// @Field: IVN1: North velocity innovation from individual EKF filter 1 (m/s)
// @Field: IVN2: North velocity innovation from individual EKF filter 2 (m/s)
// @Field: IVN3: North velocity innovation from individual EKF filter 3 (m/s)
// @Field: IVN4: North velocity innovation from individual EKF filter 4 (m/s)
// @Field: IVE0: East velocity innovation from individual EKF filter 0 (m/s)
// @Field: IVE1: East velocity innovation from individual EKF filter 1 (m/s)
// @Field: IVE2: East velocity innovation from individual EKF filter 2 (m/s)
// @Field: IVE3: East velocity innovation from individual EKF filter 3 (m/s)
// @Field: IVE4: East velocity innovation from individual EKF filter 4 (m/s)
struct PACKED log_XKY1 {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t core;
    float ivn0;
    float ivn1;
    float ivn2;
    float ivn3;
    float ivn4;
    float ive0;
    float ive1;
    float ive2;
    float ive3;
    float ive4;
};


#define LOG_STRUCTURE_FROM_NAVEKF3        \
    { LOG_XKF0_MSG, sizeof(log_XKF0), \
      "XKF0","QBBccCCcccccccc","TimeUS,C,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s#-m---mmmmmmmm", "F--B---BBBBBBBB" }, \
    { LOG_XKF1_MSG, sizeof(log_XKF1), \
      "XKF1","QBccCfffffffccce","TimeUS,C,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "s#ddhnnnnmmmkkkm", "F-BBB0000000BBBB" }, \
    { LOG_XKF2_MSG, sizeof(log_XKF2), \
      "XKF2","QBccccchhhhhhfff","TimeUS,C,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,IDX,IDY,IS", "s#---nnGGGGGGoor", "F----BBCCCCCC000" }, \
    { LOG_XKF3_MSG, sizeof(log_XKF3), \
      "XKF3","QBcccccchhhccff","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT,RErr,ErSc", "s#nnnmmmGGG??--", "F-BBBBBBCCCBB00" }, \
    { LOG_XKF4_MSG, sizeof(log_XKF4), \
      "XKF4","QBcccccfbbHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \
    { LOG_XKF5_MSG, sizeof(log_XKF5), \
      "XKF5","QBBhhhcccCCfff","TimeUS,C,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s#----m???mrnm", "F-----BBBBB000" }, \
    { LOG_XKFD_MSG, sizeof(log_XKFD), \
      "XKFD","QBffffff","TimeUS,C,IX,IY,IZ,IVX,IVY,IVZ", "s#------", "F-------" }, \
    { LOG_XKFM_MSG, sizeof(log_XKFM),   \
      "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"}, \
    { LOG_XKFS_MSG, sizeof(log_XKFS), \
      "XKFS","QBBBBB","TimeUS,C,MI,BI,GI,AI", "s#----", "F-----" }, \
    { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" }, \
    { LOG_XKT_MSG, sizeof(log_XKT),   \
      "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \
    { LOG_XKTV_MSG, sizeof(log_XKTV),                         \
      "XKTV", "QBff", "TimeUS,C,TVS,TVD", "s#rr", "F-00"}, \
    { LOG_XKY0_MSG, sizeof(log_XKY0),                         \
      "XKY0", "QBffffffffffff", "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", "s#rrrrrrr-----", "F-000000000000"}, \
    { LOG_XKY1_MSG, sizeof(log_XKY1),                         \
      "XKY1", "QBffffffffff", "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4", "s#nnnnnnnnnn", "F-0000000000"}, \
    { LOG_XKV1_MSG, sizeof(log_XKV), \
      "XKV1","QBffffffffffff","TimeUS,C,V00,V01,V02,V03,V04,V05,V06,V07,V08,V09,V10,V11", "s#------------", "F-------------" }, \
    { LOG_XKV2_MSG, sizeof(log_XKV), \
      "XKV2","QBffffffffffff","TimeUS,C,V12,V13,V14,V15,V16,V17,V18,V19,V20,V21,V22,V23", "s#------------", "F-------------" },