PMClassifier/include/line.hpp

#pragma once

#include "common.hpp"
#include "vec.hpp"
#include <algorithm>
#include <cassert>
#include <cmath>
#include <iostream>
#include <limits.h>
#include <memory>
#include <numbers>
#include <vector>

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

struct ClosestDescOnSeg {
    real t;
    real dis;
    ClosestDescOnSeg(real _t, real _dis) : t(_t), dis(_dis) {}
    ClosestDescOnSeg() : t(0), dis(std::numeric_limits<real>::max()) {}
};

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

    virtual Vec3 eval(real param) = 0;

    virtual Vec3 der1(real param) = 0;

    virtual Vec3 der2(real param) = 0;

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

template <typename VecType> // Vec2 or Vec3
struct CircularArc {
    VecType center;
    real radius;
    real theta;
    real h = -1; // straight line for h <= 0.
    VecType u;
    VecType v;
    VecType inCircleDir;
    PtBoundaryRelation inCircleCheck(const VecType &pt) const {
        real d = (pt - center).norm();
        return d < radius ? Inside : d > radius ? Outside : OnBoundary;
    }
};
;

struct AA {
    int a;
    int b;
    void print() const { std::cout << a << " " << b << std::endl; }
};

const real DISC_ARC_ANGLE = std::numbers::pi * 0.125;

class Polyline : public ILine {
public:
    using Point = Vec3;

    Polyline(Pt3Array points, std::vector<real> bugles, const Vec3 &normal, bool closed = false)
        : _points(std::move(points)), _bugles(std::move(bugles)), _closed(closed),
          _normal(normal.normalize()) {
        assert(_points.size() >= 2);
        if (closed) {
            assert(_points.size() == _points.size());
        } else {
            assert(_points.size() - 1 == _points.size());
        }
        circularArcs.resize(_bugles.size());
    }

    [[nodiscard]] const Pt3Array &getPoints() const { return _points; }

    [[nodiscard]] const std::vector<real> &getBugles() const { return _bugles; }

    [[nodiscard]] const Vec3 &getNormal() const { return _normal; }

    [[nodiscard]] bool isClosed() const { return _closed; }

    [[nodiscard]] const std::vector<CircularArc<Vec3>> &getCircularArcs() const {
        return circularArcs;
    }

private:
    Pt3Array _points;
    std::vector<real> _bugles;
    Vec3 _normal;
    bool _closed;

    std::vector<CircularArc<Vec3>> circularArcs;

public:
    void initSegInfo() {
        for (size_t i = 0; i < _bugles.size(); ++i) {
            const Point &A = _points[i];
            const Point &B = _points[(i + 1) % _points.size()];
            Vec3 ABHalf = (B - A) * 0.5;
            Vec3 ABNorm = ABHalf.normalize();
            real theta = std::atan(_bugles[i]) * 4;
            circularArcs[i].inCircleDir = _normal.cross(ABNorm) * (abs(_bugles[i]) > 1 ? -1 : 1);
            circularArcs[i].h = ABHalf.norm() * std::tan(theta * 0.5);
            circularArcs[i].center = A + ABHalf + circularArcs[i].inCircleDir * circularArcs[i].h;
            circularArcs[i].theta = theta;
            circularArcs[i].radius = (circularArcs[i].center - A).norm();
            circularArcs[i].u = (A - circularArcs[i].center).normalize();
            circularArcs[i].v = _normal.cross(circularArcs[i].u);
        }
    }

