#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"

#include <AP_Logger/AP_Logger.h>

// Write ACC data packet: raw accel data
void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const
{
        const uint64_t now = AP_HAL::micros64();
        const struct log_ACC pkt {
            LOG_PACKET_HEADER_INIT(LOG_ACC_MSG),
            time_us   : now,
            instance  : instance,
            sample_us : sample_us?sample_us:now,
            AccX      : accel.x,
            AccY      : accel.y,
            AccZ      : accel.z
        };
        AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

// Write GYR data packet: raw gyro data
void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp) const
{
        const uint64_t now = use_sample_timestamp?sample_us:AP_HAL::micros64();
        const struct log_GYR pkt{
            LOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
            time_us   : now,
            instance  : instance,
            sample_us : sample_us?sample_us:now,
            GyrX      : gyro.x,
            GyrY      : gyro.y,
            GyrZ      : gyro.z
        };
        AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

// Write IMU data packet: raw accel/gyro data
void AP_InertialSensor::Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const
{
    const Vector3f &gyro = get_gyro(imu_instance);
    const Vector3f &accel = get_accel(imu_instance);
    const struct log_IMU pkt{
        LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
        time_us : time_us,
        instance: imu_instance,
        gyro_x  : gyro.x,
        gyro_y  : gyro.y,
        gyro_z  : gyro.z,
        accel_x : accel.x,
        accel_y : accel.y,
        accel_z : accel.z,
        gyro_error  : _gyro_error_count[imu_instance],
        accel_error : _accel_error_count[imu_instance],
        temperature : get_temperature(imu_instance),
        gyro_health : (uint8_t)get_gyro_health(imu_instance),
        accel_health : (uint8_t)get_accel_health(imu_instance),
        gyro_rate : get_gyro_rate_hz(imu_instance),
        accel_rate : get_accel_rate_hz(imu_instance),
    };
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

// Write IMU data packet for all instances
void AP_InertialSensor::Write_IMU() const
{
    const uint64_t time_us = AP_HAL::micros64();

    uint8_t n = MAX(get_accel_count(), get_gyro_count());
    for (uint8_t i=0; i<n; i++) {
        Write_IMU_instance(time_us, i);
    }
}

// Write VIBE data packet for all instances
void AP_InertialSensor::Write_Vibration() const
{
    const uint64_t time_us = AP_HAL::micros64();
    for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
        if (!use_accel(i)) {
            continue;
        }

        const Vector3f vibration = get_vibration_levels(i);
        const struct log_Vibe pkt{
            LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
            time_us     : time_us,
            imu         : i,
            vibe_x      : vibration.x,
            vibe_y      : vibration.y,
            vibe_z      : vibration.z,
            clipping    : get_accel_clip_count(i)
        };
        AP::logger().WriteBlock(&pkt, sizeof(pkt));
    }
}

#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
// Write information about a series of IMU readings to log:
bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const
{
    uint8_t instance_to_write = instance;
    if (post_filter && (_doing_pre_post_filter_logging
            || (_doing_post_filter_logging && _doing_sensor_rate_logging))) {
        instance_to_write += (type == IMU_SENSOR_TYPE_ACCEL ? _imu._accel_count : _imu._gyro_count);
    }
    const struct log_ISBH pkt{
        LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG),
        time_us        : AP_HAL::micros64(),
        seqno          : isb_seqnum,
        sensor_type    : (uint8_t)type,
        instance       : instance_to_write,
        multiplier     : multiplier,
        sample_count   : (uint16_t)_real_required_count,
        sample_us      : measurement_started_us,
        sample_rate_hz : sample_rate_hz,
    };

    return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
}

// Write a series of IMU readings to log:
bool AP_InertialSensor::BatchSampler::Write_ISBD() const
{
    struct log_ISBD pkt = {
        LOG_PACKET_HEADER_INIT(LOG_ISBD_MSG),
        time_us    : AP_HAL::micros64(),
        isb_seqno  : isb_seqnum,
        seqno      : (uint16_t) (data_read_offset/samples_per_msg)
    };
    memcpy(pkt.x, &data_x[data_read_offset], sizeof(pkt.x));
    memcpy(pkt.y, &data_y[data_read_offset], sizeof(pkt.y));
    memcpy(pkt.z, &data_z[data_read_offset], sizeof(pkt.z));

    return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
}
#endif

// @LoggerMessage: FTN
// @Description: Filter Tuning Message - per motor
// @Field: TimeUS: microseconds since system startup
// @Field: I: instance
// @Field: NDn: number of active dynamic harmonic notches
// @Field: NF1: dynamic harmonic notch centre frequency for motor 1
// @Field: NF2: dynamic harmonic notch centre frequency for motor 2
// @Field: NF3: dynamic harmonic notch centre frequency for motor 3
// @Field: NF4: dynamic harmonic notch centre frequency for motor 4
// @Field: NF5: dynamic harmonic notch centre frequency for motor 5
// @Field: NF6: dynamic harmonic notch centre frequency for motor 6
// @Field: NF7: dynamic harmonic notch centre frequency for motor 7
// @Field: NF8: dynamic harmonic notch centre frequency for motor 8
// @Field: NF9: dynamic harmonic notch centre frequency for motor 9
// @Field: NF10: dynamic harmonic notch centre frequency for motor 10
// @Field: NF11: dynamic harmonic notch centre frequency for motor 11
// @Field: NF12: dynamic harmonic notch centre frequency for motor 12

// @LoggerMessage: FTNS
// @Description: Filter Tuning Message
// @Field: TimeUS: microseconds since system startup
// @Field: I: instance
// @Field: NF: dynamic harmonic notch centre frequency

void AP_InertialSensor::write_notch_log_messages() const
{
    for (auto &notch : harmonic_notches) {
        const uint8_t i = &notch - &harmonic_notches[0];
        if (!notch.params.enabled()) {
            continue;
        }
        const float* notches = notch.calculated_notch_freq_hz;
        if (notch.num_calculated_notch_frequencies > 1) {
            // log per motor center frequencies
            AP::logger().WriteStreaming(
                "FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff",
                AP_HAL::micros64(),
                i,
                notch.num_calculated_notch_frequencies,
                notches[0], notches[1], notches[2], notches[3],
                notches[4], notches[5], notches[6], notches[7],
                notches[8], notches[9], notches[10], notches[11]);
        } else {
            // log single center frequency
            AP::logger().WriteStreaming(
                "FTNS", "TimeUS,I,NF", "s#z", "F--", "QBf",
                AP_HAL::micros64(),
                i,
                notches[0]);
        }
    }
}