#include <Adaptor3d_CurveOnSurface.hxx>
#include <BRepCheck_Analyzer.hxx>
#include <BRepTools.hxx>
#include <BRep_Builder.hxx>
#include <CPnts_AbscissaPoint.hxx>
#include <Geom2dAPI_PointsToBSpline.hxx>
#include <Geom2dAdaptor_HCurve.hxx>
#include <Geom2d_BSplineCurve.hxx>
#include <GeomAPI_PointsToBSpline.hxx>
#include <GeomAdaptor_Curve.hxx>
#include <GeomAdaptor_HSurface.hxx>
#include <GeomLib.hxx>
#include <Geom_BSplineCurve.hxx>
#include <Precision.hxx>
#include <ShapeAnalysis_Curve.hxx>
#include <TColgp_HArray1OfPnt.hxx>
#include <TColgp_HArray1OfPnt2d.hxx>
#include <TopExp_Explorer.hxx>
#include <TopoDS.hxx>
#include <TopoDS_Face.hxx>

#include <iostream>
#include <vector>

gp_Pnt getSpacePoint(const TopoDS_Face& face, const gp_Pnt2d& point)
{
    const Handle(Geom_Surface) surface = BRep_Tool::Surface(face);

    gp_Pnt spacePoint;
    surface->D0(point.X(), point.Y(), spacePoint);

    return spacePoint;
}

std::vector<gp_Pnt2d> get2dPoints()
{
    std::vector<gp_Pnt2d> points;

    points.push_back(gp_Pnt2d(0.4999999999988816, 105.4014098415948));
    points.push_back(gp_Pnt2d(0.4320264616075457, 100.0423698065018));
    points.push_back(gp_Pnt2d(0.2939892671201626, 91.74565262561642));
    points.push_back(gp_Pnt2d(0.1066991623666981, 84.47664193670857));
    points.push_back(gp_Pnt2d(-0.06172436566722802, 80.77902763327997));
    points.push_back(gp_Pnt2d(-0.2065650745679524, 79.3243125043185));
    points.push_back(gp_Pnt2d(-0.3708925637386218, 79.42317053607587));
    points.push_back(gp_Pnt2d(-0.5304808436616609, 81.31403379707817));
    points.push_back(gp_Pnt2d(-0.685040759895962, 85.03428315131636));
    points.push_back(gp_Pnt2d(-0.8360996701423912, 90.88831418622379));
    points.push_back(gp_Pnt2d(-0.9712280657449973, 98.60642504358016));
    points.push_back(gp_Pnt2d(-1.107473340201843, 109.8128499136364));
    points.push_back(gp_Pnt2d(-1.235287222620387, 125.2176218856868));
    points.push_back(gp_Pnt2d(-1.336694214516567, 143.1292577640535));
    points.push_back(gp_Pnt2d(-1.422279617817822, 165.3530615092768));
    points.push_back(gp_Pnt2d(-1.482736871704577, 188.4800074377642));
    points.push_back(gp_Pnt2d(-1.517600666986478, 207.1284783455371));
    points.push_back(gp_Pnt2d(-1.539076109744018, 222.0802673453285));
    points.push_back(gp_Pnt2d(-1.552689937266964, 233.7871752000689));
    points.push_back(gp_Pnt2d(-1.563053751322408, 244.4757708002793));
    points.push_back(gp_Pnt2d(-1.571039287405315, 254.2613218922325));
    points.push_back(gp_Pnt2d(-1.577281393834402, 263.2876678184592));
    points.push_back(gp_Pnt2d(-1.582247816786106, 271.7255934531588));
    points.push_back(gp_Pnt2d(-1.586292499748858, 279.7878539164695));
    points.push_back(gp_Pnt2d(-1.589719915784794, 287.8274015592376));
    points.push_back(gp_Pnt2d(-1.592984242822523, 296.9869611980702));
    points.push_back(gp_Pnt2d(-1.59556463714567, 305.7145935695123));
    points.push_back(gp_Pnt2d(-1.596881417258233, 310.8260663225046));
    points.push_back(gp_Pnt2d(-1.59844230048339, 317.5416783018154));
    points.push_back(gp_Pnt2d(-1.600092911512606, 325.3952168790041));
    points.push_back(gp_Pnt2d(-1.601736056841309, 333.7885771313531));
    points.push_back(gp_Pnt2d(-1.603525231347935, 343.2349115827637));
    points.push_back(gp_Pnt2d(-1.605312846923844, 352.7396608452729));
    points.push_back(gp_Pnt2d(-1.605465872322137, 353.5533905929998));

    return points;
}

