/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// AIC matrices building
// Build AIC matrices for body to body, body to field, field to field, and field to body interactions
//
// I/O:
// - symY: defines symmetry about Y axis
// - bPan: (network of) body panels (structure)
// - wPan: (network of) wake panels (structure)
// - fPan: field panels (structure)
// - b2bAIC: body to body AICs (structure)
// - b2fAIC: body to field AICs (structure)
// - f2fAIC: field to field AICs (structure)
// - f2bAIC: field to body AICs (structure)
// - sp: sub-panels (structure)
// - spAIC: body to field sub-panel AICs (structure)

#include <iostream>
#include <Eigen/Dense>
#include <array>
#include "build_AIC.h"
#include "infcB.h"
#include "infcFF.h"
#include "infcFB.h"
#include "split_panel.h"

using namespace std;
using namespace Eigen;

void build_AIC(bool symY, Network &bPan, Network &wPan, Field &fPan,
               Body_AIC &b2bAIC, Body_AIC &b2fAIC, Field2field_AIC &f2fAIC, Field2body_AIC &f2bAIC,
               Subpanel &sp, Subpanel_AIC &spAIC) {

    //// Initialization
    // Temporay variables
    int ii = 0, jj = 0; // counter
    bool FLAG = 0; // flag
    Vector3d dist; // Distance between panels
    Vector3d trsfColloc, trsfCorner1, trsfCorner2, trsfCorner3, trsfCorner4; // colloc (i) and corner (j) in local (j) coordinates

    Matrix3d glob2loc; // transformation matrix
    array<double, 2> coeffB; // body influence container

    array<RowVectorXd, 2> coeffSP; // body to field influence container for subpanel
    for (int i = 0; i < 2; i++)
        coeffSP[i].resize(sp.NS);

    array<double,2> xC, yC, zC; // cell vertices
    double x, y, z; // cell center
    double coeffFF; // field to field influence container
    array<double,3> coeffFB; // field to body influence container

    //// Begin
    cout << "Building AIC matrices... " << flush;

    // Resize AIC matrices (regular)
    b2bAIC.A.resize(bPan.nP, bPan.nP);
    b2bAIC.B.resize(bPan.nP, bPan.nP);
    b2fAIC.A.resize(fPan.nF, bPan.nP);
    b2fAIC.B.resize(fPan.nF, bPan.nP);
    f2fAIC.C.resize(fPan.nF, fPan.nF);
    f2bAIC.Cu.resize(bPan.nP, fPan.nF);
    f2bAIC.Cv.resize(bPan.nP, fPan.nF);
    f2bAIC.Cw.resize(bPan.nP, fPan.nF);

    //// Panel AIC
    for (int j = 0; j < bPan.nP; ++j) {
        glob2loc.row(0) = bPan.l.row(j);
        glob2loc.row(1) = bPan.p.row(j);
        glob2loc.row(2) = bPan.n.row(j);
        trsfCorner1 = bPan.v0.row(j).transpose();
        trsfCorner1 = glob2loc * trsfCorner1;
        trsfCorner2 = bPan.v1.row(j).transpose();
        trsfCorner2 = glob2loc * trsfCorner2;
        trsfCorner3 = bPan.v2.row(j).transpose();
        trsfCorner3 = glob2loc * trsfCorner3;
        trsfCorner4 = bPan.v3.row(j).transpose();
        trsfCorner4 = glob2loc * trsfCorner4;

        // panel to panel
        for (int i = 0; i < bPan.nP; ++i) {
            dist(0) = bPan.CG(i,0) - bPan.CG(j,0);
            dist(1) = bPan.CG(i,1) - bPan.CG(j,1);
            dist(2) = bPan.CG(i,2) - bPan.CG(j,2);
            dist = glob2loc * dist;
            trsfColloc = bPan.CG.row(i).transpose();
            trsfColloc = glob2loc * trsfColloc;

            coeffB = infcB(0,i,j, trsfColloc(0),trsfColloc(1),dist(2),trsfCorner1(0),trsfCorner1(1),
                           trsfCorner2(0),trsfCorner2(1),trsfCorner3(0),trsfCorner3(1),trsfCorner4(0),trsfCorner4(1));

            b2bAIC.A(i,j) = coeffB[0];
            b2bAIC.B(i,j) = coeffB[1];
        }

        // panel to field
        ii = 0; // loop reset
        FLAG = 0;
        for (int i = 0; i < fPan.nF; ++i) {
            // Split panel and store AIC
            if (sp.sI.size() != 0 && j == sp.sI[jj] && i == sp.fI[jj][ii]) {

                coeffSP = split_panel(j, i, trsfCorner1(0), trsfCorner2(0), trsfCorner3(0), trsfCorner4(0),
                                      trsfCorner1(1), trsfCorner2(1), trsfCorner3(1), trsfCorner4(1),
                                      bPan, fPan, sp);
                spAIC.A[jj].row(ii) = coeffSP[0];
                spAIC.B[jj].row(ii) = coeffSP[1];
                // Set global AIC to 0
                b2fAIC.A(i,j) = 0;
                b2fAIC.B(i,j) = 0;

                // Set flags
                if (ii == 0)
                    FLAG = 1;
                if (ii < sp.fI[jj].size()-1)
                    ii++;
            }
            else {
                dist(0) = fPan.CG(i,0) - bPan.CG(j,0);
                dist(1) = fPan.CG(i,1) - bPan.CG(j,1);
                dist(2) = fPan.CG(i,2) - bPan.CG(j,2);
                dist = glob2loc * dist;
                trsfColloc = fPan.CG.row(i).transpose();
                trsfColloc = glob2loc * trsfColloc;

                coeffB = infcB(0, 0, 1, trsfColloc(0), trsfColloc(1), dist(2), trsfCorner1(0), trsfCorner1(1),
                                 trsfCorner2(0), trsfCorner2(1), trsfCorner3(0), trsfCorner3(1), trsfCorner4(0), trsfCorner4(1));

                b2fAIC.A(i,j) = coeffB[0];
                b2fAIC.B(i,j) = coeffB[1];
            }
            if (i == fPan.nF-1 && FLAG && jj < sp.sI.size()-1)
                jj++;
        }
    }

    // Panel symmetry
    if (symY) {
        for (int j = 0; j < bPan.nP; ++j) {
            glob2loc.row(0) = bPan.l.row(j);
            glob2loc.row(1) = -bPan.p.row(j);
            glob2loc.row(2) = bPan.n.row(j);
            glob2loc.col(1) = -glob2loc.col(1);
            trsfCorner1 = bPan.v0.row(j).transpose();
            trsfCorner1(1) = -trsfCorner1(1);
            trsfCorner1 = glob2loc * trsfCorner1;
            trsfCorner2 = bPan.v1.row(j).transpose();
            trsfCorner2(1) = -trsfCorner2(1);
            trsfCorner2 = glob2loc * trsfCorner2;
            trsfCorner3 = bPan.v2.row(j).transpose();
            trsfCorner3(1) = -trsfCorner3(1);
            trsfCorner3 = glob2loc * trsfCorner3;
            trsfCorner4 = bPan.v3.row(j).transpose();
            trsfCorner4(1) = -trsfCorner4(1);
            trsfCorner4 = glob2loc * trsfCorner4;

            // panel to panel
            for (int i = 0; i < bPan.nP; ++i) {
                dist(0) = bPan.CG(i,0) - bPan.CG(j,0);
                dist(1) = bPan.CG(i,1) + bPan.CG(j,1);
                dist(2) = bPan.CG(i,2) - bPan.CG(j,2);
                dist = glob2loc * dist;
                trsfColloc = bPan.CG.row(i).transpose();
                trsfColloc = glob2loc * trsfColloc;

                coeffB = infcB(0,0,1,trsfColloc(0),trsfColloc(1),dist(2),trsfCorner1(0),trsfCorner1(1),
                               trsfCorner2(0),trsfCorner2(1),trsfCorner3(0),trsfCorner3(1),trsfCorner4(0),trsfCorner4(1));

                b2bAIC.A(i,j) -= coeffB[0]; // minus sign because corner order is trigonometric!
                b2bAIC.B(i,j) -= coeffB[1];
            }

            // panel to field
            for (int i = 0; i < fPan.nF; ++i) {
                dist(0) = fPan.CG(i, 0) - bPan.CG(j, 0);
                dist(1) = fPan.CG(i, 1) + bPan.CG(j, 1);
                dist(2) = fPan.CG(i, 2) - bPan.CG(j, 2);
                dist = glob2loc * dist;
                trsfColloc = fPan.CG.row(i).transpose();
                trsfColloc = glob2loc * trsfColloc;

                coeffB = infcB(0, 0, 1, trsfColloc(0), trsfColloc(1), dist(2), trsfCorner1(0), trsfCorner1(1),
                                trsfCorner2(0), trsfCorner2(1), trsfCorner3(0), trsfCorner3(1), trsfCorner4(0), trsfCorner4(1));

                b2fAIC.A(i,j) -= coeffB[0];
                b2fAIC.B(i,j) -= coeffB[1];
            }
        }
    }

    //// Wake panel AIC
    // each panel i_th is influenced by each wake panel (total number m) whose mu_M = mu_1M - mu_NM
    // on each row i, columns 0 and n-1 of each wake are modified (j*n and (j+1)*n -1)
    for (int j = 0; j < wPan.nS_; ++j) {
        glob2loc.row(0) = wPan.l.row(j);
        glob2loc.row(1) = wPan.p.row(j);
        glob2loc.row(2) = wPan.n.row(j);
        trsfCorner1 = wPan.v0.row(j).transpose();
        trsfCorner1 = glob2loc * trsfCorner1;
        trsfCorner2 = wPan.v1.row(j).transpose();
        trsfCorner2 = glob2loc * trsfCorner2;
        trsfCorner3 = wPan.v2.row(j).transpose();
        trsfCorner3 = glob2loc * trsfCorner3;
        trsfCorner4 = wPan.v3.row(j).transpose();
        trsfCorner4 = glob2loc * trsfCorner4;

        // panel to panel
        for (int i = 0; i < bPan.nP; ++i) {
            dist(0) = bPan.CG(i, 0) - wPan.CG(j, 0);
            dist(1) = bPan.CG(i, 1) - wPan.CG(j, 1);
            dist(2) = bPan.CG(i, 2) - wPan.CG(j, 2);
            dist = glob2loc * dist;
            trsfColloc = bPan.CG.row(i).transpose();
            trsfColloc = glob2loc * trsfColloc;

            coeffB = infcB(1,i,j,trsfColloc(0),trsfColloc(1),dist(2),trsfCorner1(0),trsfCorner1(1),
                           trsfCorner2(0),trsfCorner2(1),trsfCorner3(0),trsfCorner3(1),trsfCorner4(0),trsfCorner4(1));

            b2bAIC.A(i, j * bPan.nC_) += coeffB[0];
            b2bAIC.A(i, (j + 1) * bPan.nC_ - 1) -= coeffB[0];
        }

        // panel to field
        for (int i = 0; i < fPan.nF; ++i) {
            dist(0) = fPan.CG(i, 0) - wPan.CG(j, 0);
            dist(1) = fPan.CG(i, 1) - wPan.CG(j, 1);
            dist(2) = fPan.CG(i, 2) - wPan.CG(j, 2);
            dist = glob2loc * dist;
            trsfColloc = fPan.CG.row(i).transpose();
            trsfColloc = glob2loc * trsfColloc;

            coeffB = infcB(1, 0, 1, trsfColloc(0), trsfColloc(1), dist(2), trsfCorner1(0), trsfCorner1(1),
                            trsfCorner2(0), trsfCorner2(1), trsfCorner3(0), trsfCorner3(1), trsfCorner4(0), trsfCorner4(1));

            b2fAIC.A(i, j * bPan.nC_) += coeffB[0];
            b2fAIC.A(i, (j + 1) * bPan.nC_ - 1) -= coeffB[0];
        }
    }

    // Wake symmetry
    if (symY) {
        for (int j = 0; j < wPan.nS_; ++j) {
            glob2loc.row(0) = wPan.l.row(j);
            glob2loc.row(1) = -wPan.p.row(j);
            glob2loc.row(2) = wPan.n.row(j);
            glob2loc.col(1) = -glob2loc.col(1);
            trsfCorner1 = wPan.v0.row(j).transpose();
            trsfCorner1(1) = -trsfCorner1(1);
            trsfCorner1 = glob2loc * trsfCorner1;
            trsfCorner2 = wPan.v1.row(j).transpose();
            trsfCorner2(1) = -trsfCorner2(1);
            trsfCorner2 = glob2loc * trsfCorner2;
            trsfCorner3 = wPan.v2.row(j).transpose();
            trsfCorner3(1) = -trsfCorner3(1);
            trsfCorner3 = glob2loc * trsfCorner3;
            trsfCorner4 = wPan.v3.row(j).transpose();
            trsfCorner4(1) = -trsfCorner4(1);
            trsfCorner4 = glob2loc * trsfCorner4;

            // panel to panel
            for (int i = 0; i < bPan.nP; ++i) {
                dist(0) = bPan.CG(i, 0) - wPan.CG(j, 0);
                dist(1) = bPan.CG(i, 1) + wPan.CG(j, 1);
                dist(2) = bPan.CG(i, 2) - wPan.CG(j, 2);
                dist = glob2loc * dist;
                trsfColloc = bPan.CG.row(i).transpose();
                trsfColloc = glob2loc * trsfColloc;

                coeffB = infcB(1,0,1,trsfColloc(0),trsfColloc(1),dist(2),trsfCorner1(0),trsfCorner1(1),
                               trsfCorner2(0),trsfCorner2(1),trsfCorner3(0),trsfCorner3(1),trsfCorner4(0),trsfCorner4(1));

                b2bAIC.A(i, j * bPan.nC_) -= coeffB[0];
                b2bAIC.A(i, (j + 1) * bPan.nC_ - 1) += coeffB[0];
            }

            // panel to field
            for (int i = 0; i < fPan.nF; ++i) {
                dist(0) = fPan.CG(i, 0) - wPan.CG(j, 0);
                dist(1) = fPan.CG(i, 1) + wPan.CG(j, 1);
                dist(2) = fPan.CG(i, 2) - wPan.CG(j, 2);
                dist = glob2loc * dist;
                trsfColloc = fPan.CG.row(i).transpose();
                trsfColloc = glob2loc * trsfColloc;

                coeffB = infcB(1, 0, 1, trsfColloc(0), trsfColloc(1), dist(2), trsfCorner1(0), trsfCorner1(1),
                                trsfCorner2(0), trsfCorner2(1), trsfCorner3(0), trsfCorner3(1), trsfCorner4(0), trsfCorner4(1));

                b2fAIC.A(i, j * bPan.nC_) -= coeffB[0];
                b2fAIC.A(i, (j + 1) * bPan.nC_ - 1) += coeffB[0];
            }
        }
    }

    //// Field AIC
    for (int j = 0; j < fPan.nF; ++j) {
        xC[0] = fPan.vX(j,0) - fPan.CG(j,0);
        xC[1] = fPan.vX(j,1) - fPan.CG(j,0);
        yC[0] = fPan.vY(j,0) - fPan.CG(j,1);
        yC[1] = fPan.vY(j,1) - fPan.CG(j,1);
        zC[0] = fPan.vZ(j,0) - fPan.CG(j,2);
        zC[1] = fPan.vZ(j,1) - fPan.CG(j,2);

        // Field to field (diagonal)
        x = fPan.CG(j,0) - fPan.CG(j,0);
        y = fPan.CG(j,1) - fPan.CG(j,1);
        z = fPan.CG(j,2) - fPan.CG(j,2);
        coeffFF = infcFF(x, y, z, xC, yC, zC);
        f2fAIC.C(j,j) = coeffFF;
        // Field to field
        for (int i = j+1; i < fPan.nF; ++i) {
            x = fPan.CG(i,0) - fPan.CG(j,0);
            y = fPan.CG(i,1) - fPan.CG(j,1);
            z = fPan.CG(i,2) - fPan.CG(j,2);
            coeffFF = infcFF(x, y, z, xC, yC, zC);
            f2fAIC.C(i,j) = coeffFF;
            f2fAIC.C(j,i) = coeffFF;
        }

        // Field to panel
        for (int i = 0; i < bPan.nP; ++i) {
            x = bPan.CG(i,0) - fPan.CG(j,0);
            y = bPan.CG(i,1) - fPan.CG(j,1);
            z = bPan.CG(i,2) - fPan.CG(j,2);
            coeffFB = infcFB(x, y, z, xC, yC, zC);
            f2bAIC.Cu(i,j) = coeffFB[0];
            f2bAIC.Cv(i,j) = coeffFB[1];
            f2bAIC.Cw(i,j) = coeffFB[2];
        }
    }

    // Field symmetry
    if (symY) {
        for (int j = 0; j < fPan.nF; ++j) {
            xC[0] = fPan.vX(j,0) - fPan.CG(j,0);
            xC[1] = fPan.vX(j,1) - fPan.CG(j,0);
            yC[1] = -fPan.vY(j,0) + fPan.CG(j,1); // Y corner order inverted to preserve sign
            yC[0] = -fPan.vY(j,1) + fPan.CG(j,1);
            zC[0] = fPan.vZ(j,0) - fPan.CG(j,2);
            zC[1] = fPan.vZ(j,1) - fPan.CG(j,2);

            // Field to field
            for (int i = 0; i < fPan.nF; ++i) {
                x = fPan.CG(i,0) - fPan.CG(j,0);
                y = fPan.CG(i,1) + fPan.CG(j,1);
                z = fPan.CG(i,2) - fPan.CG(j,2);
                coeffFF = infcFF(x, y, z, xC, yC, zC);
                f2fAIC.C(i,j) += coeffFF;
            }

            // Field to panel
            for (int i = 0; i < bPan.nP; ++i) {
                x = bPan.CG(i,0) - fPan.CG(j,0);
                y = bPan.CG(i,1) + fPan.CG(j,1);
                z = bPan.CG(i,2) - fPan.CG(j,2);
                coeffFB = infcFB(x, y, z, xC, yC, zC);
                f2bAIC.Cu(i,j) += coeffFB[0];
                f2bAIC.Cv(i,j) += coeffFB[1];
                f2bAIC.Cw(i,j) += coeffFB[2];
            }
        }
    }

    //// Control display
    cout << "Done!" << endl << endl;
}


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */
#include <vector>
#include <iostream>
#include <algorithm>
#include <math.h>
#include <assert.h>
#include <malloc.h>
#include "DualGaussMixModel.h"
#include "SampleStats.h"
#include "SampleQuantiles.h"
#include "MathOptim.h"
#include "Utils.h"
#include "IonErr.h"
#include "Stats.h"

