#include <maps/wikimap/mapspro/services/mrc/libs/self_driving_car/include/car_position.h>

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

#include <cmath>
#include <fstream>

namespace maps{
namespace mrc {
namespace self_driving {

geolib3::Heading CarPosition::heading() const
{
    double angle = 2 * acos(orientation.w);
    if (orientation.z < 0) {
        angle = -angle;
    }
    return geolib3::Heading(angle * 180 / M_PI);
}

std::vector<CarPosition> loadSelfDrivingTrack(const std::string& filename)
{
    std::ifstream input(filename);
    std::vector<CarPosition> track;
    std::string line;

    while (std::getline(input, line)) {
        auto data = maps::json::Value::fromString(line);
        auto localization = data["/localization/full"];
        CarPosition pos;
        pos.lat = localization["latitude"].as<double>();
        pos.lon = localization["longitude"].as<double>();
        pos.speed = localization["linear_velocity"]["x"].as<double>();
        pos.orientation.x = localization["global_orientation"]["x"].as<double>();
        pos.orientation.y = localization["global_orientation"]["y"].as<double>();
        pos.orientation.z = localization["global_orientation"]["z"].as<double>();
        pos.orientation.w = localization["global_orientation"]["w"].as<double>();


        int64_t seconds = localization["header"]["stamp"]["secs"].as<int64_t>();
        int64_t nanoSeconds = localization["header"]["stamp"]["nsecs"].as<int64_t>();
        pos.time = chrono::TimePoint(
            std::chrono::milliseconds(seconds * 1000 + nanoSeconds / 1000000));
        track.push_back(pos);
    }
    return track;
}


} // namespace self_driving
} // namespace mrc
} // namespace maps
