/******************************************************************************

  This source file is part of the Avogadro project.

  Copyright 2008-2009 Marcus D. Hanwell
  Copyright 2010-2013 Kitware, Inc.

  This source code is released under the New BSD License, (the "License").

  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.

******************************************************************************/

#include "slaterset.h"

#include <cmath>
#include <iostream>

#include <Eigen/LU>

using std::vector;
using std::cout;
using std::endl;
using Eigen::SelfAdjointEigenSolver;

namespace Avogadro {
namespace Core {

SlaterSet::SlaterSet() : m_initialized(false)
{
}

SlaterSet::~SlaterSet()
{
}

bool SlaterSet::addSlaterIndices(const std::vector<int>& i)
{
  m_slaterIndices = i;
  return true;
}

bool SlaterSet::addSlaterTypes(const std::vector<int>& t)
{
  m_initialized = false;
  m_slaterTypes = t;
  return true;
}

bool SlaterSet::addZetas(const std::vector<double>& z)
{
  m_initialized = false;
  m_zetas = z;
  return true;
}

bool SlaterSet::addPQNs(const std::vector<int>& pqns)
{
  m_initialized = false;
  m_pqns = pqns;
  return true;
}

bool SlaterSet::addOverlapMatrix(const Eigen::MatrixXd& m)
{
  m_initialized = false;
  m_overlap.resize(m.rows(), m.cols());
  m_overlap = m;
  return true;
}

bool SlaterSet::addEigenVectors(const Eigen::MatrixXd& e)
{
  m_eigenVectors.resize(e.rows(), e.cols());
  m_eigenVectors = e;
  return true;
}

bool SlaterSet::addDensityMatrix(const Eigen::MatrixXd& d)
{
  m_density.resize(d.rows(), d.cols());
  m_density = d;
  return true;
}

unsigned int SlaterSet::molecularOrbitalCount(ElectronType)
{
  return static_cast<unsigned int>(m_overlap.cols());
}

void SlaterSet::outputAll()
{
}

void SlaterSet::initCalculation()
{
  if (m_initialized)
    return;

  m_normalized.resize(m_overlap.cols(), m_overlap.rows());

  SelfAdjointEigenSolver<MatrixX> s(m_overlap);
  MatrixX p = s.eigenvectors();
  MatrixX m =
    p * s.eigenvalues().array().inverse().array().sqrt().matrix().asDiagonal() *
    p.inverse();
  m_normalized = m * m_eigenVectors;

  if (!(m_overlap * m * m).eval().isIdentity())
    cout << "Identity test FAILED - do you need a newer version of Eigen?\n";

  m_factors.resize(m_zetas.size());
  m_PQNs = m_pqns;
  // Calculate the normalizations of the orbitals.
  for (size_t i = 0; i < m_zetas.size(); ++i) {
    switch (m_slaterTypes[i]) {
      case S:
        m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
                       sqrt(1.0 / (4.0 * M_PI) / factorial(2 * m_pqns[i]));
        m_PQNs[i] -= 1;
        break;
      case PX:
      case PY:
      case PZ:
        m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
                       sqrt(3.0 / (4.0 * M_PI) / factorial(2 * m_pqns[i]));
        m_PQNs[i] -= 2;
        break;
      case X2:
        m_factors[i] = 0.5 * pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
                       sqrt(15.0 / (4.0 * M_PI) / factorial(2 * m_pqns[i]));
        m_PQNs[i] -= 3;
        break;
      case XZ:
        m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
                       sqrt(15.0 / (4.0 * M_PI) / factorial(2 * m_pqns[i]));
        m_PQNs[i] -= 3;
        break;
      case Z2:
        m_factors[i] = (0.5 / sqrt(3.0)) *
                       pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
                       sqrt(15.0 / (4.0 * M_PI) / factorial(2 * m_pqns[i]));
        m_PQNs[i] -= 3;
        break;
      case YZ:
      case XY:
        m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
                       sqrt(15.0 / (4.0 * M_PI) / factorial(2 * m_pqns[i]));
        m_PQNs[i] -= 3;
        break;
      default:
        cout << "Orbital " << i << " not handled, type " << m_slaterTypes[i]
             << endl;
    }
  }
  // Convert the exponents into Angstroms
  for (size_t i = 0; i < m_zetas.size(); ++i)
    m_zetas[i] = m_zetas[i] / BOHR_TO_ANGSTROM_D;

  m_initialized = true;
}

inline unsigned int SlaterSet::factorial(unsigned int n)
{
  if (n <= 1)
    return n;
  return (n * factorial(n - 1));
}

} // End namespace Core
} // End namespace Avogadro


////////////////////////////////////////////////////////////////////////////////
// TensionFieldTheory.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Implements a field theory relaxed energy density for a fully generic,
//  potentially anisotropic, 2D "C-based" energy density. Here "C-based" means
//  the energy density is expressed in terms of the Cauchy-Green deformation
//  tensor.
//  Our implementation is based on the applied math paper
//  [Pipkin1994:"Relaxed energy densities for large deformations of membranes"]
//  whose optimality criterion for the wrinkling strain we use to obtain a
//  slightly less expensive optimization formulation.
//
//  We implement the second derivatives needed for a Newton-based equilibrium
//  solver; these are nontrivial since they must account for the dependence of
//  the wrinkling strain on C.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  06/28/2020 19:01:21
////////////////////////////////////////////////////////////////////////////////
#ifndef TENSIONFIELDTHEORY_HH
#define TENSIONFIELDTHEORY_HH
#include <Eigen/Dense>
#include <MeshFEM/Types.hh>
#include <stdexcept>
#include <iostream>
#include <MeshFEM/newton_optimizer/dense_newton.hh>

template<class _Psi>
struct IsotropicWrinkleStrainProblem {
    using Psi      = _Psi;
    using Real     = typename Psi::Real;
    using VarType  = Eigen::Matrix<Real, 1, 1>;
    using HessType = Eigen::Matrix<Real, 1, 1>;
    using M2d      = Mat2_T<Real>;

    IsotropicWrinkleStrainProblem(Psi &psi, const M2d &C, const Vec2_T<Real> &n)
        : m_psi(psi), m_C(C), m_nn(n * n.transpose()) { }

    void setC(const M2d &C) { m_C = C; }
    const M2d &getC() const { return m_C; }

    size_t numVars() const { return 1; }
    void setVars(const VarType &vars) {
        m_a = vars;
        m_psi.setC(m_C + m_a[0] * m_nn);
    }
    const VarType &getVars() const { return m_a; }

    Real energy()      const { return m_psi.energy(); }
    VarType gradient() const { return  VarType(0.5 * doubleContract(m_nn, m_psi.PK2Stress())          ); }
    HessType hessian() const { return HessType(0.5 * doubleContract(m_psi.delta_PK2Stress(m_nn), m_nn)); }

    void solve() { dense_newton(*this, /* maxIter = */ 100, /*gradTol = */1e-14, /* verbose = */ false); }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    VarType m_a = VarType::Zero();
    Psi &m_psi;
    M2d m_C, m_nn;
};

template<class _Psi>
struct AnisotropicWrinkleStrainProblem {
    using Psi      = _Psi;
    using Real     = typename Psi::Real;
    using VarType  = Vec2_T<Real>;
    using HessType = Mat2_T<Real>;
    using M2d      = Mat2_T<Real>;

    AnisotropicWrinkleStrainProblem(Psi &psi, const M2d &C, const VarType &n)
        : m_psi(psi), m_C(C), m_ntilde(n) { }

    void setC(const M2d &C) { m_C = C; }
    const M2d &getC() const { return m_C; }

    size_t numVars() const { return 2; }
    void setVars(const VarType &vars) {
        m_ntilde = vars;
        m_psi.setC(m_C + m_ntilde * m_ntilde.transpose());
    }
    const VarType &getVars() const { return m_ntilde; }
    Real energy() const { return m_psi.energy(); }

    // S : (0.5 * (n delta_n^T + delta_n n^T))
    //  = S : n delta_n^T = (S n) . delta_n
    VarType gradient() const { return m_psi.PK2Stress() * m_ntilde; }

    //   psi(n * n^T)
    //  dpsi = n^T psi'(n * n^T) . dn
    // d2psi = dn_a^T psi'(n * n^T) . dn_b + n^T (psi'' : (n * dn_a^T)) . dn_b
    //       = psi' : (dn_a dn_b^T) + ...
    HessType hessian() const {
        HessType h = m_psi.PK2Stress(); // psi' : (dn_a dn_b^T)
        M2d dnn(M2d::Zero());
        dnn.col(0) = m_ntilde;
        h.row(0) += m_ntilde.transpose() * m_psi.delta_PK2Stress(symmetrized_x2(dnn));
        dnn.col(0).setZero();
        dnn.col(1) = m_ntilde;
        h.row(1) += m_ntilde.transpose() * m_psi.delta_PK2Stress(symmetrized_x2(dnn));
        if (h.array().isNaN().any()) throw std::runtime_error("NaN Hessian");
        if (std::abs(h(0, 1) - h(1, 0)) > 1e-10 * std::abs(h(1, 0)) + 1e-10)
            throw std::runtime_error("Asymmetric Hessian");
        return h;
    }

    void solve() { dense_newton(*this, /* maxIter = */ 100, /*gradTol = */1e-14, /* verbose = */ false); }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    Psi &m_psi;
    M2d m_C;
    VarType m_ntilde = VarType::Zero();
};

// Define a relaxed 2D C-based energy based on a given 2D C-based energy `Psi_C`
template<class Psi_C>
struct RelaxedEnergyDensity {
    static_assert(Psi_C::EDType == EDensityType::CBased,
                  "Tension field theory only works on C-based energy densities");
    static constexpr size_t N = Psi_C::N;
    static constexpr size_t Dimension = N;
    static constexpr EDensityType EDType = EDensityType::CBased;

    using Matrix = typename Psi_C::Matrix;
    using Real   = typename Psi_C::Real;
    using ES     = Eigen::SelfAdjointEigenSolver<Matrix>;
    using V2d    = Vec2_T<Real>;
    static_assert(N == 2, "Tension field theory relaxation only defined for 2D energies");

    static std::string name() {
        return std::string("Relaxed") + Psi_C::name();
    }

    // Forward all constructor arguments to m_psi
    template<class... Args>
    RelaxedEnergyDensity(Args&&... args)
         : m_psi(std::forward<Args>(args)...),
           m_anisoProb(m_psi, Matrix::Identity(), V2d::Zero()) { }

    // We need a custom copy constructor since m_anisoProb contains a
    // reference to this->m_psi
    RelaxedEnergyDensity(const RelaxedEnergyDensity &b)
        : m_psi(b.m_psi),
          m_anisoProb(m_psi, Matrix::Identity(), V2d::Zero()) {
        setC(b.m_C);
    }

