#pragma once
#include "vec.hpp"
#include <vector>
#include <iostream>
#include <memory>
#include <cassert>
#include <cmath>

// class ILineParam {
// public:
//     virtual ~ILineParam() = default;
// };
//
// class PolylineParam : ILineParam {
//     int segIdx;
//     real tOnSeg;
// };
//
// class PolynomialLineParam : ILineParam {
//     real t;
// };

class ILine {
public:
    virtual ~ILine() = default;

    virtual Vec3 eval(const double param) const = 0;

    virtual Vec3 tangent(const double param) const = 0;

    virtual double getClosestParam(const Vec3 &p) const = 0;
};

template<size_t N>
using PtArray = std::vector<Vec<N> >;
using Pt3Array = PtArray<3>;
using Pt2Array = PtArray<2>;


class Polyline : public ILine {
public:
    using Point = Vec<3>;

    Polyline(const Pt3Array &points, const std::vector<double> &bugles, const Vec3 &normal, bool closed = false)
        : points(points), bugles(bugles), closed(closed), normal(normal.normalize()) {
        assert(points.size() >= 2);
        if (closed) {
            assert(points.size() == bugles.size());
        } else {
            assert(points.size() - 1 == bugles.size());
        }
        circleCenters.resize(bugles.size());
        thetas.resize(bugles.size());
        radii.resize(bugles.size());
        for (size_t i = 0; i < bugles.size(); ++i) {
            const Point &A = points[i];
            const Point &B = points[(i + 1) % points.size()];
            Vec3 AB = B - A;
            Vec3 ABNorm = AB.normalize();
            Vec3 QO = normal.cross(ABNorm) * (abs(bugles[i]) > 1 ? -1 : 1);
            float theta = std::atan(bugles[i]) * 4;
            float h = AB.length() * 0.5 * std::tan(theta * 0.5);
            circleCenters[i] = A + AB * 0.5 + QO * h;
            thetas[i] = theta;
            radii[i] = (circleCenters[i] - A).length();
        }
    }

private:
    Pt3Array points;
    std::vector<real> bugles;
    Vec3 normal;
    bool closed;

    Pt3Array circleCenters;
    std::vector<real> thetas;
    std::vector<real> radii;

public:
    Vec3 eval(const real param) const override {
        assert(param >= 0 && param <= bugles.size());
        int seg = static_cast<int>(param);
        real tOnSeg = param - seg;

        const auto &A = points[seg];
        // const auto &B = points[(seg + 1) % points.size()];
        const auto &center = circleCenters[seg];
        Vec3 u = (A-center).normalize();
        Vec3 v = normal.cross(u);

        real phi = tOnSeg * thetas[seg];
        const auto &r = radii[seg];

        return center + r * (u * std::cos(phi) + v * std::sin(phi));
    }

    Vec3 tangent(const double param) const override {
        // TODO:
    }

    double getClosestParam(const Vec3 &p) const override {
        // TODO:
        return closestParam;
    }

    void addPoint(const Point &point) {
        points.push_back(point);
    }

    const Point &getPoint(size_t index) const {
        return points[index];
    }

    size_t size() const {
        return points.size();
    }

    void clear() {
        points.clear();
    }

    void print() const {
        for (const auto &point: points) {
            std::cout << "(";
            for (size_t i = 0; i < N; ++i) {
                std::cout << point[i];
                if (i < N - 1) std::cout << ", ";
            }
            std::cout << ") ";
        }
        std::cout << std::endl;
    }
};

class PolynomialLine : public ILine {
public:
};