/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis
 * Copyright (c) Leipzig, Madrid 1999-2017 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */
#include <mia/internal/autotest.hh>

#include <mia/3d/ppmatrix.hh>

NS_MIA_USE;

struct TransformSplineFixtureFieldBase {
       TransformSplineFixtureFieldBase():
              range(1.0),
              graddiv2sum(0.0)
       {
       }

       void init(int dsize, double r, const std::string& kernel)
       {
              ipf.reset(new C3DInterpolatorFactory(kernel, "mirror"));
              size = C3DBounds(2 * dsize + 1, 2 * dsize + 1, 2 * dsize + 1);
              field = C3DFVectorfield(size);
              range = r;
              scale.x = scale.y = scale.z = range / dsize;
              field_range.x = 2 * range;
              field_range.y = 2 * range;
              field_range.z = 2 * range;
              graddiv2sum = 0.0;
              C3DFVectorfield::iterator i = field.begin();

              for (int z = -dsize; z <= dsize; ++z)
                     for (int y = -dsize; y <= dsize; ++y)
                            for (int x = -dsize; x <= dsize; ++x, ++i) {
                                   double sx = x * scale.x;
                                   double sy = y * scale.y;
                                   double sz = z * scale.z;
                                   *i = C3DFVector( fx(sx, sy, sz), fy(sx, sy, sz), fz(sx, sy, sz));
                                   cvdebug() << "(" << sx << ", " << sy << ", " << sz << *i << "\n";
                                   graddiv2sum += graddiv2(sx, sy, sz);
                            }

              assert(i == field.end());
              source.reset(ipf->create(field));
              cvinfo() << "scale = " << scale << "\n";
              graddiv2sum *= scale.x * scale.y * scale.z;
       }
       C3DBounds size;
       C3DFVectorfield field;
       P3DInterpolatorFactory ipf;
       C3DFVector field_range;
       double range;
       std::shared_ptr<T3DInterpolator<C3DFVector>> source;
protected:
       virtual double fx(double x, double y, double z)const  = 0;
       virtual double fy(double x, double y, double z)const  = 0;
       virtual double fz(double x, double y, double z)const  = 0;
       virtual double graddiv2(double x, double y, double z)const = 0;
       C3DFVector scale;
       double graddiv2sum;
private:

};

struct TransformSplineFixtureDivOnly: public TransformSplineFixtureFieldBase {
       double fx(double x, double y, double z)const ;
       double fy(double x, double y, double z)const ;
       double fz(double x, double y, double z)const ;
       double graddiv2(double x, double y, double z)const;
       double integrate() const ;
};

struct TransformSplineFixtureMixed: public TransformSplineFixtureFieldBase {
       double fx(double x, double y, double z)const ;
       double fy(double x, double y, double z)const ;
       double fz(double x, double y, double z)const ;
       double graddiv2(double x, double y, double z)const;
};

struct TransformSplineFixtureMixed2: public TransformSplineFixtureFieldBase {
       double fx(double x, double y, double z)const ;
       double fy(double x, double y, double z)const ;
       double fz(double x, double y, double z)const ;
       double graddiv2(double x, double y, double z)const;
};