    // Note: UninitializedDeformationTag argument must be a rvalue reference so it exactly
    // matches the type passed by the constructor call
    // RelaxedEnergyDensity(b, UninitializedDeformationTag()); otherwise the
    // perfect forwarding constructor above will be preferred for this call,
    // incorrectly forwarding b to Psi's constructor.
    RelaxedEnergyDensity(const RelaxedEnergyDensity &b, UninitializedDeformationTag &&)
        : m_psi(b.m_psi, UninitializedDeformationTag()),
          m_anisoProb(m_psi, Matrix::Identity(), V2d::Zero()) {
        setC(b.m_C);
    }

    void setC(const Matrix &C) {
        m_C = C;
        if (!relaxationEnabled()) {
            m_psi.setC(C);
            return;
        }

        // Note: Eigen guarantees eigenvalues are sorted in ascending order.
        ES C_eigs(C);
        // std::cout << "C eigenvalues: " << C_eigs.eigenvalues().transpose() << std::endl;
        // Detect full compression
        if (C_eigs.eigenvalues()[1] < 1) {
            m_tensionState = 0;
            m_wrinkleStrain = -C;
            return;
        }

        m_psi.setC(C);
        ES S_eigs(m_psi.PK2Stress());
        // Detect full tension
        // std::cout << "S eigenvalues: " << S_eigs.eigenvalues().transpose() << std::endl;
        if (S_eigs.eigenvalues()[0] >= -1e-12) {
            m_tensionState = 2;
            m_wrinkleStrain.setZero();
            return;
        }

        // Handle partial tension
        m_tensionState = 1;
        // In the isotropic case, principal stress and strain directions
        // coincide, and the wrinkling strain must be in the form
        // a n n^T, where n is the eigenvector corresponding to the smallest
        // stress eigenvalue and a > 0 is an unknown.
        // This simplifies the determination of wrinkling strain to a convex
        // 1D optimization problem that we solve with Newton's method.
        V2d n = S_eigs.eigenvectors().col(0);
        using IWSP = IsotropicWrinkleStrainProblem<Psi_C>;
        IWSP isoProb(m_psi, C, n);
        // std::cout << "Solving isotropic wrinkle strain problem" << std::endl;
        isoProb.setVars(typename IWSP::VarType{0.0});
        isoProb.solve();
        Real a = isoProb.getVars()[0];
        if (a < 0) throw std::runtime_error("Invalid wrinkle strain");

        // We use this isotropic assumption to obtain initial guess for the
        // anisotropic case, where the wrinkling strain is in the form
        //      n_tilde n_tilde^T
        // with a 2D vector n_tilde as the unknown.
        m_anisoProb.setC(C);
        // std::cout << "Solving anisotropic wrinkle strain problem" << std::endl;
        m_anisoProb.setVars(std::sqrt(a) * n);
        m_anisoProb.solve();
        auto ntilde = m_anisoProb.getVars();
        m_wrinkleStrain = -ntilde * ntilde.transpose();

        // {
        //     ES S_eigs_new(m_psi.PK2Stress());
        //     std::cout << "new S eigenvalues: " << S_eigs_new.eigenvalues().transpose() << std::endl;
        // }
    }

    Real energy() const {
        if (relaxationEnabled() && fullCompression()) return 0.0;
        return m_psi.energy();
    }

    Matrix PK2Stress() const {
        if (relaxationEnabled() && fullCompression()) return Matrix::Zero();
        // By envelope theorem, the wrinkling strain's perturbation can be
        // neglected, and the stress is simply the stress of the underlying
        // material model evaluated on the "elastic strain".
        return m_psi.PK2Stress();
    }

    template<class Mat_>
    Matrix delta_PK2Stress(const Mat_ &dC) const {
        if (!relaxationEnabled() || fullTension()) return m_psi.delta_PK2Stress(dC);
        if (fullCompression()) return Matrix::Zero();

        // n solves m_anisoProb:
        //     (psi') n = 0
        // delta_n solves:
        //     (psi'' : dC + n delta n) n + (psi') delta_n = 0
        //     (psi'' : n delta_n) n + (psi') delta_n = -(psi'' : dC) n
        //     H delta_n = -(psi'' : dC) n
        auto ntilde = m_anisoProb.getVars();
        V2d delta_n = -m_anisoProb.hessian().inverse() * (m_psi.delta_PK2Stress(dC.matrix()) * ntilde);

        // In the partial tension case, we need to account for the wrinkling
        // strain perturbation.
        return m_psi.delta_PK2Stress(dC.matrix() + ntilde * delta_n.transpose()
                                                 + delta_n * ntilde.transpose());
    }

    template<class Mat_, class Mat2_>
    Matrix delta2_PK2Stress(const Mat_ &/* dF_a */, const Mat2_ &/* dF_b */) const {
        throw std::runtime_error("Unimplemented");
    }

    V2d principalBiotStrains() const {
        ES es(m_C);
        return es.eigenvalues().array().sqrt() - 1.0;
    }

    const Matrix &wrinkleStrain() const { return m_wrinkleStrain; }

    bool fullCompression() const { return m_tensionState == 0; }
    bool partialTension()  const { return m_tensionState == 1; }
    bool fullTension()     const { return m_tensionState == 2; }
    int tensionState()     const { return m_tensionState; }

    // Copying is super dangerous since we m_anisoProb uses a reference to m_psi...
    RelaxedEnergyDensity &operator=(const RelaxedEnergyDensity &) = delete;

    // Direct access to the energy density for debugging or
    // to change the material properties
    Psi_C &psi() { return m_psi; }
    const Psi_C &psi() const { return m_psi; }

    // Turn on or off the tension field theory approximation
    void setRelaxationEnabled(bool enable) {
        m_relaxationEnabled = enable;
        setC(m_C);
    }

    bool relaxationEnabled() const { return m_relaxationEnabled; }

    void copyMaterialProperties(const RelaxedEnergyDensity &other) { psi().copyMaterialProperties(other.psi()); }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    // Tension state:
    //      0: compression in all directions
    //      1: partial tension
    //      2: tension in all directions
    int m_tensionState = 2;
    Psi_C m_psi;
    Matrix m_wrinkleStrain = Matrix::Zero(),
           m_C = Matrix::Identity(); // full Cauchy-Green deformation tensor.
    AnisotropicWrinkleStrainProblem<Psi_C> m_anisoProb;
    bool m_relaxationEnabled = true;
};

#endif /* end of include guard: TENSIONFIELDTHEORY_HH */


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */
#include <malloc.h>
#include "Utils.h"
#include "TraceStoreCol.h"
#include "IonH5Eigen.h"
//#define EIGEN_USE_MKL_ALL 1
#include <Eigen/Dense>
#include <Eigen/LU>

#define MIN_SAMPLE_WELL 100
#define SMOOTH_REDUCE_STEP 10
#define SMOOTH_REDUCE_REGION 100
#define INTEGRATION_START 6
#define INTEGRATION_END 12
void TraceStoreCol::WellProj(TraceStoreCol &store,
                             std::vector<KeySeq> & key_vectors,
                             vector<char> &filter,
                             vector<float> &mad) {
  int useable_flows = 0;
  for (size_t i = 0; i < key_vectors.size(); i++) {
    useable_flows = std::max((int)key_vectors[i].usableKeyFlows, useable_flows);
    //    useable_flows = std::max(store.GetNumFlows(), (size_t)useable_flows);
  }
  Eigen::VectorXf norm(store.mFrameStride * useable_flows);
  Eigen::VectorXf sum(store.mFrameStride * useable_flows);

  norm.setZero();
  sum.setZero();
  int start_frame = store.GetNumFrames() - 2;
  int end_frame = store.GetNumFrames();
  int count = 0;
  for (int frame_ix = start_frame; frame_ix < end_frame; frame_ix++) {
    count++;
    int16_t *__restrict data_start = store.GetMemPtr() + frame_ix * store.mFlowFrameStride;
    int16_t *__restrict data_end = data_start + store.mFrameStride * useable_flows;
    float *__restrict norm_start = &norm[0];
    while(data_start != data_end) {
      *norm_start++ += *data_start++;
    }
  }

  // std::vector<ChipReduction> smoothed_avg(useable_flows);
  // int x_clip = mCols;
  // int y_clip = mRows;
  // if (mUseMeshNeighbors == 0) {
  //   x_clip = THUMBNAIL_SIZE;
  //   y_clip = THUMBNAIL_SIZE;
  // }
  // int last_frame = store.GetNumFrames() - 1;
  // for (int flow_ix = 0; flow_ix < useable_flows; flow_ix++) {
  //   smoothed_avg[flow_ix].Init(mRows, mCols, 1,
  //                              SMOOTH_REDUCE_STEP, SMOOTH_REDUCE_STEP,
  //                              y_clip, x_clip,
  //                              SMOOTH_REDUCE_STEP * SMOOTH_REDUCE_STEP * .4);
  //   smoothed_avg[flow_ix].ReduceFrame(&mData[0] + flow_ix * mFrameStride + last_frame  * mFlowFrameStride, &filter[0], 0);
  //   smoothed_avg[flow_ix].SmoothBlocks(SMOOTH_REDUCE_REGION, SMOOTH_REDUCE_REGION);
  // }
  // int x_count = 0;
  // for (int flow_ix = 0; flow_ix < useable_flows; flow_ix++) {
  //   for (size_t row_ix = 0; row_ix < mRows; row_ix++) {
  //     for (size_t col_ix = 0; col_ix < mCols; col_ix++) {
  //       float avg = smoothed_avg[flow_ix].GetSmoothEst(row_ix, col_ix, 0);
  //       norm[x_count] = avg;
  //       x_count++;
  //     }
  //   }
  // }

  norm = norm / count;
  //  start_frame = INTEGRATION_START;
  //  end_frame = INTEGRATION_END;
  start_frame = 5;
  end_frame = store.GetNumFrames() - 6;

  for (int frame_ix = start_frame; frame_ix < end_frame; frame_ix++) {
    int16_t *__restrict data_start = store.GetMemPtr() + frame_ix * store.mFlowFrameStride;
    int16_t *__restrict data_end = data_start + store.mFrameStride * useable_flows;
    float *__restrict norm_start = &norm[0];
    float *__restrict sum_start = &sum[0];
    int well_offset = 0;
    while(data_start != data_end) {
      if (*norm_start == 0.0f) {
        *norm_start = 1.0f;
        // avoid divide by zero but mark that something is wrong with this well..
        if (filter[well_offset] == 0) {
          filter[well_offset] = 6;
        }
      }
      *sum_start += *data_start / *norm_start;
      well_offset++;
      // reset each flow
      if (well_offset == (int) store.mFrameStride) {
        well_offset = 0;
      }
      sum_start++;
      data_start++;
      norm_start++;
    }
  }

  Eigen::MatrixXf flow_mat(store.mFrameStride, useable_flows);
  for (int flow_ix = 0; flow_ix < flow_mat.cols(); flow_ix++) {
    for (size_t well_ix = 0; well_ix < store.mFrameStride; well_ix++) {
      flow_mat(well_ix, flow_ix) = sum(flow_ix * store.mFrameStride + well_ix);
    }
  }

  Eigen::VectorXf n2;
  n2 = flow_mat.rowwise().squaredNorm();
  n2 = n2.array().sqrt();
  float *n2_start = n2.data();
  float *n2_end = n2_start + n2.rows();
  while (n2_start != n2_end) {
    if (*n2_start == 0.0f) { *n2_start = 1.0f; }
    n2_start++;
  }
  for (int flow_ix = 0; flow_ix < flow_mat.cols(); flow_ix++) {
    flow_mat.col(flow_ix).array() = flow_mat.col(flow_ix).array() / n2.array();
  }

  Eigen::VectorXf proj;
  Eigen::VectorXf max_abs_proj(flow_mat.rows());
  max_abs_proj.setZero();
  for (size_t key_ix = 0; key_ix < key_vectors.size(); key_ix++) {
    Eigen::VectorXf key(useable_flows);
    key.setZero();
    for (int f_ix = 0; f_ix < useable_flows; f_ix++) {
      key[f_ix] = key_vectors[key_ix].flows[f_ix];
    }
    proj = flow_mat * key;
    proj = proj.array().abs();
    for (int i = 0; i < proj.rows(); i++) {
      max_abs_proj(i) = max(max_abs_proj(i), proj(i));
    }
  }

  for (int i = 0; i < max_abs_proj.rows(); i++) {
    mad[i] = max_abs_proj(i);
  }

}

