/* -*- 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/>.
 *
 */


/*
  The spline kernels are based on code by
  Philippe Thevenaz http://bigwww.epfl.ch/thevenaz/interpolation/

 */

#include <cmath>
#include <cassert>
#include <iomanip>
#include <limits>
#include <miaconfig.h>
#include <mia/core/export_handler.hh>
#include <mia/core/splinekernel.hh>
#include <mia/core/errormacro.hh>
#include <mia/core/msgstream.hh>
#include <mia/core/simpson.hh>
#include <mia/core/plugin_base.cxx>
#include <mia/core/handler.cxx>
#include <mia/core/boundary_conditions.hh>

#include <boost/filesystem.hpp>


#if defined(__SSE2__)
#include <emmintrin.h>
#endif

#define USE_FASTFLOOR

NS_MIA_BEGIN
using namespace std;

const char *CSplineKernel::type_descr = "splinekernel";
const char *CSplineKernel::data_descr = "1d";

CSplineKernel::SCache::SCache(size_t s, const CSplineBoundaryCondition& bc, bool am):
       x(numeric_limits<double>::quiet_NaN()),
       start_idx(-1000),
       index_limit(bc.get_width() - s),
       weights(s),
       index(s),
       boundary_condition(bc),
       is_flat(false),
       never_flat(am)
{
}

void CSplineKernel::SCache::reset()
{
       index_limit = boundary_condition.get_width() - weights.size();
}

CSplineKernel::CSplineKernel(int degree, double shift, EInterpolation type):
       m_half_degree(degree >> 1),
       m_shift(shift),
       m_support_size(degree + 1),
       m_type(type),
       m_indices(m_support_size)
{
       for (unsigned int i = 0; i < m_support_size; ++i)
              m_indices[i] = i;
}

CSplineKernel::~CSplineKernel()
{
}

EInterpolation CSplineKernel::get_type() const
{
       return m_type;
}

void CSplineKernel::operator () (double x, VWeight& weight, VIndex& index)const
{
       assert(index.size() == m_support_size);
       int ix = get_indices(x, index);
       get_weights(x - ix, weight);
}

void CSplineKernel::operator () (double x, SCache& cache) const
{
       assert(cache.index_limit == (int)cache.boundary_condition.get_width() - (int)cache.weights.size());

       if (x == cache.x)
              return;

       int start_idx  = get_start_idx_and_value_weights(x, cache.weights);
       cache.x = x;

       if (start_idx == cache.start_idx)
              return;

       cache.start_idx = start_idx;

       if (cache.never_flat || start_idx < 0 || start_idx > cache.index_limit ) {
              cache.is_flat = false;
              fill_index(start_idx, cache.index);
              cache.boundary_condition.apply(cache.index, cache.weights);
       } else {
              cache.index[0] = start_idx;
              cache.is_flat = true;
       }
}

void CSplineKernel::get_cached(double x, SCache& cache)const
{
       int start_idx  = get_start_idx_and_value_weights(x, cache.weights);
       cache.x = x;

       // here we need an additional field that stores whether the
       // indices crossed a boundary

       // start index is same and inside the image, we can keep the new weights
       // only zero-boundary conditions must check if weights were changed
       // for ABI compatibility we check only the range here, later it should go as
       // virtual function into boundary_condition
       if (start_idx == cache.start_idx && // !cache.boundary_condition.is_zero()
           start_idx >= 0 &&
           cache.start_idx <=  cache.index_limit )
              return;

       cache.start_idx = start_idx;
       cache.is_flat = false;
       fill_index(start_idx, cache.index);
       cache.boundary_condition.apply(cache.index, cache.weights);
}

void CSplineKernel::get_uncached(double x, SCache& cache)const
{
       assert(cache.index_limit == (int)cache.boundary_condition.get_width() - (int)cache.weights.size());
       cache.start_idx  = get_start_idx_and_value_weights(x, cache.weights);

       if (cache.never_flat || cache.start_idx < 0 || cache.start_idx > cache.index_limit ) {
              cache.is_flat = false;
              fill_index(cache.start_idx, cache.index);
              cache.boundary_condition.apply(cache.index, cache.weights);
       } else {
              cache.index[0] = cache.start_idx;
              cache.is_flat = true;
       }
}

#ifdef USE_FASTFLOOR
// code taken from http://www.stereopsis.com/FPU.html
// Michael Herf
const double _double2fixmagic = 68719476736.0 * 1.5;   //2^36 * 1.5,  (52-_shiftamt=36) uses limited precisicion to floor
const int    _shiftamt        = 16;                    //16.16 fixed point representation,