bool checkCurveDistance(const Handle(Geom_BSplineCurve)& curve1,
        const Handle(Geom_BSplineCurve)& curve2)
{
    constexpr int numberOfCheckPoints = 100;
    constexpr double distanceTrigger = 1.0;

    for (int index = 0; index < numberOfCheckPoints; ++index)
    {
        const double parameter =
                curve1->FirstParameter() +
                (curve1->LastParameter() - curve1->FirstParameter()) *
                        static_cast<double>(index) /
                        static_cast<double>(numberOfCheckPoints - 1);

        const gp_Pnt curve1Point = curve1->Value(parameter);

        gp_Pnt projection;
        ShapeAnalysis_Curve analyzer;
        double curve2Parameter;

        analyzer.Project(curve2, curve1Point, Precision::Confusion(), projection,
                curve2Parameter, Standard_False);

        std::cout << "Distance: " << curve1Point.Distance(projection) << std::endl;

        if (curve1Point.Distance(projection) > distanceTrigger)
        {
            std::cerr << "Point on curve 1 is far away from curve 2." << std::endl;
            return false;
        }
    }

    return true;
}

int main(int, char**)
{
    BRep_Builder builder;

    TopoDS_Face face;

    {
        TopoDS_Shape shape;
        bool result = BRepTools::Read(shape, "Face.brep", builder);

        if (!result)
        {
            std::cerr << "There was a problem when reading in the BREP file."
                                << std::endl;
            exit(1);
        }

        for (TopExp_Explorer faceExplorer(shape, TopAbs_FACE); faceExplorer.More();
                 faceExplorer.Next())
        {
            face = TopoDS::Face(faceExplorer.Current());
            break;
        }

        if (face.IsNull())
        {
            std::cerr << "Face is null." << std::endl;
            exit(1);
        }

        BRepCheck_Analyzer faceAnalyzer(face);

        if (!faceAnalyzer.IsValid())
        {
            std::cerr << "Face is invalid." << std::endl;
            exit(1);
        }
    }

    const std::vector<gp_Pnt2d> points = get2dPoints();

    // Now interpolate on the surface and in space.

    TColgp_HArray1OfPnt2d poles(1, static_cast<Standard_Integer>(points.size()));
    TColgp_HArray1OfPnt poles3d(1, static_cast<Standard_Integer>(points.size()));

    for (size_t index = 0; index < points.size(); ++index)
    {
        poles.SetValue(index + 1, points.at(index));
        poles3d.SetValue(index + 1, getSpacePoint(face, points.at(index)));
    }

    Geom2dAPI_PointsToBSpline interpolator(poles);
    GeomAPI_PointsToBSpline interpolatorInSpace(poles3d);

    if (!interpolator.IsDone())
    {
        std::cerr << "Interpolation on face has failed." << std::endl;
        exit(1);
    }

    if (!interpolatorInSpace.IsDone())
    {
        std::cerr << "Interpolation in space has failed." << std::endl;
        exit(1);
    }

    Handle(Geom_BSplineCurve) curveOnFace;
    double maxDeviation;
    double averageDeviation;

    const Handle(Geom2dAdaptor_HCurve) curve2dAdaptor =
            new Geom2dAdaptor_HCurve(interpolator.Curve());
    const Handle(Geom_Surface) surface = BRep_Tool::Surface(face);
    const Handle(GeomAdaptor_HSurface) surfaceAdaptor =
            new GeomAdaptor_HSurface(surface);
    Adaptor3d_CurveOnSurface curveOnSurface(curve2dAdaptor, surfaceAdaptor);

    GeomLib::BuildCurve3d(Precision::Intersection(), curveOnSurface,
            curveOnSurface.FirstParameter(), curveOnSurface.LastParameter(),
            curveOnFace, maxDeviation, averageDeviation);

    const double lengthOnFace =
            CPnts_AbscissaPoint::Length(GeomAdaptor_Curve(curveOnFace));
    const double lengthInSpace = CPnts_AbscissaPoint::Length(
            GeomAdaptor_Curve(interpolatorInSpace.Curve()));

    std::cout << "Length of curve on face: " << lengthOnFace
                        << " length of curve in space: " << lengthInSpace << std::endl;

    const bool distanceCheckResult1 =
            checkCurveDistance(interpolatorInSpace.Curve(), curveOnFace);

    if (!distanceCheckResult1)
    {
        exit(1);
    }

    const bool distanceCheckResult2 =
            checkCurveDistance(curveOnFace, interpolatorInSpace.Curve());

    if (!distanceCheckResult2)
    {
        exit(1);
    }

    return 0;
}