RBFSpline::RBFSpline(const DataTable &samples, RadialBasisFunctionType type, bool normalized)
    : samples(samples),
      normalized(normalized),
      precondition(false),
      dim(samples.getNumVariables()),
      numSamples(samples.getNumSamples())
{
    if (type == RadialBasisFunctionType::THIN_PLATE_SPLINE)
    {
        fn = std::shared_ptr<RadialBasisFunction>(new ThinPlateSpline());
    }
    else if (type == RadialBasisFunctionType::MULTIQUADRIC)
    {
        fn = std::shared_ptr<RadialBasisFunction>(new Multiquadric());
    }
    else if (type == RadialBasisFunctionType::INVERSE_QUADRIC)
    {
        fn = std::shared_ptr<RadialBasisFunction>(new InverseQuadric());
    }
    else if (type == RadialBasisFunctionType::INVERSE_MULTIQUADRIC)
    {
        fn = std::shared_ptr<RadialBasisFunction>(new InverseMultiquadric());
    }
    else if (type == RadialBasisFunctionType::GAUSSIAN)
    {
        fn = std::shared_ptr<RadialBasisFunction>(new Gaussian());
    }
    else
    {
        fn = std::shared_ptr<RadialBasisFunction>(new ThinPlateSpline());
    }

    /* Want to solve the linear system A*w = b,
     * where w is the vector of weights.
     * NOTE: the system is dense and by default badly conditioned.
     * It should be solved by a specialized solver such as GMRES
     * with preconditioning (e.g. ACBF) as in matlab.
     * NOTE: Consider trying the Łukaszyk–Karmowski metric (for two variables)
     */
    //SparseMatrix A(numSamples,numSamples);
    //A.reserve(numSamples*numSamples);
    DenseMatrix A; A.setZero(numSamples, numSamples);
    DenseMatrix b; b.setZero(numSamples,1);

    int i=0;
    for(auto it1 = samples.cbegin(); it1 != samples.cend(); ++it1, ++i)
    {
        double sum = 0;
        int j=0;
        for(auto it2 = samples.cbegin(); it2 != samples.cend(); ++it2, ++j)
        {
            double val = fn->eval(dist(*it1, *it2));
            if(val != 0)
            {
                //A.insert(i,j) = val;
                A(i,j) = val;
                sum += val;
            }
        }

        double y = it1->getY();
        if(normalized) b(i) = sum*y;
        else b(i) = y;
    }

    //A.makeCompressed();

    if(precondition)
    {
        // Calcualte precondition matrix P
        DenseMatrix P = computePreconditionMatrix();

        // Preconditioned A and b
        DenseMatrix Ap = P*A;
        DenseMatrix bp = P*b;

        A = Ap;
        b = bp;
    }

#ifndef NDEBUG
    std::cout << "Computing RBF weights using dense solver." << std::endl;
#endif // NDEBUG

    // SVD analysis
    Eigen::JacobiSVD<DenseMatrix> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    auto svals = svd.singularValues();
    double svalmax = svals(0);
    double svalmin = svals(svals.rows()-1);
    double rcondnum = (svalmax <= 0.0 || svalmin <= 0.0) ? 0.0 : svalmin/svalmax;

#ifndef NDEBUG
    std::cout << "The reciprocal of the condition number is: " << rcondnum << std::endl;
    std::cout << "Largest/smallest singular value: " << svalmax << " / " << svalmin << std::endl;
#endif // NDEBUG

    // Solve for weights
    weights = svd.solve(b);

#ifndef NDEBUG
    // Compute error. If it is used later on, move this statement above the NDEBUG
    double err = (A*weights - b).norm() / b.norm();
    std::cout << "Error: " << std::setprecision(10) << err << std::endl;
#endif // NDEBUG

//    // Alternative solver
//    DenseQR s;
//    bool success = s.solve(A,b,weights);
//    assert(success);

    // NOTE: Tried using experimental GMRES solver in Eigen, but it did not work very well.
}