#include <Eigen/Dense>
#include <Eigen/LU>
using namespace std;

DualGaussMixModel::DualGaussMixModel(int n) {
  mMaxPoints = n;
  mMaxIter = 50;
  mConvergeDelta = .001;
  mTrim = .1;
}

MixModel DualGaussMixModel::FitDualGaussMixModel(const float *data, int length) {
  std::vector<float> mData;
  mData.reserve(std::min(mMaxPoints,length));
  // Copy over the data into our vector
  int step = (int) floor(1.0 * length/ mMaxPoints);
  step = std::max(step,1); // minimum step of 1;
  for (int i = 0; i < length; i+=step) {
    mData.push_back(data[i]);
  }

  // Get rid of any outliers
  TrimData(mData, mTrim);

  MixModel current;
  InitializeModel(mData, current);
  if (current.var1 == 0 || current.var2 == 0) {
    current.count = 0;
    return current;
  }
  ConvergeModel(mData, current);
  if (current.mu1 > current.mu2) {
    std::swap(current.mu1,current.mu2);
    std::swap(current.var1,current.var2);
    current.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * current.var1);
    current.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * current.var2);
    current.mix = 1.0 - current.mix;
  }
  return current;

}

int DualGaussMixModel::PredictCluster(MixModel &m, float data, double threshold, double &p2ownership) {
  if (!m.thresholdSet) {
    SetThreshold(m);
  }
  if (data < m.threshold) { return 1;}
  else {
    return 2;
  }
}