void TraceStoreCol::Init(Mask &mask, size_t frames, const char *flowOrder,
       int numFlowsBuff, int maxFlow, int rowStep, int colStep) {

    pthread_mutex_init (&mLock, NULL);
    mUseMeshNeighbors = 1;
    mRowRefStep = rowStep;
    mColRefStep = colStep;
    mMinRefProbes = floor (mRowRefStep * mColRefStep * .1);
    mRows = mask.H();
    mCols = mask.W();
    mFrames = frames;
    mFrameStride = mRows * mCols;
    mFlows = mFlowsBuf = maxFlow;
    mFlowFrameStride = mFrameStride * maxFlow;
    mMaxDist = 2 * sqrt(rowStep*rowStep + colStep+colStep);
    mFlowOrder = flowOrder;

    mWells = mRows * mCols;
    mUseAsReference.resize (mWells, false);
    int keep = 0;
    int empties = 0;
    mRefGridsValid.resize (mFlowsBuf, 0);
    mRefGrids.resize (mFlowsBuf);
    mData.resize (mWells * mFrames * mFlowsBuf);
    std::fill (mData.begin(), mData.end(), 0);
  }

/**
 * Create the basis splines for order requested at a particular value of x.
 * From "A Practical Guide To Splines - Revised Edition" by Carl de Boor p 111
 * the algorithm for BSPLVB. The value of the spline coefficients can be calculated
 * based on a cool recursion. This is a simplified and numerically stable algorithm
 * that seems to be the basis of most bspline implementations including gsl, matlab,etc.
 * @param all_knots - Original knots vector augmented with order number of endpoints
 * @param n_knots - Total number of knots.
 * @param order - Order or order of polynomials, 4 for cubic splines.
 * @param i - index of value x in knots such that all_knots[i] <= x && all_knots[i+1] > x
 * @param x - value that we want to evaluate spline at.
 * @param b - memory to write our coefficients into, must be at least order long.
 */
void basis_spline_xi_v2(float *all_knots, int n_knots, int order, int i, float x, float *b) {
  b[0] = 1;
  double kr[order];
  double kl[order];
  double term;
  double saved;
  for (int j = 0; j < order - 1; j++) {
    kr[j] = all_knots[i + j + 1] - x; // k right values
    kl[j] = x - all_knots[i - j];     // k left values
    saved = 0;
    for (int r = 0; r <= j; r++) {
      term = b[r] / (kr[r] + kl[j-r]);
      b[r] = saved + kr[r] * term;
      saved = kl[j-r] * term;
    }
    b[j+1] = saved;
  }
}

/**
 * Create the augmented knots vector and fill in matrix Xt with the spline basis vectors
 */
void basis_splines_endreps_local_v2(float *knots, int n_knots, int order, int *boundaries, int n_boundaries,  Eigen::MatrixXf &Xt) {
  assert(n_boundaries == 2 && boundaries[0] < boundaries[1]);
  int n_rows = boundaries[1] - boundaries[0]; // this is frames so evaluate at each frame we'll need
  int n_cols = n_knots + order;  // number of basis vectors we'll have at end
  Xt.resize(n_cols, n_rows); // swapped to transpose later. This order lets us use it as a scratch space
  Xt.fill(0);
  int n_std_knots = n_knots + 2 * order;
  float std_knots[n_std_knots];

  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i] = boundaries[0];
  }
  // Our original boundary knots here
  for (int i = 0; i < n_knots; i++) {
    std_knots[i+order] = knots[i];
  }
  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i+order+n_knots] = boundaries[1];
  }
  // Evaluate our basis splines at each frame.
  for (int i = boundaries[0]; i < boundaries[1]; i++) {
    int idx = -1;
    // find index such that i >= knots[idx] && i < knots[idx+1]
    float *val = std::upper_bound(std_knots, std_knots + n_std_knots - 1, 1.0f * i);
    idx = val - std_knots - 1;
    assert(idx >= 0);
    float *f = Xt.data() + i * n_cols + idx - (order - 1); //column offset
    basis_spline_xi_v2(std_knots, n_std_knots, order, idx, i, f);
  }
  // Put in our conventional format where each column is a basis vector
  Xt.transposeInPlace();
}

void FillInKnots(const std::string &strategy, int n_frame, std::vector<float> &knots) {
  if (strategy != "no-knots") {
    if (strategy == "even4") {
      float stride = n_frame / 3.0f;
      knots.push_back(stride);
      knots.push_back(stride * 2.0f);
    }
    else if (strategy == "every4") {
      int current = 4;
      while (current < (n_frame -1)) {
        knots.push_back(current);
        current+=4;
      }
    }
    else if (strategy == "every3") {
      int current = 3;
      while (current < (n_frame -1)) {
        knots.push_back(current);
        current+=3;
      }
    }
    else if (strategy == "middle") {
      float stride = n_frame / 2.0f;
      knots.push_back(stride);
    }
    else if (strategy.find("explicit:") == 0) {
      string spec = strategy.substr(9, strategy.length() - 9);
      knots = char2Vec<float>(spec.c_str(), ',');
    }
    else {
      assert(false);
    }
  }
}

void TraceStoreCol::SplineLossyCompress(const std::string &strategy, int order, char *bad_wells, float *mad) {
  Eigen::MatrixXf Basis;
  vector<float> knots;
  FillInKnots(strategy, mFrames, knots);
  //  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> Basis(, compressed.n_frames, compressed.n_basis);
  if (!knots.empty()) {
    int boundaries[2];
    boundaries[0] = 0;
    boundaries[1] = mFrames;
    basis_splines_endreps_local_v2(&knots[0], knots.size(), order, boundaries, sizeof(boundaries)/sizeof(boundaries[0]), Basis);
  }
  Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  Eigen::MatrixXf Y(mFlowFrameStride, mFrames);
  //  Eigen::MatrixXf FlowMeans(mFlows, mFrames);

  //  FlowMeans.setZero();
  int good_wells = 0;
  char *bad_start = bad_wells;
  char *bad_end = bad_start + mFrameStride;
  while(bad_start != bad_end) {
    if (*bad_start++ == 0) {
      good_wells++;
    }
  }

  // if nothing good then skip it
  if (good_wells < MIN_SAMPLE_WELL) {
    return;
  }

  std::vector<ChipReduction> smoothed_avg(mFlows);
  int x_clip = mCols;
  int y_clip = mRows;
  if (mUseMeshNeighbors == 0) {
    x_clip = THUMBNAIL_SIZE;
    y_clip = THUMBNAIL_SIZE;
  }
  for (size_t flow_ix = 0; flow_ix < mFlows; flow_ix++) {
    smoothed_avg[flow_ix].Init(mRows, mCols, mFrames,
                               SMOOTH_REDUCE_STEP, SMOOTH_REDUCE_STEP,
                               y_clip, x_clip,
                               SMOOTH_REDUCE_STEP * SMOOTH_REDUCE_STEP * .4);
    for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
      smoothed_avg[flow_ix].ReduceFrame(&mData[0] + flow_ix * mFrameStride + frame_ix * mFlowFrameStride, bad_wells, frame_ix);
    }
    smoothed_avg[flow_ix].SmoothBlocks(SMOOTH_REDUCE_REGION, SMOOTH_REDUCE_REGION);
  }

  float *y_start = Y.data();
  float *y_end = Y.data() + mData.size();
  int16_t *trace_start = &mData[0];
  while(y_start != y_end) {
    *y_start++ = *trace_start++;
  }

  // // get the flow means per frame.
  // for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
  //   for(size_t flow_ix = 0; flow_ix < mFlows; flow_ix++) {
  //     float *start = Y.data() + mFlowFrameStride * frame_ix + flow_ix * mFrameStride;
  //     float *end = start + mFrameStride;
  //     float *sum = &FlowMeans(flow_ix, frame_ix);
  //     char *bad = bad_wells;
  //     while (start != end) {
  //       if (*bad == 0) {
  //         *sum += *start;
  //       }
  //       start++;
  //       bad++;
  //     }
  //     *sum /= good_wells;
  //   }
  // }

  // subtract off flow,frame avg
  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    for(size_t flow_ix = 0; flow_ix < mFlows; flow_ix++) {
      float *start = Y.data() + mFlowFrameStride * frame_ix + flow_ix * mFrameStride;
      float *end = start + mFrameStride;
      for (size_t row = 0; row < mRows; row++) {
        for (size_t col = 0; col < mCols; col++) {
          float avg = smoothed_avg[flow_ix].GetSmoothEst(row, col, frame_ix);
          *start++ -= avg;
        }
      }
    }
  }

  // // subtract them off
  // Eigen::VectorXf col_mean = Y.colwise().sum();
  // col_mean /= Y.rows();

  // for (int i = 0; i < Y.cols(); i++) {
  //   Y.col(i).array() -= col_mean.coeff(i);
  // }

  // Get coefficients to solve
  Eigen::MatrixXf B = Y * SX.transpose();
  // Uncompress data into yhat matrix
  Eigen::MatrixXf Yhat = B * Basis.transpose();

  // add the flow/frame averages back
  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    for(size_t flow_ix = 0; flow_ix < mFlows; flow_ix++) {
      float *start = Y.data() + mFlowFrameStride * frame_ix + flow_ix * mFrameStride;
      float *end = start + mFrameStride;
      float *hstart = Yhat.data() + mFlowFrameStride * frame_ix + flow_ix * mFrameStride;
      for (size_t row = 0; row < mRows; row++) {
        for (size_t col = 0; col < mCols; col++) {
          float avg = smoothed_avg[flow_ix].GetSmoothEst(row, col, frame_ix);
          *start++ += avg;
          *hstart++ += avg;
        }
      }
    }
  }

  // for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
  //   for(size_t flow_ix = 0; flow_ix < mFlows; flow_ix++) {
  //     float *start = Y.data() + mFlowFrameStride * frame_ix + flow_ix * mFrameStride;
  //     float *hstart = Yhat.data() + mFlowFrameStride * frame_ix + flow_ix * mFrameStride;
  //     float *end = start + mFrameStride;
  //     float avg = FlowMeans(flow_ix, frame_ix);
  //     while (start != end) {
  //       *start++ += avg;
  //       *hstart++ += avg;
  //     }
  //   }
  // }

  // for (int i = 0; i < Yhat.cols(); i++) {
  //   Yhat.col(i).array() += col_mean.coeff(i);
  //   Y.col(i).array() += col_mean.coeff(i);
  // }

  float *yhat_start = Yhat.data();
  float *yhat_end = Yhat.data() + mData.size();
  trace_start = &mData[0];
  while(yhat_start != yhat_end) {
    *trace_start++ = (int)(*yhat_start + .5);
    yhat_start++;
  }

  Y = Y - Yhat;
  Eigen::VectorXf M = Y.rowwise().squaredNorm();

  for (size_t flow_ix = 0; flow_ix < mFlows; flow_ix++) {
    float *mad_start = mad;
    float *mad_end = mad_start + mFrameStride;
    float *m_start = M.data() + flow_ix * mFrameStride;
    while (mad_start != mad_end) {
      *mad_start += *m_start;
      mad_start++;
      m_start++;
    }
  }

  float *mad_start = mad;
  float *mad_end = mad + mFrameStride;
  int norm_factor = mFlows * mFrames;
  while (mad_start != mad_end) {
    *mad_start /= norm_factor;
    mad_start++;
  }
}

