/* -*- 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/core/msgstream.hh>
#include <mia/core/nccsum.hh>


NS_MIA_BEGIN

using std::make_pair;

#ifdef __SSE2__

double NCCSums::value() const
{
       double  result = 1.0;

       if (m_n > 0) {
              v2df sum2 = m_sum2;
              v2df nn = {m_n, m_n };
              v2df mean = m_sum / nn;
              v2df delta = m_sum * mean;
              sum2 -= delta;
              v2df ms2_a = _mm_unpacklo_pd(mean, sum2);
              v2df ms2_b = _mm_unpackhi_pd(mean, sum2);
              v2df prod = ms2_a * ms2_b;
// new versiond of gcc can access these array directly
#ifdef BUILD_SSE_ATTRIBUTE_VECTOR_CAN_USE_SUBSCRIPT
              double sumab = m_sumab - prod[0] * m_n;

              if (prod[1] > 1e-10) {
                     result = 1.0 - sumab * sumab / prod[1];
              } else if (sum2[0] < 1e-5 && sum2[1] < 1e-5) {
                     result = 0.0;
              }

#else // !BUILD_SSE_ATTRIBUTE_VECTOR_CAN_USE_SUBSCRIPT
              double __attribute__((aligned(16))) mprod[2];
              _mm_store_pd(mprod, prod);
              double sumab = m_sumab - mprod[0] * m_n;
              double __attribute__((aligned(16))) msum2[2];
              _mm_store_pd(msum2, sum2);

              if (mprod[1] > 1e-10) {
                     result = 1.0 - sumab * sumab / mprod[1];
              } else if (msum2[0] < 1e-5 && msum2[1] < 1e-5) {
                     result = 0.0;
              }

#endif // BUILD_SSE_ATTRIBUTE_VECTOR_CAN_USE_SUBSCRIPT
       }

       return result;
}

std::pair<double, NCCGradHelper> NCCSums::get_grad_helper() const
{
       std::pair<double, NCCGradHelper>  result = std::make_pair(1.0, NCCGradHelper());

       if (m_n > 0) {
              v2df sum2 = m_sum2;
              v2df nn = {m_n, m_n };
              v2df mean = m_sum / nn;
              v2df delta = m_sum * mean;
              sum2 -= delta;
              v2df ms2_a = _mm_unpacklo_pd(mean, sum2);
              v2df ms2_b = _mm_unpackhi_pd(mean, sum2);
              v2df prod = ms2_a * ms2_b;
#ifdef BUILD_SSE_ATTRIBUTE_VECTOR_CAN_USE_SUBSCRIPT
              double sumab = m_sumab - prod[0] * m_n;

              if (prod[1] > 1e-5) {
                     result = make_pair(1.0 - sumab * sumab / prod[1],
                                        NCCGradHelper(sumab / prod[1], sumab / sum2[0], mean[0], mean[1]));
              } else {
                     if (sum2[0] < 1e-5 && sum2[1] < 1e-5) {
                            result = make_pair(0.0, NCCGradHelper());
                     }
              }

#else // !BUILD_SSE_ATTRIBUTE_VECTOR_CAN_USE_SUBSCRIPT
              double __attribute__((aligned(16))) mprod[2];
              _mm_store_pd(mprod, prod);
              double __attribute__((aligned(16))) mmean[2];
              _mm_store_pd(mmean, mean);
              double __attribute__((aligned(16))) msum2[2];
              _mm_store_pd(msum2, sum2);
              double sumab = m_sumab - mprod[0] * m_n;

              if (mprod[1] > 1e-5) {
                     result = make_pair(1.0 - sumab * sumab / mprod[1],
                                        NCCGradHelper(sumab / mprod[1], sumab / msum2[0], mmean[0], mmean[1]));
              } else {
                     if (msum2[0] < 1e-5 && msum2[1] < 1e-5) {
                            result = make_pair(0.0, NCCGradHelper());
                     }
              }

#endif  // BUILD_SSE_ATTRIBUTE_VECTOR_CAN_USE_SUBSCRIPT
       }

       return result;
}

#else // !__SSE2__

double NCCSums::value() const
{
       double  result = 1.0;

       if (m_n > 0) {
              double mean_a = m_suma / m_n;
              double mean_b = m_sumb / m_n;
              double delta_a = m_suma * mean_a;
              double delta_b = m_sumb * mean_b;
              double suma2 = m_suma2 - delta_a;
              double sumb2 = m_sumb2 - delta_b;
              double help_0 = mean_a * mean_b;
              double help_1 = suma2 * sumb2;
              double sumab = m_sumab - help_0 * m_n;

              if (help_1 > 1e-5) {
                     result = 1.0 - sumab * sumab / help_1;
              } else if (suma2 < 1e-5 &&  sumb2 < 1e-5) {
                     result = 0.0;
              }
       }

       return result;
}

std::pair<double, NCCGradHelper> NCCSums::get_grad_helper() const
{
       std::pair<double, NCCGradHelper>  result = std::make_pair(1.0, NCCGradHelper());

       if (m_n > 0) {
              double mean_a = m_suma / m_n;
              double mean_b = m_sumb / m_n;
              double delta_a = m_suma * mean_a;
              double delta_b = m_sumb * mean_b;
              double suma2 = m_suma2 - delta_a;
              double sumb2 = m_sumb2 - delta_b;
              double help_0 = mean_a * mean_b;
              double help_1 = suma2 * sumb2;
              double sumab = m_sumab - help_0 * m_n;

              if (help_1 > 1e-10) {
                     result = make_pair(1.0 - sumab * sumab / help_1,
                                        NCCGradHelper(sumab / help_1, sumab / suma2, mean_a, mean_b));
              } else {
                     if (suma2 < 1e-5 && sumb2 < 1e-5) {
                            result = make_pair(0.0, NCCGradHelper());
                     }
              }
       }

       return result;
}

#endif // __SSE2__

NS_MIA_END