    Vec3 eval(real param) override {
        if (circularArcs.empty())
            initSegInfo();
        int seg = static_cast<int>(param);
        real tOnSeg = param - seg;
        const auto &arc = circularArcs[seg];
        real phi = tOnSeg * arc.theta;
        return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi));
    }

    Vec3 der1(real param) override {
        if (circularArcs.empty())
            initSegInfo();
        int seg = static_cast<int>(param);
        real tOnSeg = param - seg;
        const auto &arc = circularArcs[seg];
        real phi = tOnSeg * arc.theta;
        return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi));
    }

    Vec3 der2(real param) override {
        if (circularArcs.empty())
            initSegInfo();
        int seg = static_cast<int>(param);
        real tOnSeg = param - seg;
        const auto &arc = circularArcs[seg];
        real phi = tOnSeg * arc.theta;
        return -arc.radius * (arc.u * std::cos(phi) + arc.v * std::cos(phi));
    }

    // ClosestDescOnSeg getClosestParam(const Vec3 &p) override {
    //     real closestDis = std::numeric_limits<real>::max();
    //     real closestParam;
    //     for (int i = 0; i < _bugles.size(); ++i) {
    //         const Vec3 &A = _points[i];
    //         const Vec3 &B = _points[(i + 1) % _points.size()];
    //         const auto &arc = circularArcs[i];
    //         real dis2Seg = segPtDist(p, A, B).dis;
    //         if (dis2Seg - arc.h > closestDis)
    //             continue;
    //         if ((A - p).norm() < closestDis) {
    //             closestDis = (A - p).norm();
    //             closestParam = i;
    //         }
    //         if ((B - p).norm() < closestDis) {
    //             closestDis = (B - p).norm();
    //             closestParam = i + 1;
    //         }
    //         int segInsertedCnt = arc.theta / DISC_ARC_ANGLE;
    //         for (int j = 0; j < segInsertedCnt; ++j) {
    //             real insertParam = i + j * DISC_ARC_ANGLE / arc.theta;
    //             const Vec3 insertPt = eval(insertParam);
    //             real dis2InsertPt = (p - insertPt).norm();
    //             if (dis2InsertPt < closestDis) {
    //                 closestDis = dis2InsertPt;
    //                 closestParam = insertParam;
    //             }
    //         }
    //     }
    //     // TODO: 为了鲁棒和精度，应该在每个可能最近的seg上做newton iteration
    //     int seg = static_cast<int>(closestParam);
    //     // Q = arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v *
    //     // std::sin(phi)) d2 = (Q - p)^2
    //     Vec3 q = eval(closestParam);
    //     Vec3 qDer1 = der1(closestParam);
    //     Vec3 qDer2 = der2(closestParam);
    //     real lDer1 = (q - p).dot(qDer1);
    //     int iter = 0;
    //     while (fabs(lDer1) > std::numeric_limits<real>::epsilon() * 1e6) {
    //         closestParam -= lDer1 / (qDer1.dot(qDer1) + (q - p).dot(qDer2)); // -der1 / der2
    //         q = eval(closestParam);
    //         qDer1 = der1(closestParam);
    //         qDer2 = der2(closestParam);
    //         lDer1 = (q - p).dot(qDer1);
    //         printf("After iter %d, dL is %lf\n", iter, lDer1);
    //         if (closestParam < seg - std::numeric_limits<real>::epsilon()) {
    //             closestParam = seg;
    //             closestDis = (_points[seg] - p).norm();
    //             break;
    //         }
    //         if (closestParam > seg + 1 + std::numeric_limits<real>::epsilon()) {
    //             closestParam = seg + 1;
    //             closestDis = (_points[(seg + 1) % _points.size()] - p).norm();
    //             break;
    //         }
    //         closestDis = (q - p).norm();
    //         iter++;
    //     }
    //     return {closestParam, closestDis};
    // }

    ClosestDescOnSeg getClosestParam(const Vec3 &p) override {
        ClosestDescOnSeg closestDes{};
        for (int i = 0; i < _bugles.size(); ++i) {
            const Vec3 &a = _points[i];
            const Vec3 &b = _points[(i + 1) % _points.size()];
            const CircularArc<Vec3> &arc = circularArcs[i];
            const Vec3 &o = arc.center;
            // p 到圆弧平面的投影
            const Vec3 projPt = p - _normal.dot(p - a) * _normal;
            // projPt到圆的最近点
            const Vec3 clsPtOnCircle = o + arc.radius * (projPt - o).normalize();
            if ((clsPtOnCircle - a).dot(arc.inCircleDir) > 0) {
                // 在圆弧上
                real dis = (p - clsPtOnCircle).norm();
                if (dis < closestDes.dis) {
                    closestDes.dis = dis;
                    Vec3 oa = a - o;
                    Vec3 oClsPt = clsPtOnCircle - o;
                    real R2 = arc.radius * arc.radius;
                    real cosTheta = (oa).dot(oClsPt) / R2;
                    real theta = std::acos(cosTheta); // [0, pi]
                    if ((oa.cross(oClsPt)).dot(_normal) < 0) {
                        theta = 2 * std::numbers::pi - theta;
                    }
                    closestDes.t = i + theta / arc.theta;
                }
                continue;
            }
            real paDis = (p - a).norm();
            real pbDis = (p - b).norm();
            if (paDis < closestDes.dis) {
                closestDes.dis = paDis;
                closestDes.t = i;
            } else {
                closestDes.dis = pbDis;
                closestDes.t = i + 1;
            }
        }
        return closestDes;
    }

    void print() const {
        if (_closed)
            std::cout << "Closed Polyline: \n";
        else
            std::cout << "Open Polyline: \n";
        std::cout << "Points: {\n";
        for (int i = 0; i < _points.size(); ++i) {
            std::cout << _points[i].x() << ", " << _points[i].y() << ", " << _points[i].z() << ">";
            if (i != _points.size() - 1)
                std::cout << ", ";
        }
        std::cout << "}\n";
        std::cout << "的可变参数Bugles: {\n";
        for (int i = 0; i < _bugles.size(); ++i) {
            std::cout << _bugles[i];
            if (i != _bugles.size() - 1)
                std::cout << ", ";
        }
        std::cout << "}\n";
    }
    static ClosestDescOnSeg segPtDist(const Vec3 &p, const Vec3 &A, const Vec3 &B) {
        Vec3 AB = B - A;
        Vec3 AP = p - A;
        real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.);
        return {h, (AP - AB * h).norm()};
    }

    static ClosestDescOnSeg segPtDist(const Vec2 &p, const Vec2 &A, const Vec2 &B) {
        Vec2 AB = B - A;
        Vec2 AP = p - A;
        real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.);
        return {h, (AP - AB * h).norm()};
    }

    bool isEndParm(real t) { return t < EPS || t > _points.size() - 1 - EPS; }
};

class HelixLine : public ILine {
public:
    Vec3 eval(real param) override { return {}; };

    Vec3 der1(real param) override { return {}; };

    Vec3 der2(real param) override { return {}; };

    ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; };

private:
    Vec3 axisStart, axisDir;
    real advanceLen, advancePerRound;
    real startTheta; // 轴线起始时在螺旋投影（圆）上的参数
};

class SingleLine : public ILine {
public:
};

class PolynomialLine : public ILine {
public:
    Vec3 eval(real param) override { return {}; };

    Vec3 der1(real param) override { return {}; };

    Vec3 der2(real param) override { return {}; };

    ClosestDescOnSeg getClosestParam(const Vec3 &p) override { return {}; };
};