void TraceStoreCol::SplineLossyCompress(const std::string &strategy, int order, int flow_ix, char *bad_wells, float *mad) {
  Eigen::MatrixXf Basis;
  vector<float> knots;
  FillInKnots(strategy, mFrames, knots);
  //  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> Basis(, compressed.n_frames, compressed.n_basis);
  if (!knots.empty()) {
    int boundaries[2];
    boundaries[0] = 0;
    boundaries[1] = mFrames;
    basis_splines_endreps_local_v2(&knots[0], knots.size(), order, boundaries, sizeof(boundaries)/sizeof(boundaries[0]), Basis);
  }
  Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  Eigen::MatrixXf Y(mFrameStride, mFrames);
  //  Eigen::MatrixXf FlowMeans(mFlows, mFrames);

  //  FlowMeans.setZero();
  int good_wells = 0;
  char *bad_start = bad_wells;
  char *bad_end = bad_start + mFrameStride;
  while(bad_start != bad_end) {
    if (*bad_start++ == 0) {
      good_wells++;
    }
  }

  // if nothing good then skip it
  if (good_wells < MIN_SAMPLE_WELL) {
    return;
  }

  ChipReduction smoothed_avg;
  int x_clip = mCols;
  int y_clip = mRows;
  if (mUseMeshNeighbors == 0) {
    x_clip = THUMBNAIL_SIZE;
    y_clip = THUMBNAIL_SIZE;
  }

  smoothed_avg.Init(mRows, mCols, mFrames,
                    SMOOTH_REDUCE_STEP, SMOOTH_REDUCE_STEP,
                    y_clip, x_clip,
                    SMOOTH_REDUCE_STEP * SMOOTH_REDUCE_STEP * .4);
  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    smoothed_avg.ReduceFrame(&mData[0] + flow_ix * mFrameStride + frame_ix * mFlowFrameStride, bad_wells, frame_ix);
  }
  smoothed_avg.SmoothBlocks(SMOOTH_REDUCE_REGION, SMOOTH_REDUCE_REGION);

  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    float *y_start = Y.data() + frame_ix * mFrameStride;
    float *y_end = y_start + mFrameStride;
    int16_t *trace_start = &mData[0] + flow_ix * mFrameStride + frame_ix * mFlowFrameStride;
    while(y_start != y_end) {
      *y_start++ = *trace_start++;
    }
  }

  // subtract off flow,frame avg
  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    float *start = Y.data() + mFrameStride * frame_ix;
    float *end = start + mFrameStride;
    for (size_t row = 0; row < mRows; row++) {
      for (size_t col = 0; col < mCols; col++) {
        float avg = smoothed_avg.GetSmoothEst(row, col, frame_ix);
        *start++ -= avg;
      }
    }
  }

  // Get coefficients to solve
  Eigen::MatrixXf B = Y * SX.transpose();
  // Uncompress data into yhat matrix
  Eigen::MatrixXf Yhat = B * Basis.transpose();

  // add the flow/frame averages back
  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    float *start = Y.data() + mFrameStride * frame_ix;
    float *end = start + mFrameStride;
    float *hstart = Yhat.data() + mFrameStride * frame_ix;
    for (size_t row = 0; row < mRows; row++) {
      for (size_t col = 0; col < mCols; col++) {
        float avg = smoothed_avg.GetSmoothEst(row, col, frame_ix);
        *start++ += avg;
        *hstart++ += avg;
      }
    }
  }

  for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
    float *yhat_start = Yhat.data() + frame_ix * mFrameStride;
    float *yhat_end = yhat_start + mFrameStride;
    int16_t *trace_start = &mData[0] + flow_ix * mFrameStride + frame_ix * mFlowFrameStride;
    while(yhat_start != yhat_end) {
      *trace_start++ = (int)(*yhat_start + .5f);
      yhat_start++;
    }
  }

  Y = Y - Yhat;
  Eigen::VectorXf M = Y.rowwise().squaredNorm();

  float *mad_start = mad;
  float *mad_end = mad_start + mFrameStride;
  float *m_start = M.data();
  while (mad_start != mad_end) {
    *mad_start += *m_start;
    mad_start++;
    m_start++;
  }

  mad_start = mad;
  mad_end = mad + mFrameStride;
  int norm_factor = mFrames;// mFlows * mFrames;
  while (mad_start != mad_end) {
    *mad_start /= norm_factor;
    mad_start++;
  }
}

// Compress a block of a data using pca
void TraceStoreCol::PcaLossyCompress(int row_start, int row_end,
                                     int col_start, int col_end,
                                     int flow_ix,
                                     float *ssq, char *filters,
                                     int row_step, int col_step,
                                     int num_pca) {

  Eigen::MatrixXf Ysub, Y, S, Basis;

  int loc_num_wells = (col_end - col_start) * (row_end - row_start);
  int loc_num_cols = col_end - col_start;

  // Take a sample of the data at rate step, avoiding flagged wells
  // Count the good rows
  int sample_wells = 0;
  for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
    char *filt_start = filters + row_ix * mCols + col_start;
    char *filt_end = filt_start + loc_num_cols;
    while (filt_start < filt_end) {
      if (*filt_start == 0) {
        sample_wells++;
      }
      filt_start += col_step;
    }
  }
  // try backing off to every well rather than just sampled if we didn't get enough
  if (sample_wells < MIN_SAMPLE_WELL) {
    row_step = 1;
    col_step = 1;
    int sample_wells = 0;
    for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
      char *filt_start = filters + row_ix * mCols + col_start;
      char *filt_end = filt_start + loc_num_cols;
      while (filt_start < filt_end) {
        if (*filt_start == 0) {
          sample_wells++;
        }
        filt_start += col_step;
      }
    }
  }

  if (sample_wells < MIN_SAMPLE_WELL) {
    return; // just give up
  }

  // Copy the sampled data in Matrix, frame major
  Ysub.resize(sample_wells, mFrames);
  for (int frame_ix = 0; frame_ix < (int)mFrames; frame_ix++) {
    int sample_offset = 0;
    for (int row_ix = row_start; row_ix < row_end; row_ix+=row_step) {
      size_t store_offset = row_ix * mCols + col_start;
      char *filt_start = filters + store_offset;
      char *filt_end = filt_start + loc_num_cols;
      int16_t *trace_start = &mData[0] + (mFlowFrameStride * frame_ix) + (flow_ix * mFrameStride) + store_offset;
      float *ysub_start = Ysub.data() + sample_wells * frame_ix + sample_offset;
      while (filt_start < filt_end) {
        if (*filt_start == 0) {
          *ysub_start = *trace_start;
          ysub_start++;
          sample_offset++;
        }
        trace_start += col_step;
        filt_start += col_step;
      }
    }
  }

  // Copy in all the data into working matrix
  Y.resize(loc_num_wells, (int)mFrames);
  for (int frame_ix = 0; frame_ix < (int)mFrames; frame_ix++) {
    for (int row_ix = row_start; row_ix < row_end; row_ix++) {
      size_t store_offset = row_ix * mCols + col_start;
      int16_t *trace_start = &mData[0] + (mFlowFrameStride * frame_ix) + (flow_ix * mFrameStride) + store_offset;
      int16_t *trace_end = trace_start + loc_num_cols;
      float * y_start = Y.data() + loc_num_wells * frame_ix + (row_ix - row_start) * loc_num_cols;
      while( trace_start != trace_end ) {
        *y_start++ = *trace_start++;
      }
    }
  }
  Eigen::VectorXf col_mean = Y.colwise().sum();
  col_mean /= Y.rows();

  for (int i = 0; i < Y.cols(); i++) {
    Y.col(i).array() -= col_mean.coeff(i);
  }
  // Create scatter matrix
  S = Ysub.transpose() * Ysub;
  // Compute the eigenvectors
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
  es.compute(S);
  Eigen::MatrixXf Pca_Basis = es.eigenvectors();
  Eigen::VectorXf Pca_Values = es.eigenvalues();
  // Copy top eigen vectors into basis for projection
  Basis.resize(mFrames, num_pca);
  for (int i = 0; i < Basis.cols(); i++) {
    //    Basis.col(i) = es.eigenvectors().col(es.eigenvectors().cols() - i -1);
    Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
  }
  // Create solver matrix, often not a good way of solving things but eigen vectors should be stable and fast
  Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  // Get coefficients to solve
  Eigen::MatrixXf B = Y * SX.transpose();
  // Uncompress data into yhat matrix
  Eigen::MatrixXf Yhat = B * Basis.transpose();

  for (int i = 0; i < Yhat.cols(); i++) {
    Yhat.col(i).array() += col_mean.coeff(i);
    Y.col(i).array() += col_mean.coeff(i);
  }

  // H5File h5("pca_lossy.h5");
  // h5.Open();
  // char buff[256];
  // snprintf(buff, sizeof(buff), "/Y_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Y);
  // snprintf(buff, sizeof(buff), "/Yhat_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Yhat);
  // snprintf(buff, sizeof(buff), "/Basis_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Basis);
  // h5.Close();
  // Copy data out of yhat matrix into original data structure, keeping track of residuals
  for (int frame_ix = 0; frame_ix < (int)mFrames; frame_ix++) {
    for (int row_ix = row_start; row_ix < row_end; row_ix++) {
      size_t store_offset = row_ix * mCols + col_start;
      int16_t *trace_start = &mData[0] + mFlowFrameStride * frame_ix + flow_ix * mFrameStride + store_offset;
      int16_t *trace_end = trace_start + loc_num_cols;
      float * ssq_start = ssq + store_offset;
      size_t loc_offset = (row_ix - row_start) * loc_num_cols;
      float * y_start = Y.data() + loc_num_wells * frame_ix + loc_offset;
      float * yhat_start = Yhat.data() + loc_num_wells * frame_ix + loc_offset;
      while( trace_start != trace_end ) {
        *trace_start = (int16_t)(*yhat_start + .5);
        float val = *y_start - *yhat_start;
        *ssq_start += val * val;
        y_start++;
        yhat_start++;
        trace_start++;
        ssq_start++;
      }
    }
  }

  // divide ssq data out for per frame avg
  for (int row_ix = row_start; row_ix < row_end; row_ix++) {
    size_t store_offset = row_ix * mCols + col_start;
    float * ssq_start = ssq + store_offset;
    float * ssq_end = ssq_start + loc_num_cols;
    while (ssq_start != ssq_end) {
      *ssq_start /= mFrames;
      ssq_start++;
    }
  }
}