void DualGaussMixModel::AssignToCluster(int *cluster, MixModel &model, const float *data,
					int length, double threshold) {
  for (int i = 0; i < length; i++) {

    double p1 = DNorm(data[i], model.mu1, model.var1, model.var1sq2p);
    p1 = p1 * (1.0 - model.mix);

    double p2 = DNorm(data[i], model.mu2, model.var2, model.var2sq2p);
    p2 = p2 * (model.mix);

    double p2ownership = p2 / (p1 + p2);

    if(p2ownership >= threshold) {
      cluster[i] = 2;
    }
    else {
      cluster[i] = 1;
    }
  }
}

double DualGaussMixModel::DNorm(double x, double mu, double var, double varsq2p) {

  double e = ExpApprox( -1.0 * (x - mu) * (x - mu)/ (2 * var));
  double p = varsq2p * e;
  return p;
}

void DualGaussMixModel::UpdateResponsibility(std::vector<float> &ownership,
					     const std::vector<float> &data,
					     const MixModel &model) {
  const float *__restrict data_start = &data[0];
  const float *__restrict data_end = data_start + data.size();
  float *__restrict owner_start = &ownership[0];
  while(data_start != data_end) {
    CalculateResponsibility(model, *data_start, *owner_start);
    data_start++;
    owner_start++;
  }
}

void DualGaussMixModel::SetThreshold(MixModel &m) {
  // Set the threshold at which the responsibility is the same.
  // we want to find the two decision boundaries and only use the one that is between the two means.
  // Usually the bead cluster has much higher variance and will pick up outliers on the
  // other side of the empty distribution otherwise.

  // a,b,c are for usual quadradic equation x = (-b +/- sqrt(b^2 -
  // 4ac))/2a after solving case for two responsibilities being equal
  float a = (m.var1 - m.var2);
  float b = (2*m.mu1 *m.var2) - (2 * m.var1 * m.mu2);
  float v1v2 = m.var2 * m.var1 * 2;
  float c = m.var1 * m.mu2 * m.mu2  - m.var2 * m.mu1 * m.mu1  + v1v2 * (-1 * log(1/sqrt(2 *DualGaussMixModel::GPI*m.var2)) + log(1.0f/sqrt(2 * DualGaussMixModel::GPI * m.var1)) - log(m.mix) +  log(1.0f-m.mix));
  double t1 = (-1*b + sqrt(b*b - 4 * a * c))/(2 * a);
  double t2 = (-1*b - sqrt(b*b - 4 * a * c))/(2 * a);
  double d1 = fabs(m.mu1 - t1) + fabs(m.mu2 - t1);
  double d2 = fabs(m.mu1 - t2) + fabs(m.mu2 - t2);
  if (!isfinite(d1) || !isfinite(d2)) {
    // brute force if not solvable
    double step = (m.mu2 - m.mu1) / 100;
    double val = m.mu1;
    double minDiff = numeric_limits<double>::max();
    for (size_t i = 0; i < 100; i++) {
      float d = val + i * step;
      float owner = -1;
      bool g = CalculateResponsibility(m, d, owner);
      if (g && fabs(owner - .5) < minDiff) {
        m.threshold = val + i * step;
        minDiff = fabs(owner - .5);
      }
    }
  }
  else if (d1 > d2) {
    m.threshold = t2;
  }
  else {
    m.threshold = t1;
  }
  m.thresholdSet = true;
}

bool DualGaussMixModel::CalculateResponsibility(const MixModel &m, float data, float &ownership) {
  ownership = -1.0f;
  // Ok we're in a not obvious area calculate a true weight.
  double p1 = DNorm(data, m.mu1, m.var1, m.var1sq2p);
  p1 = p1 * (1.0 - m.mix);

  double p2 = DNorm(data, m.mu2, m.var2, m.var2sq2p);
  p2 = p2 * (m.mix);

  if(!(p1 >= 0 && p1 <= 1.0 && p2 >= 0 && p2 <= 1.0)) {
    return false;
  }

  // as of 11/16/2010, ExpApprox can return exactly zero and this behavior is desired
  // in other areas of the code where it is used.
  // This causes a degenerate case here since p1 and p2 can both be exactly zero,
  // which in turn causes p2/(p1+p2) to be NaN.
  // The original behavior (before the modification to ExpApprox) would have
  // resulted in equally paritioning between p1 and p2 in this case, so this special
  // case retains that behavior
  if (p1 == 0 && p2 == 0)
    ownership = 0.5;
  else
    ownership = p2 / (p1 + p2);
  return true;
}

bool DualGaussMixModel::UpdateResponsibility(std::vector<float> &ownership,
					     const std::vector<std::pair<float,int8_t> > &data,
					     const MixModel &model) {
//  ownership.resize(data.size());
  bool flag = true;
  float *__restrict owner_start = &ownership[0];
  float *__restrict owner_end = owner_start + ownership.size();
  const std::pair<float,int8_t> *__restrict data_start = &data[0];
  while (owner_start != owner_end) {
    flag &= CalculateResponsibility(model, data_start->first, *owner_start);
    owner_start++;
    data_start++;
  }
  return flag;
}

double DualGaussMixModel::CalcWeightedMean(const std::vector<float> &weights,
					   const std::vector<float> &values,
					   bool inverse) {
  double mean = 0;
  double w = 0;
  const float *__restrict weight_start = &weights[0];
  const float *__restrict weight_end = weight_start + weights.size();
  const float *__restrict values_start = &values[0];
  while (weight_start != weight_end) {
    float wi = inverse ? 1.0f - *weight_start : *weight_start;
    if (wi > 0) {
      w += wi;
      mean += (*values_start - mean) * wi/w;
    }
    weight_start++;
    values_start++;
  }
  return mean;
}

double DualGaussMixModel::CalcWeightedMean(const std::vector<float> &weights,
					   const std::vector<std::pair<float,int8_t> > &values,
					   bool inverse) {
  double mean = 0;
  double w = 0;
  const float *__restrict weight_start = &weights[0];
  const float *__restrict weight_end = weight_start + weights.size();
  const std::pair<float, int8_t> *__restrict values_start = &values[0];
  while (weight_start != weight_end) {
    float wi = inverse ? 1.0f - *weight_start : *weight_start;
    if (wi > 0) {
      w += wi;
      mean += (values_start->first - mean) * wi/w;
    }
    weight_start++;
    values_start++;
  }
  return mean;
}

//   double mean = 0;
//   double w = 0;
//   for (unsigned int i = 0; i < weights.size(); i++) {
//     float wi = weights[i];
//     if ( inverse ) {
//       wi = 1.0f - wi;
//     }
//     if (wi > 0) {
//       w += wi;
//       mean += (values[i].first - mean) * (wi/w);
//     }
//   }
//   return mean;
// }

double DualGaussMixModel::CalcWeightedVar(double mu,
					  const std::vector<float> &weights,
					  const std::vector<float> &values,
					  bool inverse) {

  double var = 0;
  double w = 0;
  double b = 0;
  const float *__restrict weight_start = &weights[0];
  const float *__restrict weight_end = weight_start + weights.size();
  const float *__restrict values_start = &values[0];
  while (weight_start != weight_end) {
    float wi = inverse ? 1.0f - *weight_start : *weight_start;
    if (wi > 0) {
      double delta = *values_start - mu;
      w += wi;
      b += wi * wi;
      var += (delta * delta - var) * wi/w;
    }
    weight_start++;
    values_start++;
  }
  double ret = var * (w * w / ((w*w) - b));
  if (ret == 0) {
    ret = 1;
  }
  return ret;
}

