#include <iostream>
#include <array>

#include <environment.h>
#include <execution.h>
#include <io.h>

#include <construct_helper.hpp>
#include "Eigen/Core"
#include "primitive_descriptor.h"
#include "internal_primitive_desc.hpp"

void testExtrudeSDFXoY()
{
    printf("Testing extrude polyline SDF XoY\n");
    auto profilePoints = std::array{
        raw_vector3d_t{-1., 0., 0.},
        raw_vector3d_t{1.,  0., 0.}
    };
    auto                  profileBulges = std::array{1.0, 1.0};
    polyline_descriptor_t profile{
        2,
        profilePoints.data(),
        2,
        profileBulges.data(),
        raw_vector3d_t{0., 0., 1.},
        true
    };
    auto axisPoints = std::array{
        raw_vector3d_t{1., 0., 0.},
        raw_vector3d_t{1., 0., 2.}
    };
    auto                  axisBulges = std::array{0.0};
    polyline_descriptor_t axis{
        2,
        axisPoints.data(),
        1,
        axisBulges.data(),
        raw_vector3d_t{0., 1., 0.},
        false
    };

    extrude_polyline_descriptor_t extrude{1, &profile, axis};

    aabb_t<>                   aabb{};
    internal::extrude_polyline extrude_polyline{extrude, aabb};
    // Eigen::Vector3d            p{1., 1., -0.1};
    // Eigen::Vector3d            p{0.3, 0.5, 1.9};
    Eigen::Vector3d            p{.7, .8, 0.6};
    double                     res           = extrude_polyline.evaluate_sdf(p);
    Eigen::Vector3d            closest_point = extrude_polyline.evaluate_cpm(p);
    printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",
           p.x(),
           p.y(),
           p.z(),
           res,
           closest_point.x(),
           closest_point.y(),
           closest_point.z());
}

void testExtrudeSDFZoX()
{
    printf("Testing extrude polyline SDF ZoX\n");
    auto profilePoints = std::array{
        raw_vector3d_t{3., 1., 2.},
        raw_vector3d_t{1., 1., 2.},
        // raw_vector3d_t{1., 1., 4.}
    };
    auto                  profileBulges = std::array{1.0, 1.0};
    polyline_descriptor_t profile{
        2,
        profilePoints.data(),
        2,
        profileBulges.data(),
        raw_vector3d_t{0., 1., 0.},
        true
    };
    auto axisPoints = std::array{
        raw_vector3d_t{3., 1., 1.5},
        raw_vector3d_t{3., 3., 1.5}
    };
    auto                  axisBulges = std::array{0.0};
    polyline_descriptor_t axis{
        2,
        axisPoints.data(),
        1,
        axisBulges.data(),
        raw_vector3d_t{0., 0., 1.},
        false
    };

    extrude_polyline_descriptor_t extrude{1, &profile, axis};

    aabb_t<>                   aabb{};
    internal::extrude_polyline extrude_polyline{extrude, aabb};
    // Eigen::Vector3d            p{1., 1., -0.1};
    Eigen::Vector3d            p{2.6, 2.9, 1.2};
    double                     res           = extrude_polyline.evaluate_sdf(p);
    Eigen::Vector3d            closest_point = extrude_polyline.evaluate_cpm(p);
    printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",
           p.x(),
           p.y(),
           p.z(),
           res,
           closest_point.x(),
           closest_point.y(),
           closest_point.z());
}

void testPolylineSDF()
{
    printf("Testing polyline SDF\n");
    auto points = std::array{
        raw_vector3d_t{-1., 1., 0.},
        raw_vector3d_t{1.,  1., 0.}
    };
    // auto bulges = std::array{0.41421356237309503, 1.0}; // 0.41421356237309503 is tan(PI/8)
    auto                  bulges = std::array{-1., -1.0}; // 0.41421356237309503 is tan(PI/8)
    polyline_descriptor_t polyline{
        2,
        points.data(),
        2,
        bulges.data(),
        raw_vector3d_t{0., 0., 1.},
        true
    };

    aabb_t<2>          aabb{};
    internal::polyline internal_polyline;
    // Eigen::Transform<double, 3, Eigen::AffineCompact> axis_to_world{};
    // const auto& matrix_handle = axis_to_world.matrix();
    // const Eigen::Matrix4d matrix_handle = Eigen::Matrix4d::Identity();
    internal_polyline.build_as_profile(polyline,
                                       Eigen::Vector3d{1., 0., 0.},
                                       Eigen::Vector3d{0., 1., 0.},
                                       Eigen::Vector3d{0., 0., 0.},
                                       aabb);
    std::array testPts = {
        // Eigen::Vector3d{-1., 0., 0.},
        // Eigen::Vector3d{0.,  0., 0.},
        // Eigen::Vector3d{1.,  0., 0.},
        // Eigen::Vector3d{1.,  1., 0.},
        // Eigen::Vector3d{1.,  2., 0.},
        Eigen::Vector3d{0., 1., 0.},

        // Eigen::Vector3d{-1., 0., 1.},
        // Eigen::Vector3d{0.,  0., 1.},
        // Eigen::Vector3d{1.,  0., 1.},
        // Eigen::Vector3d{1.,  1., 1.},
        // Eigen::Vector3d{1.,  2., 1.},
        // Eigen::Vector3d{0.,  1., 1.},
    };
    for (const auto &pt : testPts) {
        auto [axis_closest_param, distance] = internal_polyline.calculate_closest_param(pt);
        printf("For query point (%f, %f, %f), closest param: %f closest point (%f, %f, %f), distance: %f\n",
               pt.x(),
               pt.y(),
               pt.z(),
               axis_closest_param.t,
               axis_closest_param.point.x(),
               axis_closest_param.point.y(),
               axis_closest_param.point.z(),
               distance);
    }
}