bool TraceStoreCol::PcaLossyCompressChunk(int row_start, int row_end,
                                          int col_start, int col_end,
                                          int num_rows, int num_cols, int num_frames,
                                          int frame_stride,
                                          int flow_ix, int flow_frame_stride,
                                          short *data, bool replace,
                                          float *ssq, char *filters,
                                          int row_step, int col_step,
                                          int num_pca) {

  Eigen::MatrixXf Ysub, Y, S, Basis;

  int loc_num_wells = (col_end - col_start) * (row_end - row_start);
  int loc_num_cols = col_end - col_start;

  // Take a sample of the data at rate step, avoiding flagged wells
  // Count the good rows
  int sample_wells = 0;
  for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
    char *filt_start = filters + row_ix * num_cols + col_start;
    char *filt_end = filt_start + loc_num_cols;
    while (filt_start < filt_end) {
      if (*filt_start == 0) {
        sample_wells++;
      }
      filt_start += col_step;
    }
  }
  // try backing off to every well rather than just sampled if we didn't get enough
  if (sample_wells < MIN_SAMPLE_WELL) {
    row_step = 1;
    col_step = 1;
    int sample_wells = 0;
    for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
      char *filt_start = filters + row_ix * num_cols + col_start;
      char *filt_end = filt_start + loc_num_cols;
      while (filt_start < filt_end) {
        if (*filt_start == 0) {
          sample_wells++;
        }
        filt_start += col_step;
      }
    }
  }

  if (sample_wells < MIN_SAMPLE_WELL) {
    return false; // just give up
  }

  // Got enough data to work with, zero out the ssq array for accumulation
  for (int row_ix = row_start; row_ix < row_end; row_ix++) {
    float *ssq_start = ssq + row_ix * num_cols + col_start;
    float *ssq_end = ssq_start + loc_num_cols;
    while (ssq_start != ssq_end) {
      *ssq_start++ = 0;
    }
  }
  // Copy the sampled data in Matrix, frame major
  Ysub.resize(sample_wells, num_frames);
  for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
    int sample_offset = 0;
    for (int row_ix = row_start; row_ix < row_end; row_ix+=row_step) {
      size_t store_offset = row_ix * num_cols + col_start;
      char *filt_start = filters + store_offset;
      char *filt_end = filt_start + loc_num_cols;
      int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
      float *ysub_start = Ysub.data() + sample_wells * frame_ix + sample_offset;
      while (filt_start < filt_end) {
        if (*filt_start == 0) {
          *ysub_start = *trace_start;
          ysub_start++;
          sample_offset++;
        }
        trace_start += col_step;
        filt_start += col_step;
      }
    }
  }

  // Copy in all the data into working matrix
  Y.resize(loc_num_wells, (int)num_frames);
  for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
    for (int row_ix = row_start; row_ix < row_end; row_ix++) {
      size_t store_offset = row_ix * num_cols + col_start;
      int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
      int16_t *trace_end = trace_start + loc_num_cols;
      float * y_start = Y.data() + loc_num_wells * frame_ix + (row_ix - row_start) * loc_num_cols;
      while( trace_start != trace_end ) {
        *y_start++ = *trace_start++;
      }
    }
  }
  Eigen::VectorXf col_mean = Y.colwise().sum();
  col_mean /= Y.rows();

  for (int i = 0; i < Y.cols(); i++) {
    Y.col(i).array() -= col_mean.coeff(i);
  }
  // Create scatter matrix
  S = Ysub.transpose() * Ysub;
  // Compute the eigenvectors
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
  es.compute(S);
  Eigen::MatrixXf Pca_Basis = es.eigenvectors();
  Eigen::VectorXf Pca_Values = es.eigenvalues();
  // Copy top eigen vectors into basis for projection
  Basis.resize(num_frames, num_pca);
  for (int i = 0; i < Basis.cols(); i++) {
    //    Basis.col(i) = es.eigenvectors().col(es.eigenvectors().cols() - i -1);
    Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
  }
  // Create solver matrix, often not a good way of solving things but eigen vectors should be stable and fast
  Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  // Get coefficients to solve
  Eigen::MatrixXf B = Y * SX.transpose();
  // Uncompress data into yhat matrix
  Eigen::MatrixXf Yhat = B * Basis.transpose();

  for (int i = 0; i < Yhat.cols(); i++) {
    Yhat.col(i).array() += col_mean.coeff(i);
    Y.col(i).array() += col_mean.coeff(i);
  }

  // H5File h5("pca_lossy.h5");
  // h5.Open();
  // char buff[256];
  // snprintf(buff, sizeof(buff), "/Y_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Y);
  // snprintf(buff, sizeof(buff), "/Yhat_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Yhat);
  // snprintf(buff, sizeof(buff), "/Basis_%d_%d_%d", flow_ix, row_start, col_start);
  // H5Eigen::WriteMatrix(h5, buff, Basis);
  // h5.Close();
  // Copy data out of yhat matrix into original data structure, keeping track of residuals
  for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
    for (int row_ix = row_start; row_ix < row_end; row_ix++) {
      size_t store_offset = row_ix * num_cols + col_start;
      int16_t *trace_start = data + flow_frame_stride * frame_ix + flow_ix * frame_stride + store_offset;
      int16_t *trace_end = trace_start + loc_num_cols;
      float * ssq_start = ssq + store_offset;
      size_t loc_offset = (row_ix - row_start) * loc_num_cols;
      float * y_start = Y.data() + loc_num_wells * frame_ix + loc_offset;
      float * yhat_start = Yhat.data() + loc_num_wells * frame_ix + loc_offset;
      while( trace_start != trace_end ) {
        if (replace) {
          *trace_start = (int16_t)(*yhat_start + .5);
        }
        float val = *y_start - *yhat_start;
        *ssq_start += val * val;
        y_start++;
        yhat_start++;
        trace_start++;
        ssq_start++;
      }
    }
  }

  // divide ssq data out for per frame avg
  for (int row_ix = row_start; row_ix < row_end; row_ix++) {
    size_t store_offset = row_ix * num_cols + col_start;
    float * ssq_start = ssq + store_offset;
    float * ssq_end = ssq_start + loc_num_cols;
    while (ssq_start != ssq_end) {
      *ssq_start /= num_frames;
      ssq_start++;
    }
  }
  return true;
}

