#include "algorithms.h"
#include "core/math_helper.hpp"

using namespace Eigen;

fitting_circle_2d::result_type
fitting_circle_2d::operator()(const input_type &input) {
    const auto N = input.points.cols();
    auto A = MatrixX3f(N, 3);
    A.leftCols<2>() = input.points.transpose();
    A.col(2).setOnes();

    const auto x = A.col(0).array(), y = A.col(1).array();
    VectorXf b = x.pow(2) + y.pow(2);

    if (input.weights) {
        const auto w = input.weights->array();
        A.array().colwise() *= w;
        b.array().colwise() *= w;
    }

    const auto solver = ColPivHouseholderQR<MatrixX3f>(A);
    const auto c = solver.solve(b);
    const auto center = Vector2f(c[0] / 2, c[1] / 2);
    auto ret = result_type();
    ret.center = to_vec2(center);
    ret.radius = sqrtf(c[2] + center.squaredNorm());

    auto err = VectorXf(N);
    for (auto k = 0; k < N; ++k) {
        auto point = input.points.col(k);
        err[k] = ret.radius - (center - point).norm();
    }
    ret.error_rms = sqrtf(err.array().square().sum() / N);

    return ret;
}

namespace {
    MatrixX3f rodrigues_rot(const MatrixX3f &points,
                            Vector3f n0, Vector3f n1) {
        const auto N = points.rows();
        n0.normalize();
        n1.normalize();
        const auto n2 = n0.cross(n1).normalized();
        const auto theta = acos(n0.dot(n1));
        const auto cos_theta = cos(theta);
        const auto sin_theta = sin(theta);

        auto ret = MatrixX3f(N, 3);
        for (auto k = 0; k < N; k++) {
            auto p = points.row(k).transpose();
            ret.row(k) = p * cos_theta
                         + n2.cross(p) * sin_theta
                         + n2 * n2.dot(p) * (1 - cos_theta);
        }
        return ret;
    }

    Vector3f rodrigues_rot(const Vector3f &vec,
                           const Vector3f &n0, const Vector3f &n1) {
        auto matrix = MatrixX3f(1, 3);
        matrix.row(0) = vec;
        auto ret = rodrigues_rot(matrix, n0, n1);
        return ret.row(0);
    }
}

fitting_circle_3d::result_type
fitting_circle_3d::operator()(const input_type &input) {
    auto ret = result_type();

    MatrixX3f P = input.points.transpose();
    const Vector3f mean = P.colwise().mean();
    P.array().rowwise() -= mean.array().transpose();

    // Calculate the SVD of the centered points
    const auto svd = JacobiSVD(P, ComputeFullV);
    const auto normal = svd.matrixV().col(2);
    ret.axis = to_vec3(normal);

    // Fit a circle to the rotated points
    auto P_xy = rodrigues_rot(P, normal, Vector3f::UnitZ());
    auto input_2d = fitting_circle_2d::input_type();
    input_2d.points = P_xy.leftCols<2>().transpose();
    input_2d.weights = input.weights;
    const auto ret_2d = fitting_circle_2d()(input_2d);
    ret.radius = ret_2d.radius;
    ret.error_rms = ret_2d.error_rms;

    auto center = Vector3f();
    center << ret.center.x, ret.center.y, 0;
    center = rodrigues_rot(center, Vector3f::UnitZ(), normal);
    center += mean;
    ret.center = to_vec3(center);
    return ret;
}