#include "phone_orientation.h"

#include "utils.h"

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

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

#include <cmath>

using maps::geolib3::Radians;
using maps::geolib3::Vector3;
using maps::geolib3::PI;

namespace maps::mrc::pos_improvment {

// Car up direction is the average gravity force direction
UnitVector3 findCarUpDirection(const AccelerometerEvents& accEvents)
{
    REQUIRE(accEvents.size(), "empty accEvents");
    constexpr MetersPerSec2 GRAVITY_VALUE(9.8);
    constexpr MetersPerSec2 GRAVITY_VALUE_ACCURACY(0.2);
    constexpr MetersPerSec2 SIMILAR_VECTOR_THRESHOLD(0.2);
    const int HISTOGRAM_SIZE = 1000;
    std::vector<Vector3> accelerationVectors{accEvents[0].values()};
    auto prevVector = accEvents[0].values();
    for (const auto& event : accEvents) {
        // If the car is standing on a slope for a long time, it may
        // affect on car up vector calculation, so it is better to skip
        // sequencees of similar values.
        // Also acceleration value should be approximately 9.8
        if (length(event.values() - prevVector) > SIMILAR_VECTOR_THRESHOLD
            && abs(length(event.values()) - GRAVITY_VALUE) < GRAVITY_VALUE_ACCURACY)
        {
            accelerationVectors.push_back(event.values());
            prevVector = event.values();
        }
    }
    // Earth 'pushes' car up, so gravity is presented as up acceleration
    // in the accelerometer
    auto [carUpVector, confidence] = findMostPopularDirection(accelerationVectors,
                                                              HISTOGRAM_SIZE);

    INFO() << "found car up direction with " << confidence << " confidence";
    INFO() << "car up vector = ["
           << Vector3(carUpVector).x() << ", "
           << Vector3(carUpVector).y() << ", "
           << Vector3(carUpVector).z() << "]";
    return carUpVector;
}

// Returns front or back vector of the car
// To find the front/back axis we use only points where car rotation is
// zero. At these points there can only be breaking/accelerating
// acceleration or up/down acceleration (because of bad roads).
// To exclude up/down acceleration we use only points where
// acceleration vector is perpendicular to car up vector.
// When we have breaking and accelerating vectors and we need to find
// the most popular one
UnitVector3 findCarFrontBackAxis(const AccelerometerEvents& accEvents,
                                 const GyroscopeEvents& gyroEvents,
                                 const GpsEvents& gpsEvents,
                                 const UnitVector3& carUpVector)
{
    constexpr RadiansPerSec MIN_ROTATION_THRESHOLD(0.01);
    constexpr MetersPerSec2 MIN_ACCELERATION_THRESHOLD(0.5);
    const AccelerationVector APPROXIMATE_GRAVITY = Vector3(carUpVector) * 9.8_mps2;
    constexpr Radians ALMOST_HORIZONTAL_THRESHOLD(geolib3::toRadians(10.0_deg));
    constexpr MetersPerSec2 MAX_POSSIBLE_CAR_ACCELERATION(5);
    constexpr MetersPerSec2 SIMILAR_VECTOR_THRESHOLD(0.1);
    constexpr Seconds GPS_SPEED_COOLDOWN(1.0);
    const int HISTOGRAM_SIZE = 40;

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

    std::vector<Vector3> accelerationsWithoutRotation;
    bool isRotating = true;
    int pointsWithoutRotating = 0;
    int carAccelerationPoints = 0;
    AccelerationVector prevAccVector(0, 0, 0);
    MetersPerSec2 gpsAcceleration(0.0);
    int gpsIndex = -1;

    for (const auto& trackEvent : events) {
        if (trackEvent->type == EventType::Gps) {
            gpsIndex++;
            if (gpsIndex < (int)gpsEvents.size() - 1
                && gpsEvents[gpsIndex].speed
                && gpsEvents[gpsIndex + 1].speed)
            {
                gpsAcceleration =
                    (*gpsEvents[gpsIndex + 1].speed - *gpsEvents[gpsIndex].speed)
                    / (gpsEvents[gpsIndex + 1].time - gpsEvents[gpsIndex].time);
            } else {
                gpsAcceleration = 0.0_mps2;
            }
        }
        if (trackEvent->type == EventType::Gyroscope) {
            auto& event = *static_cast<GyroscopeEvent const *>(trackEvent);
            RadiansPerSec rotationSpeed = abs(event.values().rotSpeedAroundAxis(carUpVector));//length(event.values());
            if (rotationSpeed > MIN_ROTATION_THRESHOLD) {
                isRotating = true;
            } else {
                isRotating = false;
                pointsWithoutRotating++;
            }
        }

        if (trackEvent->type == EventType::Accelerometer && !isRotating) {
            auto& event = *static_cast<AccelerometerEvent const *>(trackEvent);
            if (gpsIndex == -1
                || event.time - gpsEvents[gpsIndex].time > GPS_SPEED_COOLDOWN) {
                continue;
            }

            AccelerationVector accVector = event.values() - APPROXIMATE_GRAVITY;
            MetersPerSec2 accValue = length(accVector);
            UnitVector3 accDirection(accVector);
            Radians angleBetweenAccAndUp = angleBetween(carUpVector, accDirection);
            if (accValue > MIN_ACCELERATION_THRESHOLD
                && abs(gpsAcceleration) > MIN_ACCELERATION_THRESHOLD
                && abs(angleBetweenAccAndUp - PI / 2) < ALMOST_HORIZONTAL_THRESHOLD
                && length(accVector - prevAccVector) > SIMILAR_VECTOR_THRESHOLD
                && accValue < MAX_POSSIBLE_CAR_ACCELERATION)
            {
                Vector3 gravityInAcc = Vector3(carUpVector)
                    * accVector.accAlongAxis(carUpVector);
                Vector3 accWithoutGravity = accVector - gravityInAcc;
                carAccelerationPoints++;
                prevAccVector = accVector;
                accelerationsWithoutRotation.push_back(accWithoutGravity);
                // add reverse vector to sum back and front vectors
                accelerationsWithoutRotation.push_back(accWithoutGravity * -1.0);
            }
        }
    }
    INFO() << "points without rotating: "
           << pointsWithoutRotating / (1.0 * gyroEvents.size());
    INFO() << "carAccelerationPoints points: " << carAccelerationPoints;

    auto [carFrontAxis, confidence] = findMostPopularDirection(
        accelerationsWithoutRotation, HISTOGRAM_SIZE);

    INFO() << "found car axis with " << confidence << " confidence";
    INFO() << "car axis = ["
           << Vector3(carFrontAxis).x() << ", "
           << Vector3(carFrontAxis).y() << ", "
           << Vector3(carFrontAxis).z() << "]";
    INFO() << "car axis * gravity = " << geolib3::innerProduct(carFrontAxis,
                                                               carUpVector);
    return carFrontAxis;
}


UnitVector3 findCarFrontDirection(const AccelerometerEvents& accEvents,
                                  const GyroscopeEvents& gyroEvents,
                                  const GpsEvents& gpsEvents,
                                  const UnitVector3& carUpVector)
{
    constexpr Seconds SENSORS_FILTER_TIME(0.5);
    constexpr MetersPerSec2 MIN_ACC(1.0);
    constexpr Seconds GPS_SPEED_COOLDOWN(1.0);
    constexpr double MIN_CONFIDENCE = 0.7;

    auto filteredAccEvents = getFilteredValues(accEvents, SENSORS_FILTER_TIME);
    auto filteredGyroEvents = getFilteredValues(gyroEvents, SENSORS_FILTER_TIME);

    UnitVector3 carFrontAxis = findCarFrontBackAxis(
        filteredAccEvents, filteredGyroEvents, gpsEvents, carUpVector);

    // we have the car front/back axis but we don't know if it is
    // front or back directed

    TrackEventPtrs events;
    insert(events, filteredGyroEvents);
    insert(events, filteredAccEvents);
    insert(events, gpsEvents);

    int frontCounter = 0;
    int backCounter = 0;
    MetersPerSec2 gpsAcceleration(0.0);
    int gpsIndex = -1;

    for (const auto& trackEvent : events) {
        if (trackEvent->type == EventType::Gps) {
            gpsIndex++;
            if (gpsIndex < (int)gpsEvents.size() - 1
                && gpsEvents[gpsIndex].speed
                && gpsEvents[gpsIndex + 1].speed)
            {
                gpsAcceleration =
                    (*gpsEvents[gpsIndex + 1].speed - *gpsEvents[gpsIndex].speed)
                    / (gpsEvents[gpsIndex + 1].time - gpsEvents[gpsIndex].time);
            } else {
                gpsAcceleration = 0.0_mps2;
            }
        }

        if (trackEvent->type == EventType::Accelerometer && gpsIndex != -1) {
            auto& event = *static_cast<AccelerometerEvent const *>(trackEvent);
            if (event.time - gpsEvents[gpsIndex].time > GPS_SPEED_COOLDOWN) {
                continue;
            }
            MetersPerSec2 frontAcc = event.values().accAlongAxis(carFrontAxis);
            bool sensorsFrontAcc = frontAcc > MIN_ACC;
            bool sensorsBackAcc = frontAcc < -MIN_ACC;
            bool gpsFrontAcc = gpsAcceleration > MIN_ACC;
            bool gpsBackAcc = gpsAcceleration < -MIN_ACC;

            // compare accelereometer and gps acceleration direction
            if ((sensorsFrontAcc && gpsFrontAcc)
                || sensorsBackAcc && gpsBackAcc) {
                // if gps acceleration matches accelerometer acceleration
                frontCounter++;
            }
            if ((sensorsFrontAcc && gpsBackAcc)
                || sensorsBackAcc && gpsFrontAcc) {
                // if gps acceleration is opposite to accelerometer acceleration
                backCounter++;
            }
        }
    }

    INFO() << "car back/front orientation points: " << frontCounter + backCounter;
    INFO() << "found car back/front orientation with "
           << std::max(frontCounter, backCounter) / (0.0 + frontCounter + backCounter)
           << " confidence";
    INFO() << "front/back : " << frontCounter << "/" << backCounter;

    REQUIRE(std::max(frontCounter, backCounter) / (0.0 + frontCounter + backCounter)
            > MIN_CONFIDENCE,
            "can't find car front vector");

    if (frontCounter > backCounter) {
        return carFrontAxis;
    } else {
        return UnitVector3(Vector3(carFrontAxis) * -1.0);
    }
}

} // namespace maps::mrc::pos_improvment