#if WORDS_BIGENDIAN
#define iexp_				0
#define iman_				1
#else
#define iexp_				1
#define iman_				0
#endif //BigEndian_

// ================================================================================================
// Real2Int
// ================================================================================================
inline int fastfloor(double val)
{
       union {
              double dval;
              int ival[2];
       } v;
       v.dval = val + _double2fixmagic;
       return v.ival[iman_] >> _shiftamt;
}
#else
#define fastfloor floor
#endif

void CSplineKernel::fill_index(short i, VIndex& index) const
{
       unsigned int k = 0;

       while (k < m_support_size) {
              index[k] = i + m_indices[k];
              ++k;
       }
}

int CSplineKernel::get_indices(double x, VIndex& index) const
{
       const int ix = fastfloor(x + m_shift);
       fill_index(ix - (int)m_half_degree, index);
       return ix;
}

double CSplineKernel::get_weight_at(double /*x*/, int degree) const
{
       throw create_exception<invalid_argument>("B-Spline: derivative degree ", degree, " not supported" );
}

const vector<double>& CSplineKernel::get_poles() const
{
       return m_poles;
}

void CSplineKernel::add_pole(double x)
{
       m_poles.push_back(x);
}

void CSplineKernel::derivative(double x, VWeight& weight, VIndex& index)const
{
       assert(index.size() == m_support_size);
       int ix = get_indices(x, index);
       get_derivative_weights(x - ix, weight);
}

void CSplineKernel::derivative(double x, VWeight& weight, VIndex& index, int degree)const
{
       assert(index.size() == m_support_size);
       int ix = get_indices(x, index);
       get_derivative_weights(x - ix, weight, degree);
}

double CSplineKernel::get_nonzero_radius() const
{
       return m_support_size / 2.0;
}

int CSplineKernel::get_active_halfrange() const
{
       return (m_support_size  + 1) / 2;
}

int CSplineKernel::get_start_idx_and_value_weights(double x, VWeight& weights) const
{
       const int result = fastfloor(x + m_shift);
       get_weights(x - result, weights);
       return result - (int)m_half_degree;
}

int CSplineKernel::get_start_idx(double x) const
{
       return fastfloor(x + m_shift) - (int)m_half_degree;
}

int CSplineKernel::get_start_idx_and_derivative_weights(double x, VWeight& weights) const
{
       const int result = fastfloor(x + m_shift);
       get_derivative_weights(x - result, weights);
       return result - (int)m_half_degree;
}



struct F2DKernelIntegrator {
       F2DKernelIntegrator(const CSplineKernel& spline, double s1, double s2, int deg1, int deg2):
              m_spline(spline), m_s1(s1), m_s2(s2), m_deg1(deg1), m_deg2(deg2)
       {
       }
       double operator() (double x) const
       {
              return m_spline.get_weight_at(x - m_s1, m_deg1) *
                     m_spline.get_weight_at(x - m_s2, m_deg2);
       }
private:
       const CSplineKernel& m_spline;
       double m_s1, m_s2, m_deg1, m_deg2;
};


double  EXPORT_CORE integrate2(const CSplineKernel& spline, double s1, double s2, int deg1, int deg2, double n, double x0, double L)
{
       double sum = 0.0;
       x0 /= n;
       L  /= n;
       // evaluate interval to integrate over
       double start_int = s1 - spline.get_nonzero_radius();
       double end_int = s1 + spline.get_nonzero_radius();

       if (start_int < s2 - spline.get_nonzero_radius())
              start_int = s2 - spline.get_nonzero_radius();

       if (start_int < x0)
              start_int = x0;

       if (end_int > s2 + spline.get_nonzero_radius())
              end_int = s2 + spline.get_nonzero_radius();

       if (end_int > L)
              end_int = L;

       // Simpson formula
       if (end_int <= start_int)
              return sum;

       const size_t intervals = size_t(8 * (end_int - start_int));
       sum = simpson( start_int, end_int, intervals, F2DKernelIntegrator(spline, s1, s2, deg1, deg2));
       return sum * n;
}

template<>  const char *const
TPluginHandler<TFactory<CSplineKernel>>::m_help =
              "These plug-ins provide various kernels that evaluate the wights in spline-based interpolation.";


EXPLICIT_INSTANCE_HANDLER(CSplineKernel);

NS_MIA_END
