#include "WCPData/TrackInfo.h"

using namespace WCP;

TrackInfo::TrackInfo(WCP::PointVector& tracking_path, std::vector<double>& dQ, std::vector<double>& dx,
                     std::vector<double>& pu, std::vector<double>& pv, std::vector<double>& pw, std::vector<double>& pt,
                     std::vector<double>& reduced_chi2)
  : tracking_path(tracking_path)
  , dQ(dQ)
  , dx(dx)
  , pu(pu)
  , pv(pv)
  , pw(pw)
  , pt(pt)
  , reduced_chi2(reduced_chi2)
  , parent_track(0)
{
    pcloud = new ToyPointCloud();
    pcloud->AddPoints(tracking_path);
    pcloud->build_kdtree_index();
}

TrackInfo::~TrackInfo() { delete pcloud; }

std::pair<double, WCP::Point> TrackInfo::get_closest_point(WCP::Point& p) { return pcloud->get_closest_point(p); }

std::tuple<double, double, double> TrackInfo::get_closest_2d_dis(WCP::Point& p)
{
    std::pair<int, double> results_u = pcloud->get_closest_2d_dis(p, 0);
    std::pair<int, double> results_v = pcloud->get_closest_2d_dis(p, 1);
    std::pair<int, double> results_w = pcloud->get_closest_2d_dis(p, 2);

    return std::make_tuple(results_u.second, results_v.second, results_w.second);
}

bool TrackInfo::AddTrackInfo(WCP::TrackInfo* track, WCP::Point& pos, WCP::Point& self_pos, bool isparent)
{
    if (isparent) {
        // parent tracks ...
        if (parent_track != 0) return false;
        parent_track = track;
        map_track_points[track] = std::make_pair(pos, self_pos);
    }
    else {
        // daughter tracks ...
        daughter_tracks.push_back(track);
        map_track_points[track] = std::make_pair(pos, self_pos);
    }
    return true;
}

std::pair<bool, std::pair<WCP::Point, WCP::Point> > TrackInfo::get_track_points(WCP::TrackInfo* track)
{
    auto it = map_track_points.find(track);

    if (it == map_track_points.end()) {
        return std::make_pair(false, std::make_pair(WCP::Point(0, 0, 0), WCP::Point(0, 0, 0)));
    }
    else {
        return std::make_pair(true, it->second);
    }
}

double TrackInfo::get_track_length(int flag)
{
    double length = 0;
    if (flag == 1) {
        for (auto it = dx.begin(); it != dx.end(); it++) {
            // std::cout << length << " " << *it << std::endl;
            length += *it;
        }
    }
    else {
        for (size_t i = 0; i + 1 < tracking_path.size(); i++) {
            length += sqrt(pow(tracking_path.at(i + 1).x - tracking_path.at(i).x, 2) +
                           pow(tracking_path.at(i + 1).y - tracking_path.at(i).y, 2) +
                           pow(tracking_path.at(i + 1).z - tracking_path.at(i).z, 2));
        }
    }
    return length;
}

double TrackInfo::get_medium_dQ_dx()
{
    std::vector<double> vec_dQ_dx;
    for (size_t i = 0; i != dQ.size(); i++) {
        vec_dQ_dx.push_back(dQ.at(i) / (dx.at(i) + 1e-9));
    }
    std::nth_element(vec_dQ_dx.begin(), vec_dQ_dx.begin() + vec_dQ_dx.size() / 2, vec_dQ_dx.end());
    return *std::next(vec_dQ_dx.begin(), vec_dQ_dx.size() / 2);
}

double TrackInfo::get_track_length_threshold(double threshold)
{
    double length = 0;
    for (size_t i = 0; i != dQ.size(); i++) {
        double dQ_dx = dQ.at(i) / (dx.at(i) + 1e-9);
        //    std::cout << dQ_dx/50000*units::cm << std::endl;
        if (dQ_dx > threshold) length += dx.at(i);
    }
    return length;
}