double DualGaussMixModel::CalcWeightedVar(double mu,
					  const std::vector<float> &weights,
					  const std::vector<std::pair<float, int8_t> > &values,
					  bool inverse) {

  double var = 0;
  double w = 0;
  double b = 0;
  const float *__restrict weight_start = &weights[0];
  const float *__restrict weight_end = weight_start + weights.size();
  const std::pair<float, int8_t> *__restrict values_start = &values[0];
  while (weight_start != weight_end) {
    float wi = inverse ? 1.0f - *weight_start : *weight_start;
    if (wi > 0) {
      double delta = values_start->first - mu;
      w += wi;
      b += wi * wi;
      var += (delta * delta - var) * wi/w;
    }
    weight_start++;
    values_start++;
  }
  double ret = var * (w * w / ((w*w) - b));
  if (ret == 0) {
    ret = 1;
  }
  return ret;
}

double DualGaussMixModel::CalcWeightedVar(double mu,
                                          float *weights,
                                          float *values,
                                          int count,
					  bool inverse) {

  double var = 0;
  double w = 0;
  double b = 0;
  const float *__restrict weight_start = &weights[0];
  const float *__restrict weight_end = weight_start + count;
  const float *__restrict values_start = &values[0];
  while (weight_start != weight_end) {
    float wi = inverse ? 1.0f - *weight_start : *weight_start;
    if (wi > 0) {
      double delta = *values_start - mu;
      w += wi;
      b += wi * wi;
      var += (delta * delta - var) * wi/w;
    }
    weight_start++;
    values_start++;
  }
  double ret = var * (w * w / ((w*w) - b));
  if (ret == 0) {
    ret = 1;
  }
  return ret;
}

void DualGaussMixModel::CalcWeightedMeanVar( const std::vector<float> &weights,
					  const std::vector<std::pair<float, int8_t> > &values,
					  bool inverse,
                                          double &mean, double &var) {
  double sum_weight = 0;
  double m2 = 0, temp = 0, delta = 0, R = 0;
  mean = 0;
  var = 0;
  const float *__restrict weight_start = &weights[0];
  const float *__restrict weight_end = weight_start + weights.size();
  const std::pair<float, int8_t> *__restrict values_start = &values[0];
  int count = 0;
  while (weight_start != weight_end) {
    count ++;
    float wi = inverse ? 1.0f - *weight_start : *weight_start;
    if (wi > 0.0f) {
      temp = wi + sum_weight;
      delta = values_start->first - mean;
      R = delta * wi / temp;
      mean += R;
      m2 += sum_weight * delta * R;
      sum_weight = temp;
    }
    weight_start++;
    values_start++;
  }
  if (sum_weight > 0)
    var = m2/sum_weight;
  assert(isfinite(var));
}

void DualGaussMixModel::UpdateModel(MixModel &update,
				    const std::vector<float> &data,
				    const std::vector<float> &cluster) {
  update.mu1 = CalcWeightedMean(cluster, data, true);
  update.var1 = CalcWeightedVar(update.mu1, cluster, data, true);
  update.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * update.var1);

  update.mu2 = CalcWeightedMean(cluster, data, false);
  update.var2 = CalcWeightedVar(update.mu2, cluster, data, false);
  update.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * update.var2);
  update.mix = 0;
  for (unsigned int i = 0; i < cluster.size(); i++) {
    update.mix += cluster[i];
  }
  update.mix /= cluster.size();

  assert(update.mix >= 0.0 && update.mix <= 1.0);
}

void DualGaussMixModel::UpdateModel(MixModel &update,
				    const std::vector<std::pair<float,int8_t> >&data,
				    const std::vector<float> &cluster) {
  CalcWeightedMeanVar(cluster, data, true, update.mu1, update.var1);
  //  update.mu1 = CalcWeightedMean(cluster, data, true);
  //  update.var1 = CalcWeightedVar(update.mu1, cluster, data, true);
  update.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * update.var1);

  CalcWeightedMeanVar(cluster, data, false, update.mu2, update.var2);
  //  update.mu2 = CalcWeightedMean(cluster, data, false);
  //  update.var2 = CalcWeightedVar(update.mu2, cluster, data, false);
  update.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * update.var2);
  update.mix = 0;
  const float *__restrict cluster_start = &cluster[0];
  const float *__restrict cluster_end = cluster_start + cluster.size();
  while (cluster_start != cluster_end) {
    update.mix += *cluster_start++;
  }
  update.mix /= cluster.size();
  assert(update.mix >= 0.0 && update.mix <= 1.0);
}

void DualGaussMixModel::UpdateResponsibilityVec(float * _ownership,
                                                float * _data,
                                                float * _p1,
                                                float * _p2,
                                                int size,
                                                const MixModel &model) {
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> data(_data, size);
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> p1(_p1, size);
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> p2(_p2, size);
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> own(_ownership, size);

  p1 = data - model.mu1;
  p1 = p1.square();
  // p1 = -1 * p1;
  // p1 = p1 / (2 * model.var1);
  float mult = -1.0f / (2 * model.var1);
  p1 = p1 * mult;
  p1 = p1.exp();
  mult = model.var1sq2p * (1.0f - model.mix);
  p1 = p1 * mult;
  // p1 = p1 * model.var1sq2p;
  // p1 = p1 * (1.0f - model.mix);

  mult = -1.0f / (2 * model.var2);
  p2 = data - model.mu2;
  p2 = p2.square();
  p2 = p2 * mult;
  // p2 = -1 * p2;
  // p2 = p2 / (2 * model.var2);
  p2 = p2.exp();
  mult = model.var2sq2p * model.mix;
  p2 = p2 * mult;
  // p2 = p2 * model.var2sq2p;
  // p2 = p2 * model.mix;

  own = p2 / (p1 + p2);
  // touch up to fix any crazy values
  float *__restrict own_start = _ownership;
  float *__restrict own_end = own_start + size;
  while(own_start != own_end) {
    if (!isfinite(*own_start)) {
      *own_start = .5f;
    }
    own_start++;
  }
}

// void DualGaussMixModel::ConvergeModelFaster(const std::vector<float> &vec_data, MixModel &model) {
//   bool converged = false;
//   int iterationCount = 0;
//   int total_size = sizeof(float) * vec_data.size();
//   float *cluster = (float *) memalign(32, total_size);
//   float *data = (float *) memalign(32, total_size);
//   float *p1 = (float *) memalign(32, total_size);
//   float *p2 = (float *) memalign(32, total_size);
//   memcpy(data, &vec_data[0], total_size);
//   while (!converged && iterationCount++ < mMaxIter) {
//     MixModel newModel = model;
//     //    UpdateResponsibility(cluster, data, model);
//     UpdateResponsibilityVec(cluster, data, p1, p2, vec_data.size(), model);
//     UpdateModel(newModel, data, cluster, p1, vec_data.size());
//     double mu1Diff = fabs(newModel.mu1 - model.mu1);
//     double mu2Diff = fabs(newModel.mu2 - model.mu2);
//     double var1Diff = fabs(newModel.var1 - model.var1);
//     double var2Diff = fabs(newModel.var2 - model.var2);
//     double muDiff = std::max(mu1Diff, mu2Diff);
//     double varDiff = std::max(var1Diff, var2Diff);
//     if (muDiff < mConvergeDelta && varDiff < mConvergeDelta ) {
//       converged = true;
//     }
//     model = newModel;
//   }
//   UpdateResponsibilityVec(cluster, data, model);
//   free(cluster);
//   free(data);
//   free(p1);
//   free(p2);
// }

void DualGaussMixModel::ConvergeModel(const std::vector<float> &data, MixModel &model) {
  bool converged = false;
  int iterationCount = 0;
  std::vector<float> cluster(data.size(), .5);
  while (!converged && iterationCount++ < mMaxIter) {
    MixModel newModel = model;
    UpdateResponsibility(cluster, data, model);
    UpdateModel(newModel, data, cluster);
    double mu1Diff = fabs(newModel.mu1 - model.mu1);
    double mu2Diff = fabs(newModel.mu2 - model.mu2);
    double var1Diff = fabs(newModel.var1 - model.var1);
    double var2Diff = fabs(newModel.var2 - model.var2);
    double muDiff = std::max(mu1Diff, mu2Diff);
    double varDiff = std::max(var1Diff, var2Diff);
    if (muDiff < mConvergeDelta && varDiff < mConvergeDelta ) {
      converged = true;
    }
    model = newModel;
  }
  UpdateResponsibility(cluster, data, model);
}

 void DualGaussMixModel::ConvergeModel(const std::vector<std::pair<float,int8_t> > &data, MixModel &model) {
  bool converged = false;
  int iterationCount = 0;
  std::vector<float> cluster(data.size(), .5);
  while (!converged && iterationCount++ < mMaxIter) {
    MixModel newModel = model;
    bool ok = UpdateResponsibility(cluster, data, model);
    if (!ok) {
      model.count = 0;
      return;
    }
    UpdateModel(newModel, data, cluster);
    double mu1Diff = fabs(newModel.mu1 - model.mu1);
    double mu2Diff = fabs(newModel.mu2 - model.mu2);
    double var1Diff = fabs(newModel.var1 - model.var1);
    double var2Diff = fabs(newModel.var2 - model.var2);
    double muDiff = std::max(mu1Diff, mu2Diff);
    double varDiff = std::max(var1Diff, var2Diff);
    if (muDiff < mConvergeDelta && varDiff < mConvergeDelta ) {
      converged = true;
    }
    model = newModel;
  }
  UpdateResponsibility(cluster, data, model);
  bool ok = UpdateResponsibility(cluster, data, model);
  if (!ok) {
    model.count = 0;
    return;
  }
}

