#include "dock_planner/utils/line_common.h"

double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2)
{
  double A = y2 - y1;
  double B = -(x2 - x1);
  double C = -A * x1 - B * y1;

  return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B);
}

bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance)
{
  double crossProduct = distanceFromPointToLine(x0,  y0, x1, y1, x2, y2);
  if (fabs(crossProduct) > fabs(tolerance)) 
    return false;

  // Kiểm tra tọa độ của P có nằm giữa A và B không
  if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) &&
      y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2))
  {
    return true;
  }

  return false;
}