#pragma once
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

namespace algorithm{
    template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
    class Functor
    {
    private:
        int m_inputs, m_values;
    public:
        typedef _Scalar Scalar;
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
        typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
        typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }
        int values() const { return m_values; }

    };

    class TDOAFunctor : public Functor<double>
    {
    private:
        Eigen::MatrixXd _data, _pos;
    public:
        TDOAFunctor(const Eigen::MatrixXd &data,const Eigen::MatrixXd &pos): Functor<double>(3,3),_data(data),_pos(pos) {}
        int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
        {
            for (int i = 0; i < values(); i++)
            {
                fvec(i) = sqrt(pow(_data(i,0) - x(0),2) + pow(_data(i,1) - x(1),2) + pow(_data(i,2) - x(2),2)) 
                        - sqrt(pow(_pos(0) - x(0),2) + pow(_pos(1) - x(1),2) + pow(_pos(2) - x(2),2)) 
                        - _data(i,3);
            }
            return 0;
        }
    };

	class TofFunctor : public Functor<double>
	{
	private:
		Eigen::MatrixXd _data;
	public:
		TofFunctor(const Eigen::MatrixXd &data): Functor<double>(3,3),_data(data){}
		int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
		{
			for (int i = 0; i < values(); i++)
			{
				double x1 = _data(i,0);
				double y1 = _data(i,1);
				fvec(i) = pow(_data(i,0) - x(0),2) + pow(_data(i,1) - x(1),2) - _data(i,3);
			}
			return 0;
		}
	};
};