void DualGaussMixModel::InitializeModel(const std::vector<float> &data, MixModel &model) {
  // Get initial variance estimate;
  SampleStats<float> ss;
  ss.AddValues(data);

  // Find clusters
  int mu1Est = (int)(.25 * data.size());
  int mu2Est = (int)(.75 * data.size());
  model.count = data.size();
  model.mix = .5;
  model.mu1 = data[mu1Est];
  model.mu2 = data[mu2Est];
  model.var1 = ss.GetVar();
  model.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * model.var1);
  model.var2 = ss.GetVar();
  model.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * model.var2);
}

 void DualGaussMixModel::InitializeModel(const std::vector<std::pair<float, int8_t> > &data, MixModel &model) {
  // Get initial variance estimate;
  SampleStats<float> beads;
  SampleStats<float> empties;
  SampleQuantiles<float> allData(1000);

  for (size_t i = 0; i < data.size(); i++) {
    if (data[i].second == 0) {
      empties.AddValue(data[i].first);
    }
    else if (data[i].second == 1) {
      beads.AddValue(data[i].first);
    }
    allData.AddValue(data[i].first);
  }

  // Find clusters
  if (beads.GetCount() > 30) {
    model.mu2 = beads.GetMean();
    model.var2 = beads.GetVar();
  }
  else {
    model.mu2 = allData.GetQuantile(.75);
    double iqr = model.mu2 - allData.GetQuantile(.25);
    model.var2 = iqr * iqr;
  }

  if (empties.GetCount() > 30) {
    model.mu1 = empties.GetMean();
    model.var1 = empties.GetVar();
  }
  else {
    model.mu1 = allData.GetQuantile(.25);
    double iqr = allData.GetQuantile(.75) - model.mu1;
    model.var1 = iqr * iqr;
  }
  model.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * model.var1);
  model.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * model.var2);

  model.mix = .5;
  model.count = data.size();
}

// void DualGaussMixModel::InitializeModel(const std::vector<std::pair<float, int8_t> > &data, MixModel &model) {
void DualGaussMixModel::InitializeModel(const float *data, const int8_t *assign, int size, MixModel &model) {
  // Get initial variance estimate;
  SampleStats<float> beads;
  SampleStats<float> empties;
  SampleQuantiles<float> allData(1000);

  for (int i = 0; i < size; i++) {
    if (assign[i] == 0) {
      empties.AddValue(data[i]);
    }
    else if (assign[i] == 1) {
      beads.AddValue(data[i]);
    }
    allData.AddValue(data[i]);
  }

  // Find clusters
  if (beads.GetCount() > 30) {
    model.mu2 = beads.GetMean();
    model.var2 = beads.GetVar();
  }
  else {
    model.mu2 = allData.GetQuantile(.75);
    double iqr = model.mu2 - allData.GetQuantile(.25);
    model.var2 = iqr * iqr;
  }

  if (empties.GetCount() > 30) {
    model.mu1 = empties.GetMean();
    model.var1 = empties.GetVar();
  }
  else {
    model.mu1 = allData.GetQuantile(.25);
    double iqr = allData.GetQuantile(.75) - model.mu1;
    model.var1 = iqr * iqr;
  }
  model.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * model.var1);
  model.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * model.var2);

  model.mix = .5;
  model.count = size;
}

void DualGaussMixModel::TrimData(std::vector<float> &data, double trim) {
  // trim of the min and max quantiles corresponding to our mTrim value
  std::sort(data.begin(), data.end());
  int index1 = (int) floor(data.size() * (trim/2.0)+.5);
  int index2 = (int) floor(data.size() * (1.0 - trim/2.0)+.5);
  int count = 0;
  for (int i = index1; i < index2; i++) {
    data[count++] = data[i];
  }
  data.resize(count);
}

void DualGaussMixModel::TrimData(std::vector<std::pair<float,int8_t> > &data, double trim) {
  // trim of the min and max quantiles corresponding to our mTrim value
  std::sort(data.begin(), data.end());
  int index1 = (int) floor(data.size() * (trim/2.0)+.5);
  int index2 = (int) floor(data.size() * (1.0 - trim/2.0)+.5);
  int count = 0;
  for (int i = index1; i < index2; i++) {
    data[count++] = data[i];
  }
  data.resize(count);
}

void DualGaussMixModel::TrimData(const float *data,  const int8_t *assignments, int size, double trim,
                                 float *trim_data, int8_t * trim_assignments, int &trim_count) {
  vector<float> sdata(size);
  std::copy(data, data+size, sdata.begin());
  // trim of the min and max quantiles corresponding to our mTrim value
  std::sort(sdata.begin(), sdata.end());
  float lower_threshold = ionStats::quantile_sorted(sdata, trim/2.0);
  float higher_threshold = ionStats::quantile_sorted(sdata, (1.0 - (trim/2.0)));
  trim_count = 0;
  for (int i = 0; i < size; i++) {
    if (data[i] >= lower_threshold && data[i] <= higher_threshold) {
      trim_data[trim_count] = data[i];
      trim_assignments[trim_count] = assignments[i];
      trim_count++;
    }
  }
}

MixModel DualGaussMixModel::FitDualGaussMixModel(const float *data, const int8_t *assignments, int length) {
  std::vector<std::pair<float,int8_t> > mData;
  mData.reserve(std::min(mMaxPoints,length));
  // Copy over the data into our vector
  int step = (int) floor(1.0 * length/ mMaxPoints);
  step = std::max(step,1); // minimum step of 1;
  for (int i = 0; i < length; i++) {
    if ( i % step == 0 || assignments[i] >= 0) {
      mData.push_back(std::pair<float,int8_t>(data[i], assignments[i]));
    }
  }

  // Get rid of any outliers
  TrimData(mData, mTrim);

  MixModel current;
  InitializeModel(mData, current);
  if (current.var1 <= 0 || current.var2 <= 0) {
    current.count = 0;
    return current;
  }
  ConvergeModel(mData, current);
  if (current.mu1 > current.mu2) {
    std::swap(current.mu1,current.mu2);
    std::swap(current.var1,current.var2);
    current.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * current.var1);
    current.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * current.var2);
    current.mix = 1.0 - current.mix;
  }
  return current;

}

MixModel DualGaussMixModel::FitDualGaussMixModelFaster(const float *data, const int8_t *assignments, int length) {
  // Copy over the data into our vector
  int step = (int) floor(1.0 * length/ mMaxPoints);
  float *data_sample = (float *) memalign(32, length * sizeof(float));
  int8_t *assign_sample = (int8_t*) memalign(32, length * sizeof(int8_t));
  step = std::max(step,1); // minimum step of 1;
  int count = 0;
  for (int i = 0; i < length; i++) {
    if ( i % step == 0 || assignments[i] >= 0) {
      data_sample[count] = data[i];
      assign_sample[count] = assignments[i];
      count++;
    }
  }

  float *data_trim = (float *) memalign(32, count * sizeof(float));
  int8_t*assign_trim = (int8_t *) memalign(32, count * sizeof(int8_t));
  // Get rid of any outliers
  int trim_count = 0;
  TrimData(data_sample, assign_sample, count, mTrim,
           data_trim, assign_trim, trim_count);

  MixModel current;
  //  InitializeModel(mData, current);
  InitializeModel(data_trim, assign_trim, trim_count, current);
  if (current.var1 <= 0 || current.var2 <= 0) {
    current.count = 0;
    return current;
  }
  ConvergeModelFaster(data_trim, trim_count, current);
  if (current.mu1 > current.mu2) {
    std::swap(current.mu1,current.mu2);
    std::swap(current.var1,current.var2);
    current.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * current.var1);
    current.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * current.var2);
    current.mix = 1.0 - current.mix;
  }
  free(data_sample);
  free(assign_sample);
  free(data_trim);
  free(assign_trim);
  return current;

}

void DualGaussMixModel::ConvergeModelFaster(float *data, int count, MixModel &model) {
  bool converged = false;
  int iterationCount = 0;
  int total_size = sizeof(float) * count;
  float *cluster = (float *) memalign(32, total_size);
  std::fill(cluster, cluster + count, .5f);
  float *p1 = (float *) memalign(32, total_size);
  float *p2 = (float *) memalign(32, total_size);
  while (!converged && iterationCount++ < mMaxIter) {
    MixModel newModel = model;
    //    UpdateResponsibility(cluster, data, model);
    UpdateResponsibilityVec(cluster, data, p1, p2, count, model);
    UpdateModelVec(newModel, data, cluster, p1, count);
    double mu1Diff = fabs(newModel.mu1 - model.mu1);
    double mu2Diff = fabs(newModel.mu2 - model.mu2);
    double var1Diff = fabs(newModel.var1 - model.var1);
    double var2Diff = fabs(newModel.var2 - model.var2);
    double muDiff = std::max(mu1Diff, mu2Diff);
    double varDiff = std::max(var1Diff, var2Diff);
    if (muDiff < mConvergeDelta && varDiff < mConvergeDelta ) {
      converged = true;
    }
    model = newModel;
  }
  //  UpdateResponsibility(cluster, data, model);
  UpdateResponsibilityVec(cluster, data, p1, p2, count, model);
  FREEZ(&p1);
  FREEZ(&p2);
  FREEZ(&cluster);
}

