#pragma once

#include <cmath>
#include <vector>

#include <eigen3/Eigen/Eigen>

#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>

namespace Trilateration
{
    // see: https://github.com/Wayne82/Trilateration/blob/master/source/Trilateration.cpp

    Point2 calculateLocation2d(const std::vector<Point2>& positions, const std::vector<float>& distances)
    {
        // To locate position on a 2d plan, have to get at least 3 becaons,
        // otherwise return false.
        if (positions.size() < 3)
            assert(false);
        if (positions.size() != distances.size())
            assert(false);

        // Define the matrix that we are going to use
        size_t count = positions.size();
        size_t rows = count * (count - 1) / 2;
        Eigen::MatrixXd m(rows, 2);
        Eigen::VectorXd b(rows);

        // Fill in matrix according to the equations
        size_t row = 0;
        double x1, x2, y1, y2, r1, r2;
        
        for (size_t i = 0; i < count; ++i) {
            for (size_t j = i + 1; j < count; ++j) {
                x1 = positions[i].x, y1 = positions[i].y;
                x2 = positions[j].x, y2 = positions[j].y;
                r1 = distances[i];
                r2 = distances[j];
                m(row, 0) = x1 - x2;
                m(row, 1) = y1 - y2;
                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
                    (pow(y1, 2) - pow(y2, 2)) -
                    (pow(r1, 2) - pow(r2, 2))) / 2;
                row++;
            }
        }

        // Then calculate to solve the equations, using the least square solution
        Eigen::Vector2d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

        return Point2(location.x(), location.y());
    }

    Point3 calculateLocation3d(const std::vector<Point3>& positions, const std::vector<float>& distances)
    {
        // To locate position in a 3D space, have to get at least 4 becaons
        if (positions.size() < 4)
            assert(false);
        if (positions.size() != distances.size())
            assert(false);

        // Define the matrix that we are going to use
        size_t count = positions.size();
        size_t rows = count * (count - 1) / 2;
        Eigen::MatrixXd m(rows, 3);
        Eigen::VectorXd b(rows);

        // Fill in matrix according to the equations
        size_t row = 0;
        double x1, x2, y1, y2, z1, z2, r1, r2;

        for (size_t i = 0; i < count; ++i) {
            for (size_t j = i + 1; j < count; ++j) {
                x1 = positions[i].x, y1 = positions[i].y, z1 = positions[i].z;
                x2 = positions[j].x, y2 = positions[j].y, z2 = positions[j].z;
                r1 = distances[i];
                r2 = distances[j];
                m(row, 0) = x1 - x2;
                m(row, 1) = y1 - y2;
                m(row, 2) = z1 - z2;
                b(row) = ((pow(x1, 2) - pow(x2, 2)) +
                          (pow(y1, 2) - pow(y2, 2)) +
                          (pow(z1, 2) - pow(z2, 2)) -
                          (pow(r1, 2) - pow(r2, 2))) / 2;
                row++;
            }
        }

        // Then calculate to solve the equations, using the least square solution
        Eigen::Vector3d location = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

        return Point3(location.x(), location.y(), location.z());
    }
}