void testHex()
{
    std::vector<raw_vector3d_t> points;
    std::vector<double>         bulges;
    points.push_back({-10800.000000000031, -7479.9999999988240, 0.0000000000000000});
    points.push_back({-7200.0000000000291, -7479.9999999988240, 0.0000000000000000});
    points.push_back({-7200.0000000000291, -7719.9999999988240, 0.0000000000000000});
    points.push_back({-10800.000000000031, -7719.9999999988240, 0.0000000000000000});
    bulges.push_back(0.0000000000000000);
    bulges.push_back(0.0000000000000000);
    bulges.push_back(0.0000000000000000);
    bulges.push_back(0.0000000000000000);
    polyline_descriptor_t profile{
        4,
        points.data(),
        4,
        bulges.data(),
        raw_vector3d_t{0., 0., 1.},
        true
    };
    auto axisEnd                     = points[0];
    axisEnd.z                        = 78.000000000471118;
    auto                  axisPoints = std::array{points[0], axisEnd};
    auto                  axisBulges = std::array{0.0};
    polyline_descriptor_t axis{
        2,
        axisPoints.data(),
        1,
        axisBulges.data(),
        raw_vector3d_t{0., 1., 0.},
        false
    };
    extrude_polyline_descriptor_t extrude{1, &profile, axis};
    aabb_t<>                      aabb{};
    internal::extrude_polyline    extrude_polyline{extrude, aabb};

    std::vector<Eigen::Vector3d> queryPoints = {
        {0.,    0.,    0. },
        {-8000, 0,     10 },
        {-8000, 0,     -10},
        {-8000, -7500, 10 },
        {-8000, -7481, 10 },
        {-7202, -7500, 10 }
    };
    for (const auto &queryPoint : queryPoints) {
        double          res           = extrude_polyline.evaluate_sdf(queryPoint);
        Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(queryPoint);
        printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",
               queryPoint.x(),
               queryPoint.y(),
               queryPoint.z(),
               res,
               closest_point.x(),
               closest_point.y(),
               closest_point.z());
    }
}

int main()
{
    // testExtrudeSDFZoX();
    testHex();
    // testPolylineSDF();

    // std::cout << "Setting scene..." << std::endl;
    // sphere_descriptor_t sphere1{
    //     {.0, .0, .0},
    //     0.5
    // };
    // sphere_descriptor_t sphere2{
    //     {.01, .0, .0},
    //     0.5
    // };
    // box_descriptor_t box{
    //     {0., 0., 0.},
    //     {1., 1., 1.}
    // };
    // auto points = std::array{
    //     raw_vector3d_t{-7200.0000000000282, -7479.9999999993715, 0.0},
    //     raw_vector3d_t{-4420.0000000000000, -7479.9999999993724, 0.0},
    //     raw_vector3d_t{-4420.0000000000000, -7719.9999999993724, 0.0},
    //     raw_vector3d_t{-7200.0000000000282, -7719.9999999993715, 0.0}
    // };
    // // auto                 buldges = std::array{0.0, 0.0, 0.0, 0.0};
    // // extrude_descriptor_t extrude{
    // //     static_cast<uint32_t>(buldges.size()),
    // //     raw_vector3d_t{0.0, 0.0, 78.000000000251021},
    // //     points.data(),
    // //     buldges.data()
    // // };
    // // auto tree_root = blobtree_new_node(&sphere1, PRIMITIVE_TYPE_SPHERE);
    // // auto tree_root = blobtree_new_node(&box, PRIMITIVE_TYPE_BOX);
    // // auto tree_root = blobtree_new_node(&sphere1, PRIMITIVE_TYPE_SPHERE);
    // // auto another_sphere_node = blobtree_new_node(&sphere2, PRIMITIVE_TYPE_SPHERE);
    // // virtual_node_boolean_union(&tree_root, &another_sphere_node);
    // auto tree_root = make_primitive_node_by_move(box);
    // // auto tree_root = make_primitive_node_by_move(extrude);

    // std::cout << "Setting environments..." << std::endl;
    // setting_descriptor setting_desc{21, 1e-5};
    // update_setting(setting_desc);
    // update_environment(&tree_root);

    // std::cout << "Executing solver..." << std::endl;
    // auto result = execute_solver(&tree_root);
    // std::cout << "Surface integral result: " << result.surf_int_result << std::endl;
    // std::cout << "Volume integral result: " << result.vol_int_result << std::endl;

    // std::cout << "Time statistics: " << std::endl;
    // print_statistics();

    return 0;
}