double DualGaussMixModel::CalcWeightedMeanVec(float *_weights,
                                              float *_values,
                                              float *_scratch,
                                              int size,
                                              bool inverse) {
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> values(_values, size);
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> weights(_weights, size);
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> scratch(_scratch, size);
  float mean = 0;
  if (inverse) {
    scratch = 1.0f - weights;
    float weight_sum = scratch.sum();
    scratch = scratch * values;
    float scratch_sum = scratch.sum();
    mean = scratch_sum / weight_sum;
  }
  else {
    scratch = weights * values;
    float scratch_sum = scratch.sum();
    float weight_sum = weights.sum();
    mean = scratch_sum / weight_sum;
  }
  return mean;
}

void DualGaussMixModel::UpdateModelVec(MixModel &update,
                                       float *data,
                                       float *cluster,
                                       float *scratch,
                                       int count) {
  update.mu1 = CalcWeightedMeanVec(cluster, data, scratch, count, true);
  update.var1 = CalcWeightedVar(update.mu1, cluster, data, count, true);
  update.var1sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * update.var1);

  update.mu2 = CalcWeightedMeanVec(cluster, data, scratch, count, false);
  update.var2 = CalcWeightedVar(update.mu2, cluster, data, count, false);
  update.var2sq2p = 1 / sqrt(2 * DualGaussMixModel::GPI * update.var2);
  Eigen::Map<Eigen::ArrayXf, Eigen::Aligned> cluster_vec(cluster, count);
  update.mix = cluster_vec.sum();
  update.mix /= count;

  assert(update.mix >= 0.0 && update.mix <= 1.0);
}


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTI_STUDENT_T_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTI_STUDENT_T_RNG_HPP

#include <boost/math/special_functions/gamma.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/mat/err/check_ldlt_factor.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/dot_product.hpp>
#include <stan/math/prim/mat/fun/subtract.hpp>
#include <stan/math/prim/scal/fun/log1p.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/mat/prob/multi_normal_rng.hpp>
#include <stan/math/prim/scal/prob/inv_gamma_rng.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <cstdlib>

namespace stan {
  namespace math {

    template <class RNG>
    inline Eigen::VectorXd
    multi_student_t_rng(double nu,
          const Eigen::Matrix<double, Eigen::Dynamic, 1>& mu,
          const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& s,
          RNG& rng) {
      static const char* function("multi_student_t_rng");

      check_finite(function, "Location parameter", mu);
      check_symmetric(function, "Scale parameter", s);
      check_not_nan(function, "Degrees of freedom parameter", nu);
      check_positive(function, "Degrees of freedom parameter", nu);

      Eigen::VectorXd z(s.cols());
      z.setZero();

      double w = inv_gamma_rng(nu / 2, nu / 2, rng);
      return mu + std::sqrt(w) * multi_normal_rng(z, s, rng);
    }

  }
}
#endif


#pragma once
#include <string_view>
#include <boost/beast/http.hpp>
#include <ctre.hpp>
#include "wrapper/general_store.hpp"
#include "wrapper/protocol_support/protocols.hpp"

using namespace std::literals;

constexpr static ctll::fixed_string numeric_id_pattern = "[0-9A-Fa-f]{1,8}";
constexpr auto numeric_id_match(std::string_view id) noexcept { return ctre::match<numeric_id_pattern>(id); }

// Contract: sv matches `numeric_id_pattern`
constexpr std::uint32_t hex_decode_id(std::string_view sv) noexcept {
    std::uint32_t ret = 0;
    for (char c : sv) {
        ret <<= 4u;

        if (c >= '0' && '9' >= c)
            ret += c - '0';
        else if (c >= 'A' && 'F' >= c)
            ret += c - 'A' + 10;
        else if (c >= 'a' && 'f' >= c)
            ret += c - 'a' + 10;
    }
    return ret;
}
constexpr std::array<char, sizeof(std::uint32_t) * 2> hex_encode_id(std::uint32_t id) noexcept {
    constexpr auto hex = "0123456789abcdef"sv;
    std::array<char, sizeof(std::uint32_t)* 2> ret = {};
    for (auto it = ret.rbegin(); it < ret.rend(); ++it) {
        *it = hex[id & 0xfu];
        id >>= 16u;
    }
    return ret;
}

template <TransportProto proto> auto handle_async_retrieve(GeneralStore& gstore, std::string_view str_id) {
    if constexpr (proto == TransportProto::HTTP1) {
        using Ret = std::pair<boost::beast::http::status, std::string>;

        if (!numeric_id_match(str_id))
            return Ret{boost::beast::http::status::not_found, {}};

        auto [status, val] = gstore.async_store.value_if_ready(hex_decode_id(str_id));
        switch (status) {
        case AsyncStore::TaskStatus::non_existent:
            return Ret{boost::beast::http::status::not_found, {}};
        case AsyncStore::TaskStatus::in_progress:
            return Ret{boost::beast::http::status::processing, {}};
        case AsyncStore::TaskStatus::finished:
            return Ret{boost::beast::http::status::found, val};
        }
    }
}


#ifndef USE_CUDA

#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Dense>

#include <array>
#include <algorithm>

using namespace Utility;
using Utility::Constants::Pi;

namespace Engine
{
    namespace Vectormath
    {

        void rotate(const Vector3 & v, const Vector3 & axis, const scalar & angle, Vector3 & v_out)
        {
            v_out = v * std::cos(angle) + axis.cross(v) * std::sin(angle) +
                    axis * axis.dot(v) * (1 - std::cos(angle));
        }

        // XXX: should we add test for that function since it's calling the already tested rotat()
        void rotate( const vectorfield & v, const vectorfield & axis, const scalarfield & angle,
                     vectorfield & v_out )
        {
            for( unsigned int i=0; i<v_out.size(); i++)
                rotate( v[i], axis[i], angle[i], v_out[i] );
        }

        Vector3 decompose(const Vector3 & v, const std::vector<Vector3> & basis)
        {
            Eigen::Ref<const Matrix3> A = Eigen::Map<const Matrix3>(basis[0].data());
            return A.colPivHouseholderQr().solve(v);
        }

        /////////////////////////////////////////////////////////////////

        void Build_Spins(vectorfield & positions, intfield & atom_types,
                         const std::vector<Vector3> & cell_atoms, const intfield & cell_atom_types,
                         const std::vector<Vector3> & translation_vectors, const intfield & n_cells)
        {
            // Check for erronous input placing two spins on the same location
            int max_a = std::min(10, n_cells[0]);
            int max_b = std::min(10, n_cells[1]);
            int max_c = std::min(10, n_cells[2]);
            Vector3 sp;
            for (unsigned int i = 0; i < cell_atoms.size(); ++i)
            {
                for (unsigned int j = 0; j < cell_atoms.size(); ++j)
                {
                    for (int ka = -max_a; ka <= max_a; ++ka)
                    {
                        for (int k2 = -max_b; k2 <= max_b; ++k2)
                        {
                            for (int k3 = -max_c; k3 <= max_c; ++k3)
                            {
                                // Norm is zero if translated basis atom is at position of another basis atom
                                sp = cell_atoms[i] - (cell_atoms[j]
                                    + ka * translation_vectors[0]
                                    + k2 * translation_vectors[1]
                                    + k3 * translation_vectors[2]);
                                if ( (i != j || ka != 0 || k2 != 0 || k3 != 0) &&
                                    std::abs(sp[0]) < 1e-9 && std::abs(sp[1]) < 1e-9 && std::abs(sp[2]) < 1e-9 )
                                {
                                    spirit_throw(Exception_Classifier::System_not_Initialized, Log_Level::Severe,
                                        "Unable to initialize Spin-System, since 2 spins occupy the same space.\nPlease check the config file!");
                                }
                            }
                        }
                    }
                }
            }

            // Build up the spins array
            int i, j, k, s, ispin;
            int nos_basic = cell_atoms.size();
            //int nos = nos_basic * n_cells[0] * n_cells[1] * n_cells[2];
            Vector3 build_array;
            for (k = 0; k < n_cells[2]; ++k) {
                for (j = 0; j < n_cells[1]; ++j) {
                    for (i = 0; i < n_cells[0]; ++i) {
                        for (s = 0; s < nos_basic; ++s) {
                            ispin = k * n_cells[1] * n_cells[0] * nos_basic +
                                    j * n_cells[0] * nos_basic + i * nos_basic + s;
                            build_array = i * translation_vectors[0] + j * translation_vectors[1] +
                                          k * translation_vectors[2];
                            // paste initial spin orientations across the lattice translations
                            //spins[dim*nos + ispin] = spins[dim*nos + s];
                            // calculate the spin positions
                            positions[ispin] = cell_atoms[s] + build_array;
                            atom_types[ispin] = cell_atom_types[s];
                        }// endfor s
                    }// endfor k
                }// endfor j
            }// endfor dim

        }// end Build_Spins

        std::array<scalar,3> Magnetization(const vectorfield & vf)
        {
            Vector3 vfmean = mean(vf);
            std::array<scalar, 3> M{vfmean[0], vfmean[1], vfmean[2]};
            return M;
        }