void TraceStoreCol::SplineLossyCompress(const std::string &strategy, int order, int flow_ix, char *bad_wells,
                                        float *mad, size_t num_rows, size_t num_cols, size_t num_frames, size_t num_flows,
                                        int use_mesh_neighbors, size_t frame_stride, size_t flow_frame_stride, int16_t *data) {
  Eigen::MatrixXf Basis;
  vector<float> knots;
  FillInKnots(strategy, num_frames, knots);
  if (!knots.empty()) {
    int boundaries[2];
    boundaries[0] = 0;
    boundaries[1] = num_frames;
    basis_splines_endreps_local_v2(&knots[0], knots.size(), order, boundaries, sizeof(boundaries)/sizeof(boundaries[0]), Basis);
  }
  Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  Eigen::MatrixXf Y(frame_stride, num_frames);
  //  Eigen::MatrixXf FlowMeans(num_flows, num_frames);

  //  FlowMeans.setZero();
  int good_wells = 0;
  char *bad_start = bad_wells;
  char *bad_end = bad_start + frame_stride;
  while(bad_start != bad_end) {
    if (*bad_start++ == 0) {
      good_wells++;
    }
  }

  // if nothing good then skip it
  if (good_wells < MIN_SAMPLE_WELL) {
    return;
  }

  ChipReduction smoothed_avg;
  int x_clip = num_cols;
  int y_clip = num_rows;
  if (use_mesh_neighbors == 0) {
    x_clip = THUMBNAIL_SIZE;
    y_clip = THUMBNAIL_SIZE;
  }

  smoothed_avg.Init(num_rows, num_cols, num_frames,
                    SMOOTH_REDUCE_STEP, SMOOTH_REDUCE_STEP,
                    y_clip, x_clip,
                    SMOOTH_REDUCE_STEP * SMOOTH_REDUCE_STEP * .4);
  for (size_t frame_ix = 0; frame_ix < num_frames; frame_ix++) {
    smoothed_avg.ReduceFrame(data + flow_ix * frame_stride + frame_ix * flow_frame_stride, bad_wells, frame_ix);
  }
  smoothed_avg.SmoothBlocks(SMOOTH_REDUCE_REGION, SMOOTH_REDUCE_REGION);

  for (size_t frame_ix = 0; frame_ix < num_frames; frame_ix++) {
    float *y_start = Y.data() + frame_ix * frame_stride;
    float *y_end = y_start + frame_stride;
    int16_t *trace_start = data + flow_ix * frame_stride + frame_ix * flow_frame_stride;
    while(y_start != y_end) {
      *y_start++ = *trace_start++;
    }
  }

  // subtract off flow,frame avg
  for (size_t frame_ix = 0; frame_ix < num_frames; frame_ix++) {
    float *start = Y.data() + frame_stride * frame_ix;
    float *end = start + frame_stride;
    for (size_t row = 0; row < num_rows; row++) {
      for (size_t col = 0; col < num_cols; col++) {
        float avg = smoothed_avg.GetSmoothEst(row, col, frame_ix);
        *start++ -= avg;
      }
    }
  }

  // Get coefficients to solve
  Eigen::MatrixXf B = Y * SX.transpose();
  // Uncompress data into yhat matrix
  Eigen::MatrixXf Yhat = B * Basis.transpose();

  // add the flow/frame averages back
  for (size_t frame_ix = 0; frame_ix < num_frames; frame_ix++) {
    float *start = Y.data() + frame_stride * frame_ix;
    float *end = start + frame_stride;
    float *hstart = Yhat.data() + frame_stride * frame_ix;
    for (size_t row = 0; row < num_rows; row++) {
      for (size_t col = 0; col < num_cols; col++) {
        float avg = smoothed_avg.GetSmoothEst(row, col, frame_ix);
        *start++ += avg;
        *hstart++ += avg;
      }
    }
  }

  for (size_t frame_ix = 0; frame_ix < num_frames; frame_ix++) {
    float *yhat_start = Yhat.data() + frame_ix * frame_stride;
    float *yhat_end = yhat_start + frame_stride;
    int16_t *trace_start = data + flow_ix * frame_stride + frame_ix * flow_frame_stride;
    while(yhat_start != yhat_end) {
      *trace_start++ = (int)(*yhat_start + .5f);
      yhat_start++;
    }
  }

  Y = Y - Yhat;
  Eigen::VectorXf M = Y.rowwise().squaredNorm();

  float *mad_start = mad;
  float *mad_end = mad_start + frame_stride;
  float *m_start = M.data();
  while (mad_start != mad_end) {
    *mad_start += *m_start;
    mad_start++;
    m_start++;
  }

  mad_start = mad;
  mad_end = mad + frame_stride;
  int norm_factor = num_frames;// num_flows * num_frames;
  while (mad_start != mad_end) {
    *mad_start /= norm_factor;
    mad_start++;
  }
}

int TraceStoreCol::PrepareReference(size_t flowIx, std::vector<char> &filteredWells) {
    mRefWells.resize(mUseAsReference.size());
    for (size_t i = 0; i < mRefWells.size(); i++) {
      if (mUseAsReference[i]) {
        mRefWells[i] = 0;
      }
      else {
        mRefWells[i] = 1;
      }
    }
    assert(flowIx < mRefReduction.size());

    int x_clip = mCols;
    int y_clip = mRows;
    if (mUseMeshNeighbors == 0) {
      x_clip = THUMBNAIL_SIZE;
      y_clip = THUMBNAIL_SIZE;
    }
    mRefReduction[flowIx].Init(mRows, mCols, mFrames,
                               REF_REDUCTION_SIZE, REF_REDUCTION_SIZE,
                               y_clip, x_clip, 1);
    for (size_t frame_ix = 0; frame_ix < mFrames; frame_ix++) {
      mRefReduction[flowIx].ReduceFrame(&mData[0] + flowIx * mFrameStride + frame_ix * mFlowFrameStride, &mRefWells[0], frame_ix);
    }
    mRefReduction[flowIx].SmoothBlocks(REF_SMOOTH_SIZE, REF_SMOOTH_SIZE);
    return TSM_OK;
  }

int TraceStoreCol::PrepareReferenceOld (size_t flowIx, std::vector<char> &filteredWells) {
    int fIdx = flowIx;
    CalcReference (mRowRefStep, mColRefStep, flowIx, mRefGrids[fIdx], filteredWells);
    mRefGridsValid[fIdx] = 1;
    mFineRefGrids.resize (mRefGrids.size());
    mFineRefGrids[fIdx].Init (mRows, mCols, mRowRefStep/2, mColRefStep/2);
    int numBin = mFineRefGrids[fIdx].GetNumBin();
    int rowStart = -1, rowEnd = -1, colStart = -1, colEnd = -1;
    for (int binIx = 0; binIx < numBin; binIx++) {
      mFineRefGrids[fIdx].GetBinCoords (binIx, rowStart, rowEnd, colStart, colEnd);
      vector<float> &trace = mFineRefGrids[fIdx].GetItem (binIx);
      CalcMedianReference ( (rowEnd + rowStart) /2, (colEnd + colStart) /2, mRefGrids[fIdx],
                            mDist, mValues, trace);
    }
    return TSM_OK;
  }

int TraceStoreCol::CalcMedianReference (size_t row, size_t col,
                           GridMesh<std::vector<float> > &regionMed,
                           std::vector<double> &dist,
                           std::vector<std::vector<float> *> &values,
                           std::vector<float> &reference) {
    int retVal = TraceStore::TS_OK;
    reference.resize(mFrames);
    std::fill(reference.begin(), reference.end(), 0.0);
    regionMed.GetClosestNeighbors (row, col, mUseMeshNeighbors, dist, values);
    int num_good = 0;
    size_t valSize = values.size();
    for (size_t i =0; i < valSize; i++) {
      if (values[i]->size() > 0) {
        num_good++;
      }
    }
    // try reaching a little farther if no good reference close by
    if (num_good == 0) {
      regionMed.GetClosestNeighbors (row, col, 2*mUseMeshNeighbors, dist, values);
    }
    size_t size = 0;
    double maxDist = 0;
    for (size_t i = 0; i < values.size(); i++) {
        size = max (values[i]->size(), size);
        maxDist = max(dist[i], maxDist);
    }
    reference.resize (size);
    std::fill (reference.begin(), reference.end(), 0.0);
    double distWeight = 0;
         valSize = values.size();

    for (size_t i = 0; i < valSize; i++) {
      if (values[i]->size()  == 0) {
        continue;
      }
      double w = TraceStore::WeightDist (dist[i], mRowRefStep); //1/sqrt(dist[i]+1);
      distWeight += w;
      size_t vSize = values[i]->size();
      for (size_t j = 0; j < vSize; j++) {
        reference[j] += w * values[i]->at (j);
      }
    }
    // Divide by our total weight to get weighted mean
    if (distWeight > 0)  {
      for (size_t i = 0; i < reference.size(); i++) {
        reference[i] /= distWeight;
      }
      retVal = TraceStore::TS_OK;
    }
    else {
      retVal = TraceStore::TS_BAD_DATA;
    }
    return retVal;
  }


#ifndef STAN_MATH_PRIM_MAT_PROB_ORDERED_LOGISTIC_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_ORDERED_LOGISTIC_LPMF_HPP

#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/log1m_exp.hpp>
#include <stan/math/prim/scal/fun/log1p_exp.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater.hpp>
#include <stan/math/prim/scal/err/check_less.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/mat/prob/categorical_rng.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
  namespace math {

    template <typename T>
    inline T log_inv_logit_diff(const T& alpha, const T& beta) {
      using std::exp;
      return beta + log1m_exp(alpha - beta) - log1p_exp(alpha)
        - log1p_exp(beta);
    }

    /**
     * Returns the (natural) log probability of the specified integer
     * outcome given the continuous location and specified cutpoints
     * in an ordered logistic model.
     *
     * <p>Typically the continous location
     * will be the dot product of a vector of regression coefficients
     * and a vector of predictors for the outcome.
     *
     * @tparam propto True if calculating up to a proportion.
     * @tparam T_loc Location type.
     * @tparam T_cut Cut-point type.
     * @param y Outcome.
     * @param lambda Location.
     * @param c Positive increasing vector of cutpoints.
     * @return Log probability of outcome given location and
     * cutpoints.

     * @throw std::domain_error If the outcome is not between 1 and
     * the number of cutpoints plus 2; if the cutpoint vector is
     * empty; if the cutpoint vector contains a non-positive,
     * non-finite value; or if the cutpoint vector is not sorted in
     * ascending order.
     */
    template <bool propto, typename T_lambda, typename T_cut>
    typename boost::math::tools::promote_args<T_lambda, T_cut>::type
    ordered_logistic_lpmf(int y, const T_lambda& lambda,
                         const Eigen::Matrix<T_cut, Eigen::Dynamic, 1>& c) {
      using std::exp;
      using std::log;

      static const char* function("ordered_logistic");

      int K = c.size() + 1;

      check_bounded(function, "Random variable", y, 1, K);
      check_finite(function, "Location parameter", lambda);
      check_greater(function, "Size of cut points parameter", c.size(), 0);
      for (int i = 1; i < c.size(); ++i)
        check_greater(function, "Cut points parameter", c(i), c(i - 1));

      check_finite(function, "Cut points parameter", c(c.size()-1));
      check_finite(function, "Cut points parameter", c(0));

      // log(1 - inv_logit(lambda))
      if (y == 1)
        return -log1p_exp(lambda - c(0));

      // log(inv_logit(lambda - c(K-3)));
      if (y == K) {
        return -log1p_exp(c(K-2) - lambda);
      }

      // if (2 < y < K) { ... }
      // log(inv_logit(lambda - c(y-2)) - inv_logit(lambda - c(y-1)))
      return log_inv_logit_diff(c(y-2) - lambda,
                                c(y-1) - lambda);
    }

    template <typename T_lambda, typename T_cut>
    typename boost::math::tools::promote_args<T_lambda, T_cut>::type
    ordered_logistic_lpmf(int y, const T_lambda& lambda,
                         const Eigen::Matrix<T_cut, Eigen::Dynamic, 1>& c) {
      return ordered_logistic_lpmf<false>(y, lambda, c);
    }

  }
}
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_BIT_MASKING_INCLUDE
#define MTL_BIT_MASKING_INCLUDE

#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>

#include <boost/numeric/mtl/utility/tag.hpp>

