/**
 * File: lineSegment2d.cpp
 *
 * Author: Michael Willett
 * Created: 2017-10-12
 *
 * Description: A class that holds a 2d line segment for fast intersection checks 
 *              in point-point form: 
 *                    Δy(x - x₁) - Δx(y - y₁) = 0
 *
 * Copyright: Anki, Inc. 2017
 *
 **/


#include "coretech/common/engine/math/lineSegment2d.h"
#include "util/logging/logging.h"


namespace Anki {

// cache useful data for minor speedup of intersection checks
LineSegment::LineSegment(const Point2f& from, const Point2f& to) 
: AffineHyperplane( Point2f(from.y() - to.y(),to.x() - from.x()), (from.x()-to.x()) * to.y() + (to.y() - from.y()) * to.x() )
, from(from)
, to(to)
, minX(fmin(from.x(), to.x())) 
, maxX(fmax(from.x(), to.x())) 
, minY(fmin(from.y(), to.y())) 
, maxY(fmax(from.y(), to.y()))
, dX(to.x() - from.x())
, dY(to.y() - from.y()) {}

inline bool LineSegment::InBoundingBox(const Point2f& p) const
{
  return (p.x() <= maxX && p.x() >= minX && p.y() <= maxY && p.y() >= minY);
}

inline bool LineSegment::Contains(const Point2f& p) const
{
  return InBoundingBox(p) && (Orientation(p) == EOrientation::COLINEAR);
}

inline bool LineSegment::InHalfPlane(const Halfplane2f& H) const
{
  return ( H.Contains(to) && H.Contains(from) );
}

inline float LineSegment::Evaluate(const Point2f& p) const
{ 
  return (dY * (p.x() - from.x()) - dX * (p.y() - from.y()));
}

bool LineSegment::IntersectsWith(const LineSegment& l) const
{
  // Find the four orientations needed for general and special cases
  EOrientation o1 = Orientation(l.from);
  if (o1 == EOrientation::COLINEAR && InBoundingBox(l.from)) 
  { 
    return true;
  }
  
  EOrientation o2 = Orientation(l.to);
  if (o2 == EOrientation::COLINEAR && InBoundingBox(l.to))
  { 
    return true;
  }
  
  if (o1 == o2)  // both points of test segment are on the same side of this line
  {
    return false;
  }
  
  EOrientation o3 = l.Orientation(from);
  if (o3 == EOrientation::COLINEAR && l.InBoundingBox(from)) 
  { 
    return true;
  }
  
  EOrientation o4 = l.Orientation(to);
  if (o4 == EOrientation::COLINEAR && l.InBoundingBox(to)) 
  { 
    return true;
  }
  
  if (o3 == o4) // both points of this segment are on the same side of test line
  {
    return false;
  }
  
  return true; // Doesn't fall in any of the above cases
}

LineSegment::EOrientation LineSegment::Orientation(const Point2f& p) const
{
  const float val = Evaluate(p);
  if (NEAR_ZERO(val))
  {
    return EOrientation::COLINEAR;  // point is on the line generated by the segment
  } 
  else if (FLT_GE_ZERO(val)) 
  {
    return EOrientation::CW;        // point is to the right
  } 
  else 
  {
    return EOrientation::CCW;       // point is to the left
  }
}

bool LineSegment::IntersectsAt(const LineSegment& l, Point2f& location) const
{
  // shortcutting will stop GetInterSectionPoint from running if the line-segments do not
  // intersect, even if the infinite lines defined by the two points do. If you want
  // the intersection of the infinite lines, call `GeGetIntersectionPoint` directly.
  return IntersectsWith(l) && GetIntersectionPoint(*this, l, location);
}

Line2f LineSegment::GetPerpendicularBisector() const
{
  Point2f mid = (from + to) * .5f;
  return Line2f( {dX, dY}, -dX * mid.x() - dY * mid.y() );
}


} // namespace Anki