/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/TSegment2D.h>
#include <mrpt/math/geometry.h>           // distance()
#include <mrpt/serialization/CArchive.h>  // impl of << operator

#include <iostream>

using namespace mrpt::math;

double TSegment2D::length() const { return math::distance(point1, point2); }
double TSegment2D::distance(const TPoint2D& point) const { return std::abs(signedDistance(point)); }
double TSegment2D::signedDistance(const TPoint2D& point) const
{
  // It is reckoned whether the perpendicular line to the TSegment2D which
  // passes through point crosses or not the referred segment,
  // or what is the same, whether point makes an obtuse triangle with the
  // segment or not (being the longest segment one between the point and
  // either end of TSegment2D).
  const double d1 = math::distance(point, point1);
  if (point1 == point2) return d1;

  const double d2 = math::distance(point, point2);
  const double d3 = length();
  const double ds1 = square(d1);
  const double ds2 = square(d2);
  const double ds3 = square(d3);
  if (ds1 > (ds2 + ds3) || ds2 > (ds1 + ds3))
    // Fix sign:
    return std::min(d1, d2) * (TLine2D(*this).signedDistance(point) < 0 ? -1 : 1);
  else
    return TLine2D(*this).signedDistance(point);
}
bool TSegment2D::contains(const TPoint2D& point) const
{
  return std::abs(
             math::distance(point1, point) + math::distance(point2, point) -
             math::distance(point1, point2)) < getEpsilon();
}
void TSegment2D::generate3DObject(TSegment3D& s) const { s = TSegment3D(*this); }
TSegment2D::TSegment2D(const TSegment3D& s)
{
  point1 = TPoint2D(s.point1);
  point2 = TPoint2D(s.point2);
  if (point1 == point2) throw std::logic_error("Segment is normal to projection plane");
}

bool TSegment2D::operator<(const TSegment2D& s) const
{
  if (point1 < s.point1)
    return true;
  else if (s.point1 < point1)
    return false;
  else
    return point2 < s.point2;
}

mrpt::serialization::CArchive& mrpt::math::operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TSegment2D& s)
{
  return in >> s.point1 >> s.point2;
}
mrpt::serialization::CArchive& mrpt::math::operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TSegment2D& s)
{
  return out << s.point1 << s.point2;
}

std::ostream& mrpt::math::operator<<(std::ostream& o, const TSegment2D& p)
{
  o << p.point1 << "-" << p.point2;
  return o;
}
