#include "calc.hpp"
#include <algorithm>


namespace calc {


std::pair<Position, Position>
position_segment(std::vector<Position> const& position, double time)
{
  auto second = std::lower_bound(position.begin(), position.end(), time,
    [](Position const& pos, double t) {
      return pos.time < t;
    });
  if (second == position.begin()) { // corner case
    second++;
  }
  return std::make_pair(*(second-1), *second);
}


void interpolate_positions(std::vector<Magnetic>& magnetics, std::vector<Position> const& truth)
{
  for (auto& mag : magnetics) {
    auto const truth_seg = position_segment(truth, mag.time);
    for (size_t i = 0; i < mag.position.size(); i++) {
      mag.position[i] =
        (truth_seg.second.position[i] - truth_seg.first.position[i]) /
        (truth_seg.second.time - truth_seg.first.time) *
        (mag.time - truth_seg.first.time) + truth_seg.first.position[i];
    }
  }
}


void crop_temporal_inersection(std::vector<Magnetic>& magnetics, std::vector<Position> const& truth)
{
  auto const mag_first = std::lower_bound(
    magnetics.begin(), magnetics.end(), truth[0].time,
    [](Magnetic const& mag, double t) {
      return mag.time < t;
    });
  magnetics.erase(magnetics.begin(), mag_first);
  auto const mag_last = std::upper_bound(
    magnetics.begin(), magnetics.end(), truth.back().time,
    [](double t, Magnetic const& mag) {
      return t < mag.time;
    });
  magnetics.erase(mag_last, magnetics.end());
}


std::tuple<double, double, double, double>
spatial_extent(std::vector<Magnetic> const& mag)
{
  auto [x_min, x_max] = std::minmax_element(
    std::cbegin(mag), std::cend(mag),
    [](Magnetic const& a, Magnetic const& b) {
      return a.position[0] < b.position[0];
    });
  auto [y_min, y_max] = std::minmax_element(
    std::cbegin(mag), std::cend(mag),
    [](Magnetic const& a, Magnetic const& b) {
      return a.position[1] < b.position[1];
    });
  return std::make_tuple(x_min->position[0], y_min->position[1], x_max->position[0], y_max->position[1]);
}


}