#pragma once
#include "timing/time_transform.h"

namespace time_transform {

template <IsTimeTransformer Transformer>
typename Transformer::DomainTime findDomainPointForCodomainDelta(
    const Transformer &transformer,
    const typename Transformer::DomainTime &start_domain_p,
    typename Transformer::CodomainDelta target_codomain_delta) {
  using DomainTime = typename Transformer::DomainTime;
  using CodomainTime = typename Transformer::CodomainTime;
  using CodomainDelta = typename Transformer::CodomainDelta;

  static constexpr double EPS = time_units::EPSILON;

  if (std::abs(target_codomain_delta.raw()) < EPS) {
    return start_domain_p;
  }

  DomainTime current_domain_p = start_domain_p;
  CodomainDelta remaining_c_delta = target_codomain_delta;

  const int MAX_ITERATIONS = 100;
  for (int i = 0; i < MAX_ITERATIONS; ++i) {
    if (std::abs(remaining_c_delta.raw()) < EPS) {
      break;
    }

    bool forward_travel = remaining_c_delta.raw() > 0.0;

    auto it = transformer.getSegmentIteratorAt(current_domain_p);

    if (it == transformer.end()) {
      if (forward_travel || transformer.begin() == transformer.end()) {
        break; // Cannot go further
      }
      // We are past the end of defined domain, but moving backward.
      // Let's use the very last segment.
      it = std::prev(transformer.end());
    }

    bool backward_transition =
        !forward_travel &&
        current_domain_p.raw() == it->source_range.start.raw() &&
        it != transformer.begin();
    DomainTime p_at_transition_gap(0.0);

    // When moving backward from the exact start of a segment, we should instead
    // use the previous segment for our calculation. We transition our domain
    // point to the end of that previous segment.
    if (backward_transition) {
      p_at_transition_gap = current_domain_p;
      it = std::prev(it);
      current_domain_p = it->source_range.end;
    }

    const auto &segment = *it;

    // Since we may have jumped to a new point (the end of a previous segment),
    // we need to map it to the codomain to find our current standing.
    auto current_c_p_opt = segment.map_point(current_domain_p, true);

    // If we moved backward over a gap and failed mapping due to FP error,
    // manually set our codomain point to the end of the previous segment.
    if (!current_c_p_opt && backward_transition &&
        std::abs((p_at_transition_gap - current_domain_p).raw()) < EPS) {
      current_c_p_opt = segment.target_range.end;
    }

    if (!current_c_p_opt) {
      // If we can't map the point, we're likely in a gap or at an unmapped
      // boundary. We cannot proceed.
      break;
    }
    CodomainTime current_c_p = *current_c_p_opt;

    CodomainTime segment_codomain_boundary =
        forward_travel ? segment.target_range.end : segment.target_range.start;
    DomainTime segment_domain_boundary =
        forward_travel ? segment.source_range.end : segment.source_range.start;

    CodomainDelta delta_to_boundary = segment_codomain_boundary - current_c_p;

    if (!std::isfinite(segment.slope)) { // Infinite slope (vertical jump)
      if (delta_to_boundary.raw() * remaining_c_delta.raw() < -EPS) {
        // The jump is in the opposite direction of where we want to go.
        current_domain_p = segment_domain_boundary;
        continue;
      }

      if (std::abs(delta_to_boundary.raw()) >=
          std::abs(remaining_c_delta.raw()) - EPS) {
        // The jump is large enough to satisfy the remaining delta.
        // The domain point does not change for a vertical jump.
        remaining_c_delta = CodomainDelta(0.0);
      } else {
        // Consume the jump and move to the segment boundary.
        remaining_c_delta -= delta_to_boundary;
        current_domain_p = segment_domain_boundary;
      }
      continue;
    }

    if (std::abs(segment.slope) < EPS) { // Zero slope
      // No codomain change is possible in this segment. Move to the boundary.
      current_domain_p = segment_domain_boundary;
      continue;
    }

    // Finite, non-zero slope
    if (delta_to_boundary.raw() * remaining_c_delta.raw() < -EPS) {
      // The segment's codomain moves opposite to our desired direction.
      current_domain_p = segment_domain_boundary;
      continue;
    }

    if (std::abs(delta_to_boundary.raw()) >=
        std::abs(remaining_c_delta.raw()) - EPS) {
      // The remaining delta can be satisfied within this segment.
      CodomainTime final_c_p = current_c_p + remaining_c_delta;
      auto final_d_p_opt = segment.inverse_map_point(final_c_p, true);
      if (!final_d_p_opt) {
        final_d_p_opt = segment.inverse_map_point(final_c_p, false);
      }

      if (final_d_p_opt) {
        current_domain_p = *final_d_p_opt;
      } else {
        // Fallback if inverse map fails (e.g. floating point issues at edge)
        current_domain_p = segment_domain_boundary;
      }
      remaining_c_delta = CodomainDelta(0.0);
      break;
    } else {
      // Consume the delta available in this segment and move to the boundary.
      remaining_c_delta -= delta_to_boundary;
      current_domain_p = segment_domain_boundary;
    }
  }

  return current_domain_p;
}

} // namespace time_transform