#include "glm/geometric.hpp"
#include "extension.hpp"
#include "Box.hpp"
#include "Segment.hpp"

Segment::Segment(const glm::vec2& start, const glm::vec2& end)
{
    this->start(start);
    this->end(end);
}

Segment::Segment() : Segment({0, 0}, {0, 0}) {};

Segment::Segment(const glm::vec2& location) : Segment(location, location) {};

Segment::Segment(const Box& start, const Box& end) : Segment(start.center(), end.center()) {};

glm::vec2 Segment::start() const
{
    return start_;
}

void Segment::start(const glm::vec2& start)
{
    start_ = start;
}

glm::vec2 Segment::end() const
{
    return end_;
}

void Segment::end(const glm::vec2& end)
{
    end_ = end;
}

bool Segment::intersect(const Segment& segment, std::optional<std::reference_wrapper<glm::vec2>> intersection) const
{
    float x1 = start_.x, y1 = start_.y, x2 = end_.x, y2 = end_.y, x3 = segment.start_.x,
        y3 = segment.start_.y, x4 = segment.end_.x, y4 = segment.end_.y;

    float a1, a2, b1, b2, c1, c2; // Coefficients of line eqns.
    float r1, r2, r3, r4; // 'Sign' values
    float denom, num; // Intermediate values

    // Compute a1, b1, c1, where line joining points 1 and 2 is "a1 x  +  b1 y  +  c1  =  0"
    a1 = y2 - y1;
    b1 = x1 - x2;
    c1 = x2 * y1 - x1 * y2;

    // Compute r3 and r4
    r3 = a1 * x3 + b1 * y3 + c1;
    r4 = a1 * x4 + b1 * y4 + c1;

    // Check signs of r3 and r4. If both point 3 and point 4 lie on same side of
    // line 1, the line segments do not intersect
    if (r3 != 0 && r4 != 0 && std::copysign(1, r3) == std::copysign(1, r4))
    {
        return false;
    }

    // Compute a2, b2, c2
    a2 = y4 - y3;
    b2 = x3 - x4;
    c2 = x4 * y3 - x3 * y4;

    // Compute r1 and r2
    r1 = a2 * x1 + b2 * y1 + c2;
    r2 = a2 * x2 + b2 * y2 + c2;

    // Check signs of r1 and r2. If both point 1 and point 2 lie on same side
    // of second line segment, the line segments do not intersect
    if (r1 != 0 && r2 != 0 && std::copysign(1, r1) == std::copysign(1, r2))
    {
        return false;
    }

    // Line segments intersect: compute intersection point
    denom = a1 * b2 - a2 * b1;
    if (denom == 0)
    {
        return false;
    }

    num = b1 * c2 - b2 * c1;
    if (intersection.has_value())
    {
        intersection.value().get().x = num / denom;
    }
    num = a2 * c1 - a1 * c2;
    if (intersection.has_value())
    {
        intersection.value().get().y = num / denom;
    }

    return true;
}

float Segment::dx() const
{
    return end_.x - start_.x;
}

float Segment::dy() const
{
    return end_.y - start_.y;
}

float Segment::length() const
{
    return glm::distance(start_, end_);
}

Box Segment::box() const
{
    float x = std::min(start_.x, end_.x);
    float y = std::min(start_.y, end_.y);
    float w = std::abs(dx());
    float h = std::abs(dy());
    return Box({x, y}, {w, h});
}

void Segment::move(const glm::vec2& delta)
{
    start_ += delta;
    end_ += delta;
}

glm::vec2 Segment::center() const
{
    return subsegments(2)[0].end_;
}

float Segment::angle() const
{
    return glm::atan(end_.x - start_.x, end_.y - start_.y);
}

glm::vec2 Segment::step(float distance) const
{
    float angle = this->angle();
    return glm::vec2(distance * glm::sin(angle), distance * glm::cos(angle));
}

glm::vec2 Segment::step_relative(float relative_length_per_step) const
{
    return step(glm::distance(start_, end_) * relative_length_per_step);
}

std::vector<Segment> Segment::subsegments(int count) const
{
    glm::vec2 step = step_relative(1.0f / count);
    std::vector<Segment> subsegments;
    subsegments.reserve(count);
    glm::vec2 subsegment_start = start_, subsegment_end;
    for (int ii = 0; ii < count; ii++)
    {
        subsegment_end = subsegment_start + step;
        subsegments.emplace_back(subsegment_start, subsegment_end);
        subsegment_start = subsegment_end;
    }
    return subsegments;
}

std::ostream& std::operator<<(std::ostream& out, const Segment& segment)
{
    out << "{(" << segment.start().x << ", " << segment.start().y << "), (" <<
        segment.end().x << ", " << segment.end().y << ")}";
    return out;
}