/* -*- 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 <cassert>
#include <cstring>

#ifdef __SSE3__
#include <pmmintrin.h>
#endif


#include <mia/core/interpolator1d.hh>
#include <mia/2d/interpolator.hh>

#include <mia/core/interpolator1d.cxx>
#include <mia/2d/interpolator.cxx>

NS_MIA_BEGIN

using namespace std;

C2DWeightCache::C2DWeightCache(int kernel_size,
                               const CSplineBoundaryCondition& xbc,
                               const CSplineBoundaryCondition& ybc):
       x(kernel_size, xbc, kernel_size < 3),
       y(kernel_size, ybc, true)
{
}

C2DInterpolatorFactory::C2DInterpolatorFactory(const std::string& kernel, const std::string& bc):
       m_kernel(produce_spline_kernel(kernel)),
       m_xbc(produce_spline_boundary_condition(bc)),
       m_ybc(produce_spline_boundary_condition(bc))
{
}

C2DInterpolatorFactory::C2DInterpolatorFactory(PSplineKernel kernel, const CSplineBoundaryCondition& bc):
       m_kernel(kernel),
       m_xbc(bc.clone()),
       m_ybc(bc.clone())
{
}

C2DInterpolatorFactory::C2DInterpolatorFactory(PSplineKernel kernel,
              const CSplineBoundaryCondition& xbc, const CSplineBoundaryCondition& ybc):
       m_kernel(kernel),
       m_xbc(xbc.clone()),
       m_ybc(ybc.clone())
{
}

C2DInterpolatorFactory::C2DInterpolatorFactory(PSplineKernel kernel, const std::string& bc):
       m_kernel(kernel),
       m_xbc(produce_spline_boundary_condition(bc)),
       m_ybc(produce_spline_boundary_condition(bc))
{
}

C2DInterpolatorFactory::C2DInterpolatorFactory(const C2DInterpolatorFactory& o):
       m_kernel(o.m_kernel),
       m_xbc(o.m_xbc->clone()),
       m_ybc(o.m_ybc->clone())

{
}

C2DInterpolatorFactory& C2DInterpolatorFactory::operator = ( const C2DInterpolatorFactory& o)
{
       m_kernel = o.m_kernel;
       m_xbc.reset(o.m_xbc->clone());
       m_ybc.reset(o.m_ybc->clone());
       return *this;
}

C2DInterpolatorFactory::~C2DInterpolatorFactory()
{
}

const CSplineKernel *C2DInterpolatorFactory::get_kernel() const
{
       return m_kernel.get();
}

#ifdef __SSE__
float add_2d_new<T2DDatafield< float >, 4>::value(const T2DDatafield< float >&  coeff,
              const C2DWeightCache& spline_cache)
{
       typedef float v4df __attribute__ ((vector_size (16)));
       float __attribute__((aligned(16))) cache[16];
       float __attribute__((aligned(16))) xweight[4];
       copy(spline_cache.x.weights.begin(), spline_cache.x.weights.end(), xweight);
       const int dx = coeff.get_size().x;
       int idx = 0;

       if (spline_cache.x.is_flat) {
              for (size_t y = 0; y < 4; ++y, idx += 4) {
                     const float *p = &coeff[spline_cache.y.index[y] * dx];
                     memcpy(&cache[idx], &p[spline_cache.x.start_idx], 4 * sizeof(float));
              }
       } else {
              for (size_t y = 0; y < 4; ++y, idx += 4) {
                     const float *p = &coeff[spline_cache.y.index[y] * dx];
                     cache[idx  ] = p[spline_cache.x.index[0]];
                     cache[idx + 1] = p[spline_cache.x.index[1]];
                     cache[idx + 2] = p[spline_cache.x.index[2]];
                     cache[idx + 3] = p[spline_cache.x.index[3]];
              }
       }

       v4df wx  = _mm_loadu_ps(xweight);
       v4df w0  = _mm_set1_ps(spline_cache.y.weights[0]);
       v4df w1  = _mm_set1_ps(spline_cache.y.weights[1]);
       v4df w2  = _mm_set1_ps(spline_cache.y.weights[2]);
       v4df w3  = _mm_set1_ps(spline_cache.y.weights[3]);
       v4df x0 = _mm_load_ps(&cache[0]);
       v4df x1 = _mm_load_ps(&cache[4]);
       v4df x2 = _mm_load_ps(&cache[8]);
       v4df x3 = _mm_load_ps(&cache[12]);
       v4df res = (x0 * w0 + x1 * w1 + x2 * w2 + x3 * w3 ) * wx;
#ifdef __SSE3__
       float result;
       res = _mm_hadd_ps(res, res);
       res = _mm_hadd_ps(res, res);
       _mm_store_ss(&result, res);
       return result;
#else
       float __attribute__((aligned(16))) r[4];
       _mm_store_ps(r, res);
       return r[0] + r[1] + r[2] + r[3];
#endif
}
#endif


#ifdef __SSE2__
double add_2d_new<T2DDatafield< double >, 4>::value(const T2DDatafield< double >&  coeff,
              const C2DWeightCache& spline_cache)
{
       typedef double v2df __attribute__ ((vector_size (16)));
       double __attribute__((aligned(16))) cache[16];
       const int dx = coeff.get_size().x;
       int idx = 0;

       if (spline_cache.x.is_flat) {
              for (size_t y = 0; y < 4; ++y, idx += 4) {
                     const double *p = &coeff[spline_cache.y.index[y] * dx];
                     v2df y1 = _mm_loadu_pd(&p[spline_cache.x.start_idx]);
                     v2df y2 = _mm_loadu_pd(&p[spline_cache.x.start_idx + 2]);
                     _mm_store_pd(&cache[idx  ], y1);
                     _mm_store_pd(&cache[idx + 2], y2);
              }
       } else {
              for (size_t y = 0; y < 4; ++y, idx += 4) {
                     const double *p = &coeff[spline_cache.y.index[y] * dx];
                     cache[idx  ] = p[spline_cache.x.index[0]];
                     cache[idx + 1] = p[spline_cache.x.index[1]];
                     cache[idx + 2] = p[spline_cache.x.index[2]];
                     cache[idx + 3] = p[spline_cache.x.index[3]];
              }
       }

       v2df w0  = _mm_set1_pd(spline_cache.y.weights[0]);
       v2df w1  = _mm_set1_pd(spline_cache.y.weights[1]);
       v2df x00 = _mm_load_pd(&cache[0]);
       v2df x01 = _mm_load_pd(&cache[2]);
       v2df x10 = _mm_load_pd(&cache[4]);
       v2df x11 = _mm_load_pd(&cache[6]);
       v2df y1 = x00 * w0 + x10 * w1;
       v2df y2 = x01 * w0 + x11 * w1;
       v2df w2  = _mm_set1_pd(spline_cache.y.weights[2]);
       v2df w3  = _mm_set1_pd(spline_cache.y.weights[3]);
       v2df x20 = _mm_load_pd(&cache[8]);
       v2df x21 = _mm_load_pd(&cache[10]);
       v2df x30 = _mm_load_pd(&cache[12]);
       v2df x31 = _mm_load_pd(&cache[14]);
       y1 += x20 * w2 + x30 * w3;
       y2 += x21 * w2 + x31 * w3;
       w0 = _mm_loadu_pd(&spline_cache.x.weights[0]);
       w1 = _mm_loadu_pd(&spline_cache.x.weights[2]);
       y1 *= w0;
       y2 *= w1;
       y1 += y2;
       double __attribute__((aligned(16))) result[2];
       _mm_storeu_pd(result, y1);
       return result[0] + result[1];
}
#endif

#define INSTANCIATE_INTERPOLATORS(TYPE)			\
	template class EXPORT_2D T2DInterpolator<TYPE>

INSTANCIATE_INTERPOLATORS(bool);
INSTANCIATE_INTERPOLATORS(int8_t);
INSTANCIATE_INTERPOLATORS(int16_t);
INSTANCIATE_INTERPOLATORS(int32_t);
INSTANCIATE_INTERPOLATORS(int64_t);
INSTANCIATE_INTERPOLATORS(uint8_t);
INSTANCIATE_INTERPOLATORS(uint16_t);
INSTANCIATE_INTERPOLATORS(uint32_t);
INSTANCIATE_INTERPOLATORS(uint64_t);
INSTANCIATE_INTERPOLATORS(float);
INSTANCIATE_INTERPOLATORS(double);


INSTANCIATE_INTERPOLATORS(C2DFVector);

template class EXPORT_2D T1DInterpolator<C2DFVector>;
template class EXPORT_2D T1DConvoluteInterpolator<C2DFVector>;



NS_MIA_END