        scalar solid_angle_1(const Vector3 & v1, const Vector3 & v2, const Vector3 & v3)
        {
            // Get sign
            scalar pm = v1.dot(v2.cross(v3));
            if (pm != 0) pm /= std::abs(pm);

            // angle
            scalar solid_angle = ( 1 + v1.dot(v2) + v2.dot(v3) + v3.dot(v1) ) /
                                std::sqrt( 2 * (1+v1.dot(v2)) * (1+v2.dot(v3)) * (1+v3.dot(v1)) );
            if (solid_angle == 1)
                solid_angle = 0;
            else
                solid_angle = pm * 2 * std::acos(solid_angle);

            return solid_angle;
        }

        scalar solid_angle_2(const Vector3 & v1, const Vector3 & v2, const Vector3 & v3)
        {
            // Using the solid angle formula by Oosterom and Strackee (note we assume vectors to be normalized to 1)
            // https://en.wikipedia.org/wiki/Solid_angle#Tetrahedron

            scalar x = v1.dot(v2.cross(v3));
            scalar y = 1 + v1.dot(v2) + v1.dot(v3) + v2.dot(v3);
            scalar solid_angle = 2 * std::atan2( x , y );

            return solid_angle;
        }

        scalar TopologicalCharge(const vectorfield & vf, const vectorfield & vf_pos, const std::vector<std::array<int, 3>> & triangulation)
        {
            // TODO: this still ignores periodical boundaries, as they are not part of the delaunay triangulation!

            scalar charge = 0, sign;
            Vector3 triangle_normal;
            for (int i = 0; i < triangulation.size(); ++i)
            {
                int i1 = triangulation[i][0];
                int i2 = triangulation[i][1];
                int i3 = triangulation[i][2];

                auto& vp1 = vf_pos[i1];
                auto& vp2 = vf_pos[i2];
                auto& vp3 = vf_pos[i3];

                // TODO: this will only work if the vf_pos are in the xy-plane!
                triangle_normal = (vp1-vp2).cross(vp1-vp3);
                triangle_normal.normalize();
                sign = triangle_normal[2]/std::abs(triangle_normal[2]);

                auto& v1 = vf[i1];
                auto& v2 = vf[i2];
                auto& v3 = vf[i3];

                // charge += sign * solid_angle_1(v1, v2, v3);
                charge += sign * solid_angle_2(v1, v2, v3);
            }
            return charge / (4*Pi);
        }

        // Utility function for the SIB Solver
        void transform(const vectorfield & spins, const vectorfield & force, vectorfield & out)
        {
            #pragma omp parallel for
            for (unsigned int i = 0; i < spins.size(); ++i)
            {
                Vector3 A = 0.5 * force[i];

                // 1/determinant(A)
                scalar detAi = 1.0 / (1 + pow(A.norm(), 2.0));

                // calculate equation without the predictor?
                Vector3 a2 = spins[i] - spins[i].cross(A);

                out[i][0] = (a2[0] * (A[0] * A[0] + 1   ) + a2[1] * (A[0] * A[1] - A[2]) + a2[2] * (A[0] * A[2] + A[1])) * detAi;
                out[i][1] = (a2[0] * (A[1] * A[0] + A[2]) + a2[1] * (A[1] * A[1] + 1   ) + a2[2] * (A[1] * A[2] - A[0])) * detAi;
                out[i][2] = (a2[0] * (A[2] * A[0] - A[1]) + a2[1] * (A[2] * A[1] + A[0]) + a2[2] * (A[2] * A[2] + 1   )) * detAi;
            }
        }

        void get_random_vector(std::uniform_real_distribution<scalar> & distribution, std::mt19937 & prng, Vector3 & vec)
        {
            for (int dim = 0; dim < 3; ++dim)
            {
                vec[dim] = distribution(prng);
            }
        }
        void get_random_vectorfield(std::mt19937 & prng, vectorfield & xi)
        {
            // PRNG gives RN [-1,1] -> multiply with epsilon
            auto distribution = std::uniform_real_distribution<scalar>(-1, 1);
            // TODO: parallelization of this is actually not quite so trivial
            #pragma omp parallel for
            for (unsigned int i = 0; i < xi.size(); ++i)
            {
                get_random_vector(distribution, prng, xi[i]);
            }
        }

        void get_random_vector_unitsphere(std::uniform_real_distribution<scalar> & distribution, std::mt19937 & prng, Vector3 & vec)
        {
            scalar v_z = distribution(prng);
            scalar phi = distribution(prng);

            scalar r_xy = std::sqrt(1 - v_z*v_z);

            vec[0] = r_xy * std::cos(2*Pi*phi);
            vec[1] = r_xy * std::sin(2*Pi*phi);
            vec[2] = v_z;
        }
        void get_random_vectorfield_unitsphere(std::mt19937 & prng, vectorfield & xi)
        {
            // PRNG gives RN [-1,1] -> multiply with epsilon
            auto distribution = std::uniform_real_distribution<scalar>(-1, 1);
            // TODO: parallelization of this is actually not quite so trivial
            #pragma omp parallel for
            for (unsigned int i = 0; i < xi.size(); ++i)
            {
                get_random_vector_unitsphere(distribution, prng, xi[i]);
            }
        }

        void get_gradient_distribution(const Data::Geometry & geometry, Vector3 gradient_direction, scalar gradient_start, scalar gradient_inclination, scalarfield & distribution, scalar range_min, scalar range_max)
        {
            // Ensure a normalized direction vector
            gradient_direction.normalize();

            // Basic linear gradient distribution
            set_c_dot(gradient_inclination, gradient_direction, geometry.positions, distribution);

            // Get the minimum (i.e. starting point) of the distribution
            scalar bmin = geometry.bounds_min.dot(gradient_direction);
            scalar bmax = geometry.bounds_max.dot(gradient_direction);
            scalar dist_min = std::min(bmin, bmax);
            // Set the starting point
            add(distribution, gradient_start - gradient_inclination*dist_min);

            // Cut off negative values
            set_range(distribution, range_min, range_max);
        }

        void directional_gradient(const vectorfield & vf, const Data::Geometry & geometry, const intfield & boundary_conditions, const Vector3 & direction, vectorfield & gradient)
        {
            // std::cout << "start gradient" << std::endl;
            vectorfield translations = { { 0,0,0 }, { 0,0,0 }, { 0,0,0 } };
            auto& n_cells = geometry.n_cells;

            Vector3 a = geometry.bravais_vectors[0]; // translation vectors of the system
            Vector3 b = geometry.bravais_vectors[1];
            Vector3 c = geometry.bravais_vectors[2];

            neighbourfield neigh;

            // TODO: calculate Neighbours outside iterations
            // Neighbours::get_Neighbours(geometry, neigh);

            // TODO: proper usage of neighbours
            // Hardcoded neighbours - for spin current in a rectangular lattice
            neigh = neighbourfield(0);
            Neighbour neigh_tmp;
            neigh_tmp.i = 0;
            neigh_tmp.j = 0;
            neigh_tmp.idx_shell = 0;

            neigh_tmp.translations[0] = 1;
            neigh_tmp.translations[1] = 0;
            neigh_tmp.translations[2] = 0;
            neigh.push_back(neigh_tmp);

            neigh_tmp.translations[0] = -1;
            neigh_tmp.translations[1] = 0;
            neigh_tmp.translations[2] = 0;
            neigh.push_back(neigh_tmp);

            neigh_tmp.translations[0] = 0;
            neigh_tmp.translations[1] = 1;
            neigh_tmp.translations[2] = 0;
            neigh.push_back(neigh_tmp);

            neigh_tmp.translations[0] = 0;
            neigh_tmp.translations[1] = -1;
            neigh_tmp.translations[2] = 0;
            neigh.push_back(neigh_tmp);

            neigh_tmp.translations[0] = 0;
            neigh_tmp.translations[1] = 0;
            neigh_tmp.translations[2] = 1;
            neigh.push_back(neigh_tmp);

            neigh_tmp.translations[0] = 0;
            neigh_tmp.translations[1] = 0;
            neigh_tmp.translations[2] = -1;
            neigh.push_back(neigh_tmp);

            // Loop over vectorfield
            for(unsigned int ispin = 0; ispin < vf.size(); ++ispin)
            {
                auto translations_i = translations_from_idx(n_cells, geometry.n_cell_atoms, ispin); // transVec of spin i
                // int k = i%geometry.n_cell_atoms; // index within unit cell - k=0 for all cases used in the thesis
                scalar n = 0;

                gradient[ispin].setZero();

                std::vector<Vector3> euclidean { {1,0,0}, {0,1,0}, {0,0,1} };
                std::vector<Vector3> contrib = { {0, 0, 0}, {0, 0, 0}, {0, 0, 0} };
                Vector3 proj = {0, 0, 0};
                Vector3 projection_inv = {0, 0, 0};

                // TODO: both loops together.

                // Loop over neighbours of this vector to calculate contributions of finite differences to current direction
                for(unsigned int j = 0; j < neigh.size(); ++j)
                {
                    if ( boundary_conditions_fulfilled(geometry.n_cells, boundary_conditions, translations_i, neigh[j].translations) )
                    {
                        // Index of neighbour
                        int ineigh = idx_from_translations(n_cells, geometry.n_cell_atoms, translations_i, neigh[j].translations);
                        if (ineigh >= 0)
                        {
                            auto d = geometry.positions[ineigh] - geometry.positions[ispin];
                            for (int dim=0; dim<3; ++dim)
                            {
                                proj[dim] += std::abs(euclidean[dim].dot(d.normalized()));
                            }
                        }
                    }
                }
                for (int dim=0; dim<3; ++dim)
                {
                    if (std::abs(proj[dim]) > 1e-10)
                        projection_inv[dim] = 1.0/proj[dim];
                }
                // Loop over neighbours of this vector to calculate finite differences
                for(unsigned int j = 0; j < neigh.size(); ++j)
                {
                    if ( boundary_conditions_fulfilled(geometry.n_cells, boundary_conditions, translations_i, neigh[j].translations) )
                    {
                        // Index of neighbour
                        int ineigh = idx_from_translations(n_cells, geometry.n_cell_atoms, translations_i, neigh[j].translations);
                        if (ineigh >= 0)
                        {
                            auto d = geometry.positions[ineigh] - geometry.positions[ispin];
                            for (int dim=0; dim<3; ++dim)
                            {
                                contrib[dim] += euclidean[dim].dot(d) / d.dot(d) * ( vf[ineigh] - vf[ispin] );
                            }
                        }
                    }
                }

                for (int dim=0; dim<3; ++dim)
                {
                    gradient[ispin] += direction[dim]*projection_inv[dim] * contrib[dim];
                }
            }
        }

