#include "gyroscope.h"

#include <maps/libs/common/include/exception.h>
#include <maps/libs/log8/include/log8.h>

namespace maps::mrc::pos_improvment {

double findMedian(std::vector<double>& values) {
    const size_t middleIndex = values.size() / 2;
    std::nth_element(values.begin(),
                     values.begin() + middleIndex,
                     values.end());
    return values[middleIndex];
}

GyroscopeEvents calibrateGyroscope(GyroscopeEvents gyroEvents) {
    std::vector<double> values(gyroEvents.size());

    for (size_t i = 0; i < gyroEvents.size(); i++) {
        values[i] = (gyroEvents[i].values().x());
    }
    double medianX = findMedian(values);

    for (size_t i = 0; i < gyroEvents.size(); i++) {
        values[i] = (gyroEvents[i].values().y());
    }
    double medianY = findMedian(values);

    for (size_t i = 0; i < gyroEvents.size(); i++) {
        values[i] = (gyroEvents[i].values().z());
    }
    double medianZ = findMedian(values);

    geolib3::Vector3 medianGyroValues(medianX, medianY, medianZ);
    for (auto& event : gyroEvents) {
        event.values() -= medianGyroValues;
    }
    return gyroEvents;
}

std::pair<AccelerometerEvents, GyroscopeEvents> fixSensorsAndGpsTimeLag(
    const GpsEvents& gpsEvents,
    AccelerometerEvents accEvents,
    GyroscopeEvents gyroEvents,
    const CarGroundDirectionEvents& directionEvents)
{
    const Seconds MAX_TIME_SHIFT(1.0);
    // if we shift sensors time, some border gps points will not be
    // covered by sensors, so better not to use border gps points
    const int BORDER_GPS_POINTS = 5;

    REQUIRE(directionEvents.size(), "no direction events were provided");
    double bestError = DBL_MAX;//100000000.0;
    Seconds bestShift(0);

    for (Seconds shift = -MAX_TIME_SHIFT; shift <= MAX_TIME_SHIFT; shift += 0.01_sec) {
        CarGroundDirectionEvents shiftedDirectionEvents = directionEvents;
        for (auto& event : shiftedDirectionEvents) {
            event.time += Seconds(shift);
        }

        TrackEventPtrs events;
        events.reserve(gpsEvents.size() + shiftedDirectionEvents.size());
        insert(events, gpsEvents);
        insert(events, shiftedDirectionEvents);

        double error = 0;
        geolib3::Radians directionBySensors = shiftedDirectionEvents[0].angle;
        int curGpsIndex = -1;

        for (auto trackEvent : events) {
            if (trackEvent->type == EventType::CarGroundDirection) {
                auto& event = *static_cast<CarGroundDirectionEvent const *>(trackEvent);
                directionBySensors = event.angle;
            }
            if (trackEvent->type == EventType::Gps) {
                curGpsIndex++;
                if (curGpsIndex < BORDER_GPS_POINTS
                    || curGpsIndex > (int)gpsEvents.size() - BORDER_GPS_POINTS) {
                    continue;
                }
                auto& event = *static_cast<GpsEvent const *>(trackEvent);
                if (event.direction) {
                    auto angleBetweenSensorsAndGps = geolib3::angleBetween(
                        geolib3::Direction2(directionBySensors),
                        geolib3::Direction2(*event.direction));
                    error += angleBetweenSensorsAndGps.value();
                }
            }
        }

        if (error < bestError
            || (error == bestError && abs(shift) < abs(bestShift)))
        {
            bestError = error;
            bestShift = shift;
        }
    }

    if (abs(bestShift) < 0.00001_sec) {
        bestShift = 0.0_sec; // for easier tests writing
    }
    INFO() << "sensors lag seconds " << bestShift.count();

    for (auto& event : gyroEvents) {
        event.time += bestShift;
    }
    for (auto& event : accEvents) {
        event.time += bestShift;
    }
    return {accEvents, gyroEvents};
}


} // namespace maps::mrc::pos_improvment
