#include "line.hpp"

#include <cstdio>
#include <cassert>

namespace gaia {

  Line::Line() {
  }

  Line::Line(const Vector2 vec1, const Vector2 vec2) {
    SetVertex(0, vec1);
    SetVertex(1, vec2);
  }

  Line::~Line() {
  }


  void Line::SetVertex(unsigned char pos, const float x, const float y) {
    assert(pos < 2);
    vertices[pos].coords[0] = x;
    vertices[pos].coords[1] = y;
  }

  void Line::SetVertex(unsigned char pos, const Vector2 &vec) {
    assert(pos < 2);
    vertices[pos] = vec;
  }

  const Vector2 &Line::GetVertex(unsigned char pos) const {
    assert(pos < 2);
    return vertices[pos];
  }

/*
  const AABB Line::GetAABB() const {
    AABB aabb;

    aabb.SetMinXYZ(Vector2(vertices[0].coords[0], vertices[0].coords[1], vertices[0].coords[2]));
    aabb.SetMaxXYZ(Vector2(vertices[0].coords[0], vertices[0].coords[1], vertices[0].coords[2]));

    for (int i = 1; i < 2; i++) {
      if (vertices[i].coords[0] < aabb.minxyz.coords[0]) aabb.minxyz.coords[0] = vertices[i].coords[0];
      if (vertices[i].coords[0] > aabb.maxxyz.coords[0]) aabb.maxxyz.coords[0] = vertices[i].coords[0];
      if (vertices[i].coords[1] < aabb.minxyz.coords[1]) aabb.minxyz.coords[1] = vertices[i].coords[1];
      if (vertices[i].coords[1] > aabb.maxxyz.coords[1]) aabb.maxxyz.coords[1] = vertices[i].coords[1];
      if (vertices[i].coords[2] < aabb.minxyz.coords[2]) aabb.minxyz.coords[2] = vertices[i].coords[2];
      if (vertices[i].coords[2] > aabb.maxxyz.coords[2]) aabb.maxxyz.coords[2] = vertices[i].coords[2];
    }

    aabb.MakeDirty();

    return aabb;
  }
*/

  float Line::GetDistanceToPoint(const Vector2 &point, float &u) const {
    u = 0.0;
    if (vertices[0] == vertices[1]) return 0;

    float lineDistance = (vertices[1] - vertices[0]).GetLength();
    if (lineDistance < 0.000001f) return (vertices[0] - point).GetLength();

    // u == where on the line is the intersection point, where 0 == v1 and 1 == v2
    u = ((point.coords[0] - vertices[0].coords[0]) * (vertices[1].coords[0] - vertices[0].coords[0]) +
         (point.coords[1] - vertices[0].coords[1]) * (vertices[1].coords[1] - vertices[0].coords[1])) /
        (lineDistance * lineDistance);

    Vector2 intersect;
    intersect.coords[0] = vertices[0].coords[0] + u * (vertices[1].coords[0] - vertices[0].coords[0]);
    intersect.coords[1] = vertices[0].coords[1] + u * (vertices[1].coords[1] - vertices[0].coords[1]);

    return (intersect - point).GetLength();
  }

  Vector2 Line::GetIntersectionPoint(const Line &line) const {
    float u; //dud
    return GetIntersectionPoint(line, u);
  }

  Vector2 Line::GetIntersectionPoint(const Line &line, float &u) const {
    float divisor = (line.GetVertex(1).coords[1] - line.GetVertex(0).coords[1]) * (vertices[1].coords[0] - vertices[0].coords[0]) - (line.GetVertex(1).coords[0] - line.GetVertex(0).coords[0]) * (vertices[1].coords[1] - vertices[0].coords[1]);
    if (divisor == 0.0f) return vertices[0];
    u = ( (line.GetVertex(1).coords[0] - line.GetVertex(0).coords[0]) * (vertices[0].coords[1] - line.GetVertex(0).coords[1]) - (line.GetVertex(1).coords[1] - line.GetVertex(0).coords[1]) * (vertices[0].coords[0] - line.GetVertex(0).coords[0]) ) /
        ( divisor );

    return vertices[0] + (vertices[1] - vertices[0]) * u;
  }

  bool Line::WhatSide(const Vector2 &point) {
    //return ((b.x - a.x)*(c.y - a.y) - (b.y - a.y)*(c.x - a.x)) > 0;
    return ((vertices[1].coords[0] - vertices[0].coords[0]) * (point.coords[1] - vertices[0].coords[1]) - (vertices[1].coords[1] - vertices[0].coords[1]) * (point.coords[0] - vertices[0].coords[0])) > 0;
  }

}