namespace mtl {

/*
     The bit masks are row masks, that mean 1s represent rows and 0s columns.

     Bit masks:

     i-order (cyrillic i):
     ---------------------

     binary:     01010101 ... 01
     0x55555555

     z-order:
     --------

     binary:     10101010 ... 10
     0xaaaaaaaa

     row major:
     ----------

     with 2^k columns
     binary:    111111111...1000...0
                             ------- k 0s at the end (LSB), all bits before 1s (MSB)

     column major:
     -------------

     with 2^k rows
     binary:    000000000...0111...1
                             ------- k 1s at the end (LSB), all bits before 0s (MSB)

     hybrid (Doppled):
     -----------------

     i-order
     with 2^k by 2^k base case
     row major
     binary     0101....011...10...0
                               ----- k 0s at the end (LSB); means columns
                          ----- k 1s before; means rows
                ---------- i order
     e.g. 32 by 32 base case 0101...01 11111 00000 = 0x555557e0

     column major
     binary     0101....010...01...1
                               ----- k 1s at the end (LSB); means rows
                          ----- k 0s before; means columns
                ---------- i order
     e.g. 32 by 32 base case 0101...01 00000 11111 = 0x5555541f

     Shark-tooth base case:
     ----------------------

     2^t tooth length
     in  2^k by 2^k base case (of course t <= k)
     row-major
     binary     1..1 00..0 1..1
                           ---- t 1s at the end (LSB); means 2^t tooth allong rows
                     ---- k 0s before; means columns
                ---- k-t 1s before; means rows

     column-major
     binary     0..0 11..1 0..0
                           ---- t 0s at the end (LSB); means 2^t tooth allong columns
                     ---- k 1s before; means rows
                ---- k-t 0s before; means columns

*/

// Mask for the last N bits
template <unsigned long N>
struct lsb_mask
{
    static const unsigned long value= (lsb_mask<N-1>::value << 1) | 1;
};

template <>
struct lsb_mask<0>
{
    static const unsigned long value= 0;
};

/// Last N bits of Value
template <unsigned long N, unsigned long Value>
struct lsb_bits
{
    static const unsigned long value= lsb_mask<N>::value & Value;
};

/// Compares two masks
template <unsigned long Mask1, unsigned long Mask2>
struct same_mask
{
    static const bool value= false;
};

template <unsigned long Mask>
struct same_mask<Mask, Mask>
{
    static const bool value= true;
};

/// Row-major mask for 2^K by 2^K base case
template <unsigned long K>
struct row_major_mask
{
    static const unsigned long value= lsb_mask<K>::value << K;
};

/// Column-major mask for 2^K by 2^K base case
template <unsigned long K>
struct col_major_mask
    : public lsb_mask<K>
{};

/// Checks whether 2^K by 2^K base case of hybric matrix, defined by Mask, is a row-major matrix
template <unsigned long K, unsigned long Mask>
struct is_k_power_base_case_row_major
{
    static const bool value= same_mask<lsb_bits<2*K, Mask>::value, row_major_mask<K>::value>::value;
    // typedef
};

/// Checks whether 2^K by 2^K base case of hybric matrix, defined by Mask, is a column-major matrix
template <unsigned long K, unsigned long Mask>
struct is_k_power_base_case_col_major
{
    static const bool value= same_mask<lsb_bits<2*K, Mask>::value, col_major_mask<K>::value>::value;
};

/// Checks whether 32x32 base case of hybric matrix, defined by Mask, is a row-major matrix
template <unsigned long Mask>
struct is_32_base_case_row_major
    : public is_k_power_base_case_row_major<5, Mask>
{};

/// Checks whether 32x32 base case of hybric matrix, defined by Mask, is a col-major matrix
template <unsigned long Mask>
struct is_32_base_case_col_major
    : public is_k_power_base_case_col_major<5, Mask>
{};

/// Row-major mask for 2^K by 2^K base case with 2^T shark teeth
template <unsigned long K, unsigned long T>
struct row_major_shark_mask
{
    static const unsigned long value= (lsb_mask<K-T>::value << (K+T)) | lsb_mask<T>::value;
};

/// Row-major mask for 2^K by 2^K base case with 2^T shark teeth
template <unsigned long K, unsigned long T>
struct col_major_shark_mask
{
    static const unsigned long value= lsb_mask<K>::value << T;
};

/** Checks whether 2^K by 2^K base case of hybric matrix, defined by Mask,
    is a row-major matrix shark-tooth with 2^T tooth length
**/
template <unsigned long K, unsigned long T, unsigned long Mask>
struct is_k_power_base_case_row_major_t_shark
{
    static const bool value= same_mask<lsb_bits<2*K, Mask>::value, row_major_shark_mask<K, T>::value>::value;
};

/** Checks whether 2^K by 2^K base case of hybric matrix, defined by Mask,
    is a col-major matrix shark-tooth with 2^T tooth length
**/
template <unsigned long K, unsigned long T, unsigned long Mask>
struct is_k_power_base_case_col_major_t_shark
{
    static const bool value= same_mask<lsb_bits<2*K, Mask>::value, col_major_shark_mask<K, T>::value>::value;
};

  // e-order
/// N-order mask of N bits
template <unsigned long N>
struct i_order_mask
{
    // Check if N is even !!!
    static const unsigned long value= (i_order_mask<N-2>::value << 2) | 1;
};

template<> struct i_order_mask<0> : public lsb_mask<0> {};  // set to 0

/// Z-order mask of N bits
template <unsigned long N>
struct z_order_mask
{
    // Check if N is even !!!
    static const unsigned long value= (z_order_mask<N-2>::value << 2) | 2;
};

template<> struct z_order_mask<0> : public lsb_mask<0> {};  // set to 0

/** Generate arbitrary hybrid mask.
    \param IOrder if true then i-order otherwise z-order
    \param K      2^K by 2^K base case
    \param Orientation  mtl::row_major or mtl::col_major
    \param T      2^T tooth length
**/
template <bool IOrder, unsigned long K, typename Orientation, unsigned long T>
class generate_mask
{
    static const unsigned long rec_size= 8 * sizeof(unsigned long) - 2 * K,
	rec_part= (IOrder ? i_order_mask<rec_size>::value : z_order_mask<rec_size>::value) << 2*K;
    typedef typename boost::mpl::if_<
	boost::is_same<Orientation, row_major>
      , row_major_shark_mask<K, T>
      , col_major_shark_mask<K, T>
    >::type base_part_type;
public:
    static const unsigned long value= rec_part | base_part_type::value;
};

} // namespace mtl

#endif // MTL_BIT_MASKING_INCLUDE


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_BENCHMARK_LONESTAR_AGGLOMERATIVECLUSTERING_HPP
#define STAPL_BENCHMARK_LONESTAR_AGGLOMERATIVECLUSTERING_HPP

#include <iostream>
#include <sstream>
#include <map>

#include <stapl/utility/do_once.hpp>
#include <stapl/containers/array/array.hpp>
#include <stapl/views/array_view.hpp>

#include <stapl/containers/graph/hierarchical_graph.hpp>
#include <stapl/containers/graph/views/hgraph_view.hpp>
#include <stapl/containers/graph/dynamic_graph.hpp>
#include <stapl/containers/graph/graph.hpp>
#include <stapl/containers/graph/views/graph_view.hpp>
#include <stapl/containers/graph/algorithms/hierarchical_view.hpp>
#include <stapl/views/repeated_view.hpp>
#include <stapl/views/native_view.hpp>
#include <stapl/containers/graph/algorithms/create_level.hpp>
#include <stapl/containers/graph/algorithms/graph_io.hpp>
#include <stapl/containers/graph/views/property_maps.hpp>

#include <stapl/algorithms/algorithm.hpp>
#include <stapl/skeletons/utility/tags.hpp>
#include <boost/random.hpp>

using namespace stapl;

//////////////////////////////////////////////////////////////////////
/// @brief Holds the actual points to be clustered.
//////////////////////////////////////////////////////////////////////
struct my_point
{
  double m_x, m_y, m_z;

  my_point(double val=0.0)
    : m_x(val), m_y(val), m_z(val)
  { }

  void set(double x, double y, double z)
  {
    m_x = x;
    m_y = y;
    m_z = z;
  }

  void set(my_point const& other)
  {
    m_x = other.m_x;
    m_y = other.m_y;
    m_z = other.m_z;
  }

  void scale(double factor)
  {
    m_x *= factor;
    m_y *= factor;
    m_z *= factor;
  }

  void sub(my_point const& other)
  {
    m_x -= other.m_x;
    m_y -= other.m_y;
    m_z -= other.m_z;
  }

  void add(my_point const& other)
  {
    m_x += other.m_x;
    m_y += other.m_y;
    m_z += other.m_z;
  }

  double get_Magnitude() const
  {
    return m_x*m_x + m_y*m_y + m_z*m_z;
  }

  double get_SquaredEuclideanDistance(my_point const& other) const
  {
    return pow(other.m_x - m_x, 2)
         + pow(other.m_y - m_y, 2)
         + pow(other.m_z - m_z, 2);
  }

  double get_EuclideanDistance(my_point const& other) const
  {
    return sqrt(get_SquaredEuclideanDistance(other));
  }

  double get_ManhattanDistance(my_point const& other) const
  {
    return std::abs(other.m_x - m_x)
         + std::abs(other.m_y - m_y)
         + std::abs(other.m_z - m_z);
  }

  double get_DotProduct(my_point const& other) const
  {
    return other.m_x * m_x
         + other.m_y * m_y
         + other.m_z * m_z;
  }

  double get_CosineSimilarity(my_point const& other) const
  {
    return get_DotProduct(other) / (get_Magnitude() * other.get_Magnitude());
  }

  double get_Distance(my_point const& other) const
  {
    return get_SquaredEuclideanDistance(other);
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_x);
    t.member(m_y);
    t.member(m_z);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Stores the points in a full cluster, weight, and nearest neighbor.
//////////////////////////////////////////////////////////////////////
class my_vertex_property
{
private:
  /// @todo Currently only a single point average of all
  /// the true points is stored. This is naive. It will later be changed.
  my_point m_centroid;

  /// The number of points in the cluster.
  int m_count;

  /// The ID of the closest cluster.
  size_t m_my_nearest_neighbor;

public:
  typedef int property_type;

  my_vertex_property()
    : m_centroid(0.0), m_count(0), m_my_nearest_neighbor(0)
  { }

  my_vertex_property(my_point const& other)
    : m_centroid(other), m_count(1), m_my_nearest_neighbor(0)
  { }

  my_point get_Centroid() const
  {
    return m_centroid;
  }

  void set_Centroid(double new_x, double new_y, double new_z)
  {
    m_centroid.set(new_x, new_y, new_z);
  }

  void set_Centroid(my_vertex_property const& other)
  {
    m_centroid = other.m_centroid;
    m_count = other.m_count;
  }

  size_t get_nearest_neighbor() const
  {
    return m_my_nearest_neighbor;
  }

  void set_nearest_neighbor(size_t nearest_neighbor_id)
  {
    m_my_nearest_neighbor = nearest_neighbor_id;
  }