        /////////////////////////////////////////////////////////////////

        void fill(scalarfield & sf, scalar s)
        {
            #pragma omp parallel for
            for (unsigned int i = 0; i<sf.size(); ++i)
                sf[i] = s;
        }
        void fill(scalarfield & sf, scalar s, const intfield & mask)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<sf.size(); ++i)
                sf[i] = mask[i]*s;
        }

        void scale(scalarfield & sf, scalar s)
        {
            #pragma omp parallel for
            for (unsigned int i = 0; i<sf.size(); ++i)
                sf[i] *= s;
        }

        void add(scalarfield & sf, scalar s)
        {
            #pragma omp parallel for
            for (unsigned int i = 0; i<sf.size(); ++i)
                sf[i] += s;
        }

        scalar sum(const scalarfield & sf)
        {
            scalar ret = 0;
            #pragma omp parallel for reduction(+:ret)
            for (unsigned int i = 0; i<sf.size(); ++i)
                ret += sf[i];
            return ret;
        }

        scalar mean(const scalarfield & sf)
        {
            scalar ret = sum(sf)/sf.size();
            return ret;
        }

        void set_range(scalarfield & sf, scalar sf_min, scalar sf_max)
        {
            #pragma omp parallel for
            for (unsigned int i = 0; i<sf.size(); ++i)
                sf[i] = std::min( std::max( sf_min, sf[i] ), sf_max );
        }

        void fill(vectorfield & vf, const Vector3 & v)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<vf.size(); ++i)
                vf[i] = v;
        }
        void fill(vectorfield & vf, const Vector3 & v, const intfield & mask)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<vf.size(); ++i)
                vf[i] = mask[i]*v;
        }

        void normalize_vectors(vectorfield & vf)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<vf.size(); ++i)
                vf[i].normalize();
        }

        void norm( const vectorfield & vf, scalarfield & norm )
        {
            for (unsigned int i=0; i<vf.size(); ++i)
                norm[i] = vf[i].norm();
        }

        std::pair<scalar, scalar> minmax_component(const vectorfield & v1)
        {
            scalar minval=1e6, maxval=-1e6;
            std::pair<scalar, scalar> minmax;
            #pragma omp parallel for reduction(min: minval) reduction(max : maxval)
            for (unsigned int i = 0; i < v1.size(); ++i)
            {
                for (int dim = 0; dim < 3; ++dim)
                {
                    if (v1[i][dim] < minval) minval = v1[i][dim];
                    if (v1[i][dim] > maxval) maxval = v1[i][dim];
                }
            }
            minmax.first = minval;
            minmax.second = maxval;
            return minmax;
        }
        scalar  max_abs_component(const vectorfield & vf)
        {
            // We want the Maximum of Absolute Values of all force components on all images
            scalar absmax = 0;
            // Find minimum and maximum values
            std::pair<scalar,scalar> minmax = minmax_component(vf);
            // Mamimum of absolute values
            absmax = std::max(absmax, std::abs(minmax.first));
            absmax = std::max(absmax, std::abs(minmax.second));
            // Return
            return absmax;
        }

        void scale(vectorfield & vf, const scalar & sc)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<vf.size(); ++i)
                vf[i] *= sc;
        }

        Vector3 sum(const vectorfield & vf)
        {
            Vector3 ret = { 0,0,0 };
            #pragma omp parallel for reduction(+:ret)
            for (unsigned int i = 0; i<vf.size(); ++i)
                ret += vf[i];
            return ret;
        }

        Vector3 mean(const vectorfield & vf)
        {
            Vector3 ret = sum(vf)/vf.size();
            return ret;
        }

        void divide( const scalarfield & numerator, const scalarfield & denominator, scalarfield & out )
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<out.size(); ++i)
                out[i] = numerator[i] / denominator[i];
        }

        // computes the inner product of two vectorfields v1 and v2
        scalar dot(const vectorfield & v1, const vectorfield & v2)
        {
            scalar ret = 0;
            #pragma omp parallel for reduction(+:ret)
            for (unsigned int i = 0; i<v1.size(); ++i)
                ret += v1[i].dot(v2[i]);
            return ret;
        }

        // computes the inner products of vectors in vf1 and vf2
        // vf1 and vf2 are vectorfields
        void dot(const vectorfield & vf1, const vectorfield & vf2, scalarfield & out)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                out[i] = vf1[i].dot(vf2[i]);
        }

        // computes the product of scalars in s1 and s2
        // s1 and s2 are scalarfields
        void dot( const scalarfield & s1, const scalarfield & s2, scalarfield & out )
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<s1.size(); i++)
                out[i] = s1[i] * s2[i];
        }

        // computes the vector (cross) products of vectors in v1 and v2
        // v1 and v2 are vector fields
        void cross(const vectorfield & v1, const vectorfield & v2, vectorfield & out)
        {
            #pragma omp parallel for
            for (unsigned int i=0; i<v1.size(); ++i)
                out[i] = v1[i].cross(v2[i]);
        }

        // out[i] += c*a
        void add_c_a(const scalar & c, const Vector3 & vec, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c*vec;
        }
        // out[i] += c*a[i]
        void add_c_a(const scalar & c, const vectorfield & vf, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c*vf[idx];
        }
        void add_c_a(const scalar & c, const vectorfield & vf, vectorfield & out, const intfield & mask)
        {
            #pragma omp parallel for
            for (unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += mask[idx] * c*vf[idx];
        }
        // out[i] += c[i]*a[i]
        void add_c_a( const scalarfield & c, const vectorfield & vf, vectorfield & out )
        {
            #pragma omp parallel for
            for( unsigned int idx = 0; idx < out.size(); ++idx )
                out[idx] += c[idx] * vf[idx];
        }

        // out[i] = c*a
        void set_c_a(const scalar & c, const Vector3 & vec, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = c*vec;
        }
        // out[i] = c*a
        void set_c_a(const scalar & c, const Vector3 & vec, vectorfield & out, const intfield & mask)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = mask[idx]*c*vec;
        }

        // out[i] = c*a[i]
        void set_c_a(const scalar & c, const vectorfield & vf, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = c*vf[idx];
        }
        // out[i] = c*a[i]
        void set_c_a(const scalar & c, const vectorfield & vf, vectorfield & out, const intfield & mask)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = mask[idx] * c*vf[idx];
        }
        // out[i] = c[i]*a[i]
        void set_c_a( const scalarfield & c, const vectorfield & vf, vectorfield & out )
        {
            #pragma omp parallel for
            for( unsigned int idx=0; idx < out.size(); ++idx)
                out[idx] = c[idx] * vf[idx];
        }

        // out[i] += c * a*b[i]
        void add_c_dot(const scalar & c, const Vector3 & vec, const vectorfield & vf, scalarfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c*vec.dot(vf[idx]);
        }
        // out[i] += c * a[i]*b[i]
        void add_c_dot(const scalar & c, const vectorfield & vf1, const vectorfield & vf2, scalarfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c*vf1[idx].dot(vf2[idx]);
        }

        // out[i] = c * a*b[i]
        void set_c_dot(const scalar & c, const Vector3 & a, const vectorfield & b, scalarfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = c*a.dot(b[idx]);
        }
        // out[i] = c * a[i]*b[i]
        void set_c_dot(const scalar & c, const vectorfield & a, const vectorfield & b, scalarfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = c*a[idx].dot(b[idx]);
        }

        // out[i] += c * a x b[i]
        void add_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c*a.cross(b[idx]);
        }
        // out[i] += c * a[i] x b[i]
        void add_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c*a[idx].cross(b[idx]);
        }
        // out[i] += c[i] * a[i] x b[i]
        void add_c_cross(const scalarfield & c, const vectorfield & a, const vectorfield & b, vectorfield & out)
        {
            #pragma omp parallel for
            for (unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] += c[idx] * a[idx].cross(b[idx]);
        }

        // out[i] = c * a x b[i]
        void set_c_cross(const scalar & c, const Vector3 & a, const vectorfield & b, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = c*a.cross(b[idx]);
        }
        // out[i] = c * a[i] x b[i]
        void set_c_cross(const scalar & c, const vectorfield & a, const vectorfield & b, vectorfield & out)
        {
            #pragma omp parallel for
            for(unsigned int idx = 0; idx < out.size(); ++idx)
                out[idx] = c*a[idx].cross(b[idx]);
        }
    }
}

#endif