BOOST_FIXTURE_TEST_CASE( test_bspline3_8_4_mix2,  TransformSplineFixtureMixed2 )
{
       init(8, 4, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double h = 21.0 * pow(M_PI, 1.5) / (sqrt(2.0) * 2.0);
       const double testdiv = h / 4.0;
       const double testcurl = h;
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       BOOST_CHECK_CLOSE( div  * coeffs, testdiv, 0.5);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       BOOST_CHECK_CLOSE( divcurl  * coeffs, testcurl + testdiv, 0.5);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       BOOST_CHECK_CLOSE( rot * coeffs, testcurl, .5);
}

BOOST_FIXTURE_TEST_CASE( test_bspline3_8_4, TransformSplineFixtureDivOnly )
{
       init(8, 4, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double testvalue = 105.0 * pow(M_PI, 1.5) / (sqrt(2.0) * 8.0);
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       BOOST_CHECK_CLOSE( div  * coeffs, testvalue, 0.5);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       BOOST_CHECK_CLOSE( divcurl  * coeffs, testvalue, 0.5);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       BOOST_CHECK_CLOSE( 1.0 + rot * coeffs, 1.0, 1.0);
}


BOOST_FIXTURE_TEST_CASE( test_nocurl_bspline3_7_4, TransformSplineFixtureDivOnly )
{
       init(9, 4, "bspline:d=3");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double testvalue = 105.0 * pow(M_PI, 1.5) / (sqrt(2.0) * 8.0);
       // sanity check
       BOOST_CHECK_CLOSE(integrate(), testvalue, 0.1);
       BOOST_CHECK_CLOSE(graddiv2sum, testvalue, 0.1);
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       BOOST_CHECK_CLOSE( div  * coeffs, testvalue, 0.8);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       BOOST_CHECK_CLOSE( divcurl  * coeffs, testvalue, 0.8);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       BOOST_CHECK_CLOSE( 1.0 + rot * coeffs, 1.0, 4.0);
}



double TransformSplineFixtureDivOnly::fx(double x, double y, double z)const
{
       return x * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureDivOnly::fy(double x, double y, double z)const
{
       return y * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureDivOnly::fz(double x, double y, double z)const
{
       return z * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureDivOnly::graddiv2(double x, double y, double z) const
{
       double h1 = x * x + y * y + z * z;
       double h2 = 2.0 * h1;
       double h3 = h2 - 5.0;
       return 4 * h1 * h3 * h3 * exp(-h2);
}

double TransformSplineFixtureDivOnly::integrate() const
{
       const double step = range / 100.0;
       double sum = 0.0;

       for (double z = -range; z < range; z += step)
              for (double y = -range; y < range; y += step)
                     for (double x = -range; x < range; x += step) {
                            sum += graddiv2(x, y, z);
                     }

       return sum * step * step * step;
}


double TransformSplineFixtureMixed::fx(double x, double y, double z)const
{
       return x * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixed::fy(double x, double y, double z)const
{
       return y * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixed::fz(double, double, double )const
{
       return 0.0;
}

double TransformSplineFixtureMixed::graddiv2(double, double, double )const
{
       return 0.0;
}


BOOST_FIXTURE_TEST_CASE( test_mix_bspline4_10_4, TransformSplineFixtureMixed )
{
       init(10, 4, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double testdiv = 7.0 * pow(M_PI, 1.5) / sqrt(2.0);
       const double testcurl = testdiv / 4.0;
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       double graddiv = div  * coeffs;
       BOOST_CHECK_CLOSE( graddiv, testdiv, 0.1);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       double graddivcurl = divcurl  * coeffs;
       BOOST_CHECK_CLOSE( graddivcurl, testdiv + testcurl, 0.1);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       double gradcurl = rot  * coeffs;
       BOOST_CHECK_CLOSE( gradcurl, testcurl, 0.1);
       cvinfo() << "graddiv  / testdiv= " << graddiv  / testdiv << "\n";
       cvinfo() << "gradcurl / testcurl = " << gradcurl / testcurl << "\n";
}


BOOST_FIXTURE_TEST_CASE( test_mix_bspline4_20_4, TransformSplineFixtureMixed )
{
       init(20, 4, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double testdiv = 7.0 * pow(M_PI, 1.5) / sqrt(2.0);
       const double testcurl = testdiv / 4.0;
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       double graddiv = div  * coeffs;
       BOOST_CHECK_CLOSE( graddiv, testdiv, 0.1);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       double graddivcurl = divcurl  * coeffs;
       BOOST_CHECK_CLOSE( graddivcurl, testdiv + testcurl, 0.1);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       double gradcurl = rot  * coeffs;
       BOOST_CHECK_CLOSE( gradcurl, testcurl, 0.1);
       cvinfo() << "graddiv  / testdiv= " << graddiv  / testdiv << "\n";
       cvinfo() << "gradcurl / testcurl = " << gradcurl / testcurl << "\n";
}

BOOST_FIXTURE_TEST_CASE( test_mix_bspline4_20_7, TransformSplineFixtureMixed )
{
       init(20, 7, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double testdiv = 7.0 * pow(M_PI, 1.5) / sqrt(2.0);
       const double testcurl = testdiv / 4.0;
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       double graddiv = div  * coeffs;
       BOOST_CHECK_CLOSE( graddiv, testdiv, 0.1);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       double graddivcurl = divcurl  * coeffs;
       BOOST_CHECK_CLOSE( graddivcurl, testdiv + testcurl, 0.1);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       double gradcurl = rot  * coeffs;
       BOOST_CHECK_CLOSE( gradcurl, testcurl, 0.1);
       cvinfo() << "graddiv  / testdiv= " << graddiv  / testdiv << "\n";
       cvinfo() << "gradcurl / testcurl = " << gradcurl / testcurl << "\n";
}

BOOST_FIXTURE_TEST_CASE( test_mix_bspline4_10_4_grad, TransformSplineFixtureMixed )
{
       init(4, 2, "bspline:d=3");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       CDoubleVector gradient(3 * coeffs.size());
       divcurl.evaluate(coeffs, gradient);
       const float delta = 0.001;
       int i = 0;

       for (auto c = coeffs.begin(); c != coeffs.end(); ++c, i += 3) {
              c->x += delta;
              double vpx = divcurl  * coeffs;
              c->x -= 2.0 * delta;
              double vmx = divcurl  * coeffs;
              c->x += delta;
              double test_x = (vpx - vmx) / (2.0 * delta);

              if (fabs(test_x) < 1e-8)
                     BOOST_CHECK_CLOSE( gradient[i] + 1e-7, test_x + 1e-7, 5);
              else
                     BOOST_CHECK_CLOSE(gradient[i], test_x, 0.1);

              c->y += delta;
              double vpy = divcurl  * coeffs;
              c->y -= 2.0 * delta;
              double vmy = divcurl  * coeffs;
              c->y += delta;
              double test_y = (vpy - vmy) / (2.0 * delta);

              if (fabs(test_y) < 1e-8)
                     BOOST_CHECK_CLOSE( gradient[i + 1] + 1e-7, test_y + 1e-7, 5);
              else
                     BOOST_CHECK_CLOSE(gradient[i + 1], test_y, 0.1);

              c->z += delta;
              double vpz = divcurl  * coeffs;
              c->z -= 2.0 * delta;
              double vmz = divcurl  * coeffs;
              c->z += delta;
              double test_z = (vpz - vmz) / (2.0 * delta);

              if (fabs(test_z) < 1e-8  )
                     BOOST_CHECK_CLOSE( gradient[i + 2] + 1e-7, test_z + 1e-7, 5);
              else
                     BOOST_CHECK_CLOSE(gradient[i + 2], test_z, 0.1);
       }
}



struct TransformSplineFixtureFieldNonuniform {
       TransformSplineFixtureFieldNonuniform():
              range(1.0)
       {
       }
       void init(const C3DBounds& dsize, double r, const std::string& kernel)
       {
              ipf.reset(new C3DInterpolatorFactory(kernel, "mirror"));
              size = C3DBounds(2 * dsize.x + 1, 2 * dsize.y + 1, 2 * dsize.z + 1);
              field = C3DFVectorfield(size);
              range = r;
              scale.x = range / dsize.x;
              scale.y = range / dsize.y;
              scale.z = range / dsize.z;
              field_range.x = 2 * range;
              field_range.y = 2 * range;
              field_range.z = 2 * range;
              double divcurl2sum = 0.0;
              C3DFVectorfield::iterator i = field.begin();

              for (int z = -(int)dsize.z; z <= (int)dsize.z; ++z)
                     for (int y = -(int)dsize.y; y <= (int)dsize.y; ++y)
                            for (int x = -(int)dsize.x; x <= (int)dsize.x; ++x, ++i) {
                                   double sx = x * scale.x;
                                   double sy = y * scale.y;
                                   double sz = z * scale.z;
                                   *i = C3DFVector( fx(sx, sy, sz), fy(sx, sy, sz), fz(sx, sy, sz));
                                   cvdebug() << "(" << sx << ", " << sy << ", " << sz << *i << "\n";
                                   divcurl2sum += divcurl2(sx, sy, sz);
                            }

              assert(i == field.end());
              source.reset(ipf->create(field));
              divcurl2sum *= scale.x * scale.y * scale.z;
              BOOST_CHECK_CLOSE(divcurl2sum, 105.0 * pow(M_PI, 1.5) / (sqrt(2.0) * 8.0), 1);
              cvinfo() << "scale = " << scale << "\n";
       }
       C3DBounds size;
       C3DFVectorfield field;
       P3DInterpolatorFactory ipf;
       C3DFVector field_range;
       double range;
       std::shared_ptr<T3DInterpolator<C3DFVector>> source;
protected:
       virtual double fx(double x, double y, double z)const  = 0;
       virtual double fy(double x, double y, double z)const  = 0;
       virtual double fz(double x, double y, double z)const  = 0;
       virtual double divcurl2(double x, double y, double z)const  = 0;
       C3DFVector scale;
private:

};

struct TransformSplineFixtureMixedNonuniform: public TransformSplineFixtureFieldNonuniform {
       double fx(double x, double y, double z)const ;
       double fy(double x, double y, double z)const ;
       double fz(double x, double y, double z)const ;
       double divcurl2(double x, double y, double z)const;
};


double TransformSplineFixtureMixedNonuniform::fx(double x, double y, double z)const
{
       return x * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixedNonuniform::fy(double x, double y, double z)const
{
       return -z * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixedNonuniform::fz(double x, double y, double z)const
{
       return y * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixedNonuniform::divcurl2(double x, double y, double z)const
{
       double h = -x * x - y * y - z * z;
       double exp2h = 4 * exp(2 * h);
       double f = 4 * pow(z, 6) + (12 * y * y + 12 * x * x - 16) * pow(z, 4) +
                  (12 * pow(y, 4) + (24 * x * x - 32) * y * y + 12 * pow(x, 4) - 28 * x * x + 19) * z * z +
                  4 * pow(y, 6) + (12 * x * x - 16) * pow(y, 4) + (12 * x * x * x * x - 28 * x * x + 19) * y * y + 4 * pow(x, 6) - 12 * pow(x, 4) + 17 * x * x;
       return f * exp2h;
}

double TransformSplineFixtureMixed2::fx(double x, double y, double z)const
{
       return x * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixed2::fy(double x, double y, double z)const
{
       return -z * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixed2::fz(double x, double y, double z)const
{
       return y * exp(-x * x - y * y - z * z);
}

double TransformSplineFixtureMixed2::graddiv2(double x, double y, double z)const
{
       double h = -x * x - y * y - z * z;
       double exp2h = 4 * exp(2 * h);
       double f = 4 * pow(z, 6) + (12 * y * y + 12 * x * x - 16) * pow(z, 4) +
                  (12 * pow(y, 4) + (24 * x * x - 32) * y * y + 12 * pow(x, 4) - 28 * x * x + 19) * z * z +
                  4 * pow(y, 6) + (12 * x * x - 16) * pow(y, 4) + (12 * x * x * x * x - 28 * x * x + 19) * y * y + 4 * pow(x, 6) - 12 * pow(x, 4) + 17 * x * x;
       return f * exp2h;
}


BOOST_FIXTURE_TEST_CASE( test_bspline3_nonuniform, TransformSplineFixtureMixedNonuniform )
{
       C3DBounds size(8, 9, 7);
       init(size, 4, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double h = 21.0 * pow(M_PI, 1.5) / (sqrt(2.0) * 2.0);
       const double testdiv = h / 4.0;
       const double testcurl = h;
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       double realdiv = div  * coeffs;
       BOOST_CHECK_CLOSE( realdiv, testdiv, 0.6);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       double realsum = divcurl  * coeffs;
       BOOST_CHECK_CLOSE( realsum, testdiv + testcurl, 0.6);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       double realrot = rot * coeffs;
       BOOST_CHECK_CLOSE( realrot, testcurl, 0.6);
       cvinfo() << "realdiv/ testdiv" << realdiv / testdiv << "\n";
       cvinfo() << "realsum/ sum" << realsum / (testdiv + testcurl) << "\n";
       cvinfo() << "realrot/ testcurl" << realrot / testcurl << "\n";
}

BOOST_FIXTURE_TEST_CASE( test_bspline3_uniform, TransformSplineFixtureMixedNonuniform )
{
       C3DBounds size(8, 8, 8);
       init(size, 4, "bspline:d=4");
       const T3DConvoluteInterpolator<C3DFVector>& interp =
              static_cast<const T3DConvoluteInterpolator<C3DFVector>&>(*source);
       auto coeffs = interp.get_coefficients();
       const double h = 21.0 * pow(M_PI, 1.5) / (sqrt(2.0) * 2.0);
       const double testdiv = h / 4.0;
       const double testcurl = h;
       C3DPPDivcurlMatrix div(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 0.0);
       double realdiv = div  * coeffs;
       BOOST_CHECK_CLOSE( realdiv, testdiv, 0.5);
       C3DPPDivcurlMatrix divcurl(field.get_size(), field_range, *ipf->get_kernel(), 1.0, 1.0);
       double realsum = divcurl  * coeffs;
       BOOST_CHECK_CLOSE( realsum, testdiv + testcurl, 0.5);
       C3DPPDivcurlMatrix rot(field.get_size(), field_range, *ipf->get_kernel(), 0.0, 1.0);
       double realrot = rot * coeffs;
       BOOST_CHECK_CLOSE( realrot, testcurl, 0.5);
       cvinfo() << "realdiv/ testdiv" << realdiv / testdiv << "\n";
       cvinfo() << "realsum/ sum" << realsum / (testdiv + testcurl) << "\n";
       cvinfo() << "realrot/ testcurl" << realrot / testcurl << "\n";
}