  double get_Distance(const my_vertex_property& other) const
  {
    return m_centroid.get_Distance(other.m_centroid);
  }

  int get_Count() const
  {
    return m_count;
  }

  //////////////////////////////////////////////////////////////////////
  /// @todo Merges this vertex with another and returns a
  /// new my_vertex_property. This is currently naive, using centroids.
  /// @param other The other cluster to be merged.
  /// @return my_vertex_property The new, merged, cluster.
  //////////////////////////////////////////////////////////////////////
  my_vertex_property mergeVertex(const my_vertex_property& other) const
  {
    //calculate centroid
    my_point p1(m_centroid);
    my_point p2(other.m_centroid);

    p1.scale(m_count);
    p2.scale(other.m_count);

    p1.add(p2);
    p1.scale(1.0 / (other.m_count + m_count));

    my_vertex_property vp_out(p1);
    vp_out.m_count = other.m_count + m_count;

    return vp_out;
  }

  my_vertex_property operator=(my_vertex_property const& vertex)
  {
    m_centroid = vertex.m_centroid;
    m_count = vertex.m_count;
    m_my_nearest_neighbor = vertex.m_my_nearest_neighbor;
    return my_vertex_property(vertex);
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_centroid);
    t.member(m_count);
    t.member(m_my_nearest_neighbor);
  }
};

namespace stapl
{
  STAPL_PROXY_HEADER(my_vertex_property)
  {
    STAPL_PROXY_DEFINES(my_vertex_property)
    STAPL_PROXY_METHOD_RETURN(get_Centroid, my_point)
    STAPL_PROXY_METHOD(set_Centroid, double, double, double)
    STAPL_PROXY_METHOD(set_Centroid, my_vertex_property)
    STAPL_PROXY_METHOD_RETURN(get_nearest_neighbor, size_t)
    STAPL_PROXY_METHOD(set_nearest_neighbor, size_t)
    STAPL_PROXY_METHOD_RETURN(get_Distance, my_vertex_property&, double)
    STAPL_PROXY_METHOD_RETURN(get_Count, int)
    STAPL_PROXY_METHOD_RETURN(mergeVertex, my_vertex_property,
      my_vertex_property)
    STAPL_PROXY_METHOD_RETURN(operator=, my_vertex_property,
      my_vertex_property)
    STAPL_PROXY_METHOD(define_type, stapl::typer)
  };
}

//////////////////////////////////////////////////////////////////////
/// @brief Work function to merge vertices.
//////////////////////////////////////////////////////////////////////
struct functor_merge_vertex
{
  typedef my_vertex_property result_type;

  result_type operator()(my_vertex_property& vertex1,
                         my_vertex_property const& vertex2)
  {
    return vertex1.mergeVertex(vertex2);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Creates pairs of the vertices to be merged in the next level.
//////////////////////////////////////////////////////////////////////
struct make_node_pairs
{
  typedef std::pair<size_t, double> result_type;

  my_vertex_property m_vertex_prop;
  size_t m_vertex_descriptor;

  make_node_pairs(my_vertex_property clust_prop, size_t vertex_id)
    : m_vertex_prop(clust_prop), m_vertex_descriptor(vertex_id)
  { }

  template<typename GraphNode>
  result_type operator()(GraphNode element) const
  {
    if (element.descriptor() != m_vertex_descriptor) {
      double distance =
         m_vertex_prop.get_Distance(element.property().property);
      return std::make_pair(element.descriptor(), distance);
    }
    return std::make_pair(0, 0.0);
  }

  void define_type(typer &t)
  {
    t.member(m_vertex_prop);
    t.member(m_vertex_descriptor);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Reduces two single element pairs to the smaller value.
/// Multi-element pairs are preferred.
//////////////////////////////////////////////////////////////////////
struct pick_smaller_node
{
  typedef std::pair<size_t, double> result_type;

  result_type operator()(result_type el1, result_type el2) const
  {
    const result_type null_value = result_type(0, 0.0);

    if (el1 != null_value && el2 != null_value) {
      if (el1.second < el2.second) {
        return el1;
      } else {
        return el2;
      }
    } else if (el1 == null_value) {
      return el2;
    } else {
      return el1;
    }
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Work function to determine the nearest neighbors
/// for all of the clusters.
//////////////////////////////////////////////////////////////////////
struct find_nearest_node
{
  typedef void result_type;

  find_nearest_node()
  { }

  template<typename Node, typename GraphView>
  result_type operator()(Node node, const GraphView g_view)
  {
    make_node_pairs pairs_wf(
            node.property().property, node.descriptor());
    size_t mynearestnode =
        (nc_map_reduce(pairs_wf, pick_smaller_node(), g_view)).first;
    node.property().property.set_nearest_neighbor(mynearestnode);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Gives an empty property. This is needed for create_level.
//////////////////////////////////////////////////////////////////////
struct empty_functor
{
  typedef int value_type;

  template<class Graph>
  properties::no_property operator()(Graph& g, size_t lvl) const
  {
    return properties::no_property();
  }

  template<class EdgePropertyType>
  properties::no_property operator()(EdgePropertyType& p,
                                     properties::no_property const& ref) const
  {
    return properties::no_property();
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Creates a property map of the graph, indicating which vertices are
/// to be clustered with which other vertices.
//////////////////////////////////////////////////////////////////////
struct create_property_map
{
  typedef void result_type;

  create_property_map()
  { }

  template<typename Vertex, typename PropMap, typename GraphView>
  result_type operator()
    (Vertex element, PropMap& property_map, GraphView whole_graph)
  {
    size_t element_descriptor = element.descriptor();
    size_t element_nearest_node =
      element.property().property.get_nearest_neighbor();
    if (whole_graph[element_nearest_node].property()
         .property.get_nearest_neighbor() == element_descriptor) {
      // Nodes are to be clustered
      if (element_descriptor < element_nearest_node) {
        property_map[element_descriptor] = element_descriptor;
      } else {
        property_map[element_descriptor] = element_nearest_node;
      }
    } else {
      // Node is not to be clustered
      property_map[element_descriptor] = element_descriptor;
    }
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Used to fill the graph used in @ref agglomerative_clustering
/// with starting points.
//////////////////////////////////////////////////////////////////////
struct fill_graph
{
  typedef void result_type;

  template<typename Vertex, typename Point>
  void operator()(Vertex element, Point point) {
    element.property() = my_vertex_property(point);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Wrapper class for agglomerative clustering algorithm
/// Class needed to store so as to contain typedef for return_type
//////////////////////////////////////////////////////////////////////
class agglomerative_clustering
{
private:
  typedef stapl::static_array<size_t> array_type;
  typedef array_view<array_type> array_vw;
  typedef graph<stapl::DIRECTED, stapl::MULTIEDGES,
    super_vertex_property<my_vertex_property>,
    super_edge_property<properties::no_property> > graph_type;
  typedef graph_view<graph_type> graph_vw;
  typedef graph_external_property_map<graph_vw, size_t, array_vw>
    graph_ext_prop_map;

public:
  typedef std::vector<graph_vw> return_type;

  //////////////////////////////////////////////////////////////////////
  /// @brief Agglomeratively clusters an array of points.
  /// @param inputView The view of the graph to be clustered.
  //////////////////////////////////////////////////////////////////////
  template<typename View>
  return_type cluster(View inputView) {
    graph_type* g = new graph_type(inputView.size());
    graph_vw g_vw(g);

    map_func(fill_graph(), g_vw, inputView);

    std::vector<graph_vw> g_level_vw;
    g_level_vw.push_back(g_vw);

    while (g_vw.size() > 1)
    {
      //Generates the array of nearest neighbors
      nc_map_func(find_nearest_node(), g_vw, make_repeat_view(g_vw));

      array_type group_array(g_vw.size());
      array_vw group_array_vw(group_array);
      nc_map_func(create_property_map(), g_vw, make_repeat_view(group_array_vw),
        make_repeat_view(g_vw));

      graph_ext_prop_map group_id_prop_map(g_vw, group_array_vw);

      g_level_vw.push_back(create_level(g_vw, group_id_prop_map,
        functor_merge_vertex(), empty_functor()));

      g_vw = g_level_vw.back();
    }
    return g_level_vw;
  }

  #ifdef SHOW_RESULTS
  //////////////////////////////////////////////////////////////////////
  /// @brief Converts a point to its RGB hex representation.
  //////////////////////////////////////////////////////////////////////
  std::string ConvertPointToRGBColor(my_point const& point) {
    char b[10];
    sprintf(&b[0], "%02X", (unsigned char)(point.m_x * 255));
    sprintf(&b[2], "%02X", (unsigned char)(point.m_y * 255));
    sprintf(&b[4], "%02X", (unsigned char)(point.m_z * 255));
    return std::string(b);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Writes the graph to a dot file (colors points using RGB)
  /// @todo Make more efficient version that does not redundantly build strings
  /// during the creation of dot file.
  //////////////////////////////////////////////////////////////////////
  template<typename Clusters>
  void printgraph2dotfile(Clusters clusters, char const* file="cluster.dot")
  {
    std::map<size_t, std::map<size_t, std::string> > mymap;

    do_once([&clusters, &mymap, file, this](void) {
      for (size_t lvl=0; lvl<clusters.size(); ++lvl) {
        for (size_t n=0; n<clusters[lvl].size(); ++n) {
          auto v = clusters[lvl][n];
          int v_id = v.descriptor();
          if (v.property().children.size() == 1) {
            mymap[lvl][v_id] =  mymap[lvl-1][v.property().children[0]];
            continue;
          }
          std::stringstream ss;
          ss << "subgraph cluster_" << lvl << "_" << v_id << " {\n";

          for (size_t k=0; k<v.property().children.size(); ++k) {
            ss << mymap[lvl-1][v.property().children[k]] << "\n";
          }
          if (lvl != 0) {
            ss << "label = \"";
          } else {
            ss << "\"[" << v_id<< "]\\n";
          }
          my_point p = v.property().property.get_Centroid();
          ss << p.m_x << "\\n" << p.m_y << "\\n" << p.m_z << "\";\n";
          ss << "style=\"filled\";fillcolor=\"#";
          ss << ConvertPointToRGBColor(p) << "\";\n}\n";

          mymap[lvl][v_id] = ss.str();
        }
      }
      std::ofstream ofile;
      ofile.open(file, std::fstream::out);
      ofile << "digraph pGraph {\n";
      ofile << mymap[clusters.size()-1][0];
      ofile << "}\n";
      ofile.close();
    });
    rmi_fence();
  }
  #endif
};

#endif /* STAPL_BENCHMARK_LONESTAR_AGGLOMERATIVECLUSTERING_HPP */