/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/* matmul1D.cpp - Data-movement operations on arrays of slots
 */
#include <algorithm>
#include <NTL/BasicThreadPool.h>
#include "EncryptedArray.h"
#include "matmul.h"
//#include "multiAutomorph.h"

#if 0

// Translate a value x in Zm* to the index e s.t. g_i^e=x (mod m)
// in a native dimension, returns e in 0..ord_i-1
// in a non-native dimension, returns e in -ord_i+1..ord_i-1
// returns -ord_i if it can't find such an e

// For now, this is just done by brute-force search, which is
// probably good enough.
static
long val2index(const PAlgebra& zMStar, long dim, long x)
{
  FHE_TIMER_START;

  long m = zMStar.getM();
  long g = zMStar.ZmStarGen(dim);
  long ord = zMStar.OrderOf(dim);

  long e = 0;
  long g2e = 1;

  mulmod_precon_t gminv = PrepMulModPrecon(g, m);

  while (e < ord && g2e != x) {
    e++;
    g2e = MulModPrecon(g2e, g, m, gminv);
  }

  if (e < ord) return e;

  if (zMStar.SameOrd(dim)) return -ord;

  x = MulMod(x, g2e, m);

  e = 0;
  g2e = 1;

  while (e < ord && g2e != x) {
    e++;
    g2e = MulModPrecon(g2e, g, m, gminv);
  }

  if (e < ord) return e-ord;

  return -ord;

}

// A class that implements the basic (sparse) 1D matrix-vector functions
template<class type> class matmul1D_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  MatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  matmul1D_impl(MatMulBase& _mat, MatrixCacheType tag, long dim)
    : buildCache(tag), mat(dynamic_cast< MatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    long D = ea.sizeOfDimension(dim);
    long sz = ea.nativeDimension(dim) ? D : (2*D-1);

    if (buildCache==cachezzX)
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE,sz));
    else if (buildCache==cacheDCRT)
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE,sz));
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. All blocks use the same transofmration.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal1(zzX& cPoly, long dim, long i, long D)
  {
    vector<RX> tmpDiag(D);
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    for (long j = 0; j < D; j++) { // process entry j
      bool zEntry = mat.get(entry, mcMod(j-i, D), j); // entry [j-i mod D, j]
      assert(zEntry || deg(entry) < ea.getDegree());
      // get(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry = true;// zero is an empty entry too

      if (!zEntry) {   // not a zero entry
        zDiag = false; // mark diagonal as non-empty

        // clear entries between last nonzero entry and this one
        for (long jj = nzLast+1; jj < j; jj++) clear(tmpDiag[jj]);
        tmpDiag[j] = entry;
        nzLast = j;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < D; jj++) clear(tmpDiag[jj]);

    vector<RX> diag(ea.size());
    if (D==1) diag.assign(ea.size(), tmpDiag[0]); // dimension of size one
    else for (long j = 0; j < ea.size(); j++)
           diag[j] = tmpDiag[ ea.coordinate(dim,j) ];
           // rearrange the indexes based on the current dimension

    ea.encode(cPoly, diag);
    return false; // a nonzero diagonal
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. Different blocks use different transofmrations.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal2(zzX& poly, long dim, long idx, long D)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    long blockIdx, innerIdx;
    vector<RX> diag(ea.size());
    for (long j=0; j<ea.size(); j++) {
      if (D==1) {
	blockIdx=j; innerIdx = 0;
      } else {
	std::tie(blockIdx, innerIdx) // std::pair<long,long> idxes
	  = ea.getContext().zMStar.breakIndexByDim(j, dim);
	//	blockIdx = idxes.first;  // which transformation
	//	innerIdx = idxes.second; // index along diemnssion dim
      }
      // process entry j
      bool zEntry=mat.multiGet(entry,mcMod(innerIdx-idx,D),innerIdx,blockIdx);
      // entry [i,j-i mod D] in the block corresponding to blockIdx
      // multiGet(...) returns true if the entry is empty, false otherwise

      // If non-zero, make sure the degree is not too large
      assert(zEntry || deg(entry) < ea.getDegree());

      if (!zEntry && IsZero(entry)) zEntry = true; // zero is an empty entry too

      if (!zEntry) {   // not a zero entry
	zDiag = false; // mark diagonal as non-empty

	// clear entries between last nonzero entry and this one
	for (long jj = nzLast+1; jj < j; jj++) clear(diag[jj]);
	nzLast = j;
	diag[j] = entry;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < ea.size(); jj++) clear(diag[jj]);
    ea.encode(poly, diag);
    return false; // a nonzero diagonal
  }

  void fixup(zzX& cpoly, const RX& c, long i, long signed_i, long dim)
  {
    const PAlgebraModDerived<type>& tab = ea.getTab();
    const vector< vector< RX > >& maskTable = tab.getMaskTable();
    const RXModulus& PhimXMod = tab.getPhimXMod();

    long D = ea.sizeOfDimension(dim);

    if (signed_i > 0) {
      RX mask = maskTable[dim][i];
      MulMod(mask, mask, c, PhimXMod);
      convert(cpoly, mask);
    }
    else {
      RX mask;
      NTL::negate(mask, maskTable[dim][i]);
      MulMod(mask, mask, c, PhimXMod);
      add(mask, mask, c);
      convert(cpoly, mask);
    }
  }

  void multiply(Ctxt* ctxt, long dim, bool oneTransform)
  {
    FHE_TIMER_START;

    assert(dim >= 0 && dim < ea.dimension());
    RBak bak; bak.save(); ea.getTab().restoreContext(); // backup NTL modulus

    std::unique_ptr<Ctxt> res, shCtxt;
    if (ctxt!=nullptr) { // we need to do an actual multiplication
      ctxt->cleanUp(); // not sure, but this may be a good idea
      res.reset(new Ctxt(ZeroCtxtLike, *ctxt));
      shCtxt.reset(new Ctxt(ZeroCtxtLike, *ctxt));
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    // set up the AutoIterator, if we have a ctxt
    std::unique_ptr<AutoIterator> autoIterator;
    if (ctxt) {
      FHE_NTIMER_START(AutoIterator_build);
      autoIterator.reset(AutoIterator::build(*ctxt, ctxt->getPubKey().getTree4dim(dim)));
    }

    // Process the diagonals one at a time
    if (ea.nativeDimension(dim)) {
      zzX cpoly;
      long D = ea.sizeOfDimension(dim);

      for (long cnt = 0; cnt < D; cnt++) { // process one diagonal
	long i; //process diagonal i

	if (!ctxt) {
	  i = cnt;
	}
	else if (cnt == 0) {
	  i = 0;
	  *shCtxt = *ctxt;
	}
	else {
          FHE_NTIMER_START(AutoIterator_next);
	  long x = autoIterator->next(*shCtxt);
	  assert(x !=0);
	  i = val2index(ea.getContext().zMStar, dim, x);
	  assert(i >= 0);
	}

	zzX* zxPtr=nullptr;
	DoubleCRT* dxPtr=nullptr;

	if (dcp != nullptr)         // DoubleCRT cache exists
	  dxPtr = (*dcp)[i].get();
	else if (zcp != nullptr)    // zzx cache exists but no DoubleCRT
	  zxPtr = (*zcp)[i].get();
	else { // no cache, compute const
	  bool zero = oneTransform? processDiagonal1(cpoly, dim, i, D)
				  : processDiagonal2(cpoly, dim, i, D);
	  if (!zero) zxPtr = &cpoly; // if it is not a zero value, point to it
	}

	// if zero diagonal, nothing to do for this iteration
	if (zxPtr==nullptr && dxPtr==nullptr)
	  continue;

	// Non-zero diagonal, store it in cache and/or multiply/add it

	if (ctxt) {
	  if (dxPtr!=nullptr) shCtxt->multByConstant(*dxPtr);
	  else                shCtxt->multByConstant(*zxPtr);

	  *res += *shCtxt;

	}

	if (buildCache==cachezzX) {
	  (*zCache)[i].reset(new zzX(*zxPtr));
	}
	else if (buildCache==cacheDCRT) {
	  (*dCache)[i].reset(new DoubleCRT(*zxPtr, ea.getContext()));
	}
      } // end of loop over diagonals
    }
    else {

      zzX cpoly;
      long D = ea.sizeOfDimension(dim);

      std::vector< unique_ptr<RX> > local_cache;
      if (!dcp && !zcp) {
        local_cache.resize(D);
      }

      for (long cnt = 0; cnt < 2*D-1; cnt++) { // process one diagonal
	long i; //process diagonal i
        long signed_i;

	if (!ctxt) {
          if (cnt == 0) {
             i = 0;
             signed_i = 0;
          }
          else {
             i = (cnt+1)/2;
             signed_i = (cnt % 2) ? (i - D) : i;
          }
	}
	else if (cnt == 0) {
	  i = 0;
          signed_i = 0;
	  *shCtxt = *ctxt;
	}
	else {
	  long x = autoIterator->next(*shCtxt);
	  assert(x !=0);
	  signed_i = val2index(ea.getContext().zMStar, dim, x);
          i = signed_i;
          if (i < 0) i += D;
	}

        long i_off = signed_i + (D-1);

	zzX* zxPtr=nullptr;
	DoubleCRT* dxPtr=nullptr;

	if (dcp != nullptr)         // DoubleCRT cache exists
	  dxPtr = (*dcp)[i_off].get();
	else if (zcp != nullptr)    // zzx cache exists but no DoubleCRT
	  zxPtr = (*zcp)[i_off].get();
	else if (local_cache[i]) {
          if (*local_cache[i] != 0) {
             convert(cpoly, *local_cache[i]);
             zxPtr = &cpoly;
             if (i) fixup(cpoly, *local_cache[i], i, signed_i, dim);
          }
          local_cache[i].reset(); // we've used it, so we can kill it
        }
        else { // no cache, compute const
	  bool zero = oneTransform? processDiagonal1(cpoly, dim, i, D)
				  : processDiagonal2(cpoly, dim, i, D);
	  if (!zero) {
            zxPtr = &cpoly; // if it is not a zero value, point to it
            local_cache[i].reset(new RX());
            convert(*local_cache[i], cpoly);
            if (i) fixup(cpoly, *local_cache[i], i, signed_i, dim);
          }
          else {
            local_cache[i].reset(new RX());
          }
	}

	// if zero diagonal, nothing to do for this iteration
	if (zxPtr==nullptr && dxPtr==nullptr)
	  continue;

	// Non-zero diagonal, store it in cache and/or multiply/add it

	if (ctxt) {
	  if (dxPtr!=nullptr) shCtxt->multByConstant(*dxPtr);
	  else                shCtxt->multByConstant(*zxPtr);
	  *res += *shCtxt;
	}
	if (buildCache==cachezzX) {
	  (*zCache)[i_off].reset(new zzX(*zxPtr));
	}
	else if (buildCache==cacheDCRT) {
	  (*dCache)[i_off].reset(new DoubleCRT(*zxPtr, ea.getContext()));
	}
      } // end of loop over diagonals
    }

    if (ctxt) // copy result back to ctxt
      *ctxt = *res;

    // "install" the cache (if needed)
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void matmul1d(Ctxt* ctxt, MatMulBase& mat, long dim,
		     bool oneTransform, MatrixCacheType buildCache)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_impl<PA_GF2> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    case PA_zz_p_tag: {
      matmul1D_impl<PA_zz_p> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    default:
      throw std::logic_error("matmul1d: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4MatMul1D(MatMulBase& mat, long dim, MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, true, buildCache); }

void matMul1D(Ctxt& ctxt, MatMulBase& mat,long dim, MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, true, buildCache); }

void buildCache4MatMulti1D(MatMulBase& mat,long dim,MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, false, buildCache); }

void matMulti1D(Ctxt& ctxt,MatMulBase& mat,long dim,MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, false, buildCache); }

/********************************************************************
 ********************************************************************/
// Applying matmul to plaintext, useful for debugging

template<class type>
class matmul1D_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, MatMul<type>& mat,
		       long dim, bool oneTrans)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long D = ea.sizeOfDimension(dim);

    vector< vector<RX> > data1(n/D);
    for (long k = 0; k < n/D; k++)
      data1[k].resize(D);

    // copy the data into a vector of 1D vectors
    vector<RX>& data = pa.getData<type>();
    for (long i = 0; i < n; i++) {
      long k,j;
      std::tie(k,j) = ea.getContext().zMStar.breakIndexByDim(i, dim);
      data1[k][j] = data[i];       // k= along dim, j = the rest of i
    }

    // multiply each one of the vectors by the same matrix
    for (long k = 0; k < n/D; k++) {
      for (long j = 0; j < D; j++) { // simple matrix-vector multiplication
	std::pair<long,long> p(k,j);
	long idx = ea.getContext().zMStar.assembleIndexByDim(p, dim);

	RX acc, val, tmp;
	acc = 0;
        for (long i = 0; i < D; i++) {
          bool zero = oneTrans? mat.get(val, i, j) : mat.multiGet(val,i,j,k);
          if (!zero) {
            NTL::mul(tmp, data1[k][i], val);
            NTL::add(acc, acc, tmp);
          }
        }
        rem(data[idx], acc, ea.getG()); // store the result in the data array
      }
    }
  }
};
// A wrapper around the implementation class
static void matmul1d(NewPlaintextArray& pa, MatMulBase& mat,
		     long dim, bool oneTrans)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_pa_impl<PA_GF2>::multiply(pa,
                      dynamic_cast< MatMul<PA_GF2>& >(mat), dim, oneTrans);
      return;
    }
    case PA_zz_p_tag: {
      matmul1D_pa_impl<PA_zz_p>::multiply(pa,
                      dynamic_cast<MatMul<PA_zz_p>&>(mat), dim, oneTrans);
      return;
    }
    default:
      throw std::logic_error("matMul1D: neither PA_GF2 nor PA_zz_p");
  }
}

void matMul1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, true);
}
void matMulti1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, false);
}

#else

// baby-step giant-step implementation

struct PtxtPtr {
  enum Type { ZERO=0, ZZX=1, DCRT=2 };
  zzX*       zp;
  DoubleCRT* dp;

  PtxtPtr(): zp(nullptr), dp(nullptr) {}
};

// A class that implements the basic (sparse) 1D matrix-vector functions
template<class type> class matmul1D_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  MatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  matmul1D_impl(MatMulBase& _mat, long dim, MatrixCacheType tag)
    : buildCache(tag), mat(dynamic_cast< MatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    if (buildCache==cachezzX) {
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE,ea.sizeOfDimension(dim)));
    } else if (buildCache==cacheDCRT) {
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE,ea.sizeOfDimension(dim)));
    }
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. All blocks use the same transofmration.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal1(zzX& cPoly, long dim, long i, long D, long rotAmt)
  {
    vector<RX> tmpDiag(D);
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    for (long j = 0; j < D; j++) { // process entry j
      long rotJ = (j+rotAmt) % D;  // need to rotate constant by rotAmt
      bool zEntry = mat.get(entry, mcMod(rotJ-i, D), rotJ); // entry [j-i mod D, j]
      assert(zEntry || deg(entry) < ea.getDegree());
      // get(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry = true;// zero is an empty entry too

      if (!zEntry) {   // not a zero entry
        zDiag = false; // mark diagonal as non-empty

        // clear entries between last nonzero entry and this one
        for (long jj = nzLast+1; jj < j; jj++) clear(tmpDiag[jj]);
        tmpDiag[j] = entry;
        nzLast = j;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < D; jj++) clear(tmpDiag[jj]);

    vector<RX> diag(ea.size());
    if (D==1) diag.assign(ea.size(), tmpDiag[0]); // dimension of size one
    else for (long j = 0; j < ea.size(); j++)
           diag[j] = tmpDiag[ ea.coordinate(dim,j) ];
           // rearrange the indexes based on the current dimension

    ea.encode(cPoly, diag);
    return false; // a nonzero diagonal
  }

  // Get the i'th diagonal along dimension dim, encoded as a
  // single constant. Different blocks use different transofmrations.
  // Returns true if this is a zero diagonal, false otherwise
  bool processDiagonal2(zzX& poly, long dim, long idx, long D, long rotAmt)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry
    RX entry;

    // Process the entries in this diagonal one at a time
    long blockIdx, innerIdx;
    vector<RX> diag(ea.size());
    for (long j=0; j<ea.size(); j++) {
      if (D==1) {
	blockIdx=j; innerIdx = 0;
      } else {
	std::tie(blockIdx, innerIdx) // std::pair<long,long> idxes
	  = ea.getContext().zMStar.breakIndexByDim(j, dim);
	//	blockIdx = idxes.first;  // which transformation
	//	innerIdx = idxes.second; // index along diemnssion dim
        innerIdx = (innerIdx+rotAmt) % D;  // need to rotate constant by rotAmt
      }
      // process entry j
      bool zEntry=mat.multiGet(entry,mcMod(innerIdx-idx,D),innerIdx,blockIdx);
      // entry [i,j-i mod D] in the block corresponding to blockIdx
      // multiGet(...) returns true if the entry is empty, false otherwise

      // If non-zero, make sure the degree is not too large
      assert(zEntry || deg(entry) < ea.getDegree());

      if (!zEntry && IsZero(entry)) zEntry = true; // zero is an empty entry too

      if (!zEntry) {   // not a zero entry
	zDiag = false; // mark diagonal as non-empty

	// clear entries between last nonzero entry and this one
	for (long jj = nzLast+1; jj < j; jj++) clear(diag[jj]);
	nzLast = j;
	diag[j] = entry;
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < ea.size(); jj++) clear(diag[jj]);

    ea.encode(poly, diag);
    return false; // a nonzero diagonal
  }

  // Returns in vec all the constants needed for a single giant-step.
  // The return value is ZERO(=0) if all these constants are zero.
  // Otherwise it is ZZX(=1) if using zzX or DCRT(=2) if using DoubleCRT
  PtxtPtr::Type getConsts(std::vector<PtxtPtr>& vec, std::vector<zzX>& polys,
                          CachedzzxMatrix* zcp, CachedDCRTMatrix* dcp,
                          long dim, long jg, long D, bool oneTrans)
  {
    PtxtPtr::Type typ = PtxtPtr::ZERO;
    long g = polys.size(); // how many constants do we need
    vec.assign(g,PtxtPtr());

    if (dcp!=nullptr) {        // we have DCRT cache
      for (long i=0, idx=jg; i<g && idx<D; i++, idx++) {
        if ((vec[i].dp = (*dcp)[idx].get()) != nullptr)
          typ = PtxtPtr::DCRT;
      }
    } else if (zcp!=nullptr) { // we have ZZX cache
      for (long i=0, idx=jg; i<g && idx<D; i++, idx++) {
        if ((vec[i].zp = (*zcp)[idx].get()) != nullptr)
          typ = PtxtPtr::ZZX;
      }
    } else {                   // no cache, compute consts
      for (long i=0, idx=jg; i<g && idx<D; i++, idx++) {
        bool zero = oneTrans? processDiagonal1(polys[i], dim, idx, D, i)
                            : processDiagonal2(polys[i], dim, idx, D, i);
        if (!zero) {
          vec[i].zp = &(polys[i]); // if not a zero, point to it
          typ = PtxtPtr::ZZX;
        }
      }
    }
    return typ;
  }

  void multiply(Ctxt* ctxt, long dim, bool oneTransform)
  {
    assert(dim >= 0 && dim <= ea.dimension());
    RBak bak; bak.save(); ea.getTab().restoreContext(); // backup NTL modulus

    long D = (dim==ea.dimension())? 1 : ea.sizeOfDimension(dim);
    long g = mat.getGstep();
    if (g<1 || g>=D) g=1; // sanity check
    long dDivg = divc(D,g);

    // Process the diagonals in baby-step/giant-step ordering.
    //   sum_{i=0}^{d-1} const_i rot^i(X)
    //   = \sum_{i=0}^{g-1} \sum_{j=0}^{d/g -1} const_{i+g*j} rot^{i+g*j}(X)
    //   = \sum_{i=0}^{g-1} rot^i(sum_j rot^{-i}(const_{i+g*j}) rot^{g*j}(X))
    //
    // so for i=0..g-1 we let
    //    Y_i = sum_j rot^{-i}(const_{i+g*j}) rot^{g*j}(X)
    // then compute \sum_{i=0}^{g-1} rot^i(Y_i).
    //
    // Computing the Y_i's, we initialize an accumulator for each Y_i,
    // then compute the rotations X_j = rot^{g*j}(X), j=0,...,d/g-1.
    // Each X_j is multiplied by all the constants rot^{-i}(const_{i+g*j}),
    // i=0,...,g-1, and the i'th product is added to the accumulator for
    // the corresponding Y_i.

    long lastRotate = 0;
    std::vector<Ctxt> acc; // accumulators
    if (ctxt!=nullptr) {   // we need to do an actual multiplication
      ctxt->cleanUp();     // not sure, but this may be a good idea
      Ctxt tmp(ZeroCtxtLike, *ctxt);
      acc.resize(g, tmp);
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    // Process the diagonals in giant-step/baby-step order
    std::vector<zzX> cpolys(g); // scratch space for encoding consts
    std::vector<PtxtPtr> ptrs;  // pointers to these constants
    for (long j = 0; j < dDivg; j++) { // giant steps
      long jg = j*g;            // beginning index of this giant step

      // get all the constants rot^{-i}(const_{i+g*j}) for this step
      PtxtPtr::Type ty = getConsts(ptrs, cpolys, zcp, dcp,
                                   dim, jg, D, oneTransform);

      if (ty==PtxtPtr::ZERO) continue; // all consts are zero

      // Store constants in cache and/or multiply/add them

      if (ctxt!=nullptr) {  // rotate by jg, multiply & add
        Ctxt shCtxt(*ctxt); // temporary to hold current rot^{g*j}(X)

        if (j>0) {
          shCtxt = *ctxt;
          ea.rotate1D(shCtxt, dim, jg); // rotate by i
	} // if j==0 we already have *shCtxt == *ctxt

        if (ty==PtxtPtr::DCRT) for (long i=0; i<min(g,D-jg); i++) {
            if (ptrs[i].dp!=nullptr) {
              Ctxt tmp(shCtxt);
              tmp.multByConstant(*(ptrs[i].dp));
              acc[i] += tmp;
            }
          }
        else /*ty==PtxtPtr::ZZX*/ for (long i=0; i<min(g,D-jg); i++) {
            if (ptrs[i].zp!=nullptr) {
              Ctxt tmp(shCtxt);
              tmp.multByConstant(*(ptrs[i].zp));
              acc[i] += tmp;
            }
          }
      }

      if (buildCache==cachezzX) for (long i=0; i<min(g,D-jg); i++) {
          if (ptrs[i].zp!=nullptr)
            (*zCache)[i+jg].reset(new zzX(*(ptrs[i].zp)));
        }
      else if (buildCache==cacheDCRT) for (long i=0; i<min(g,D-jg); i++) {
          if (ptrs[i].zp!=nullptr)
            (*dCache)[i+jg].reset(new DoubleCRT(*(ptrs[i].zp),ea.getContext()));
        }
      // end of giant-step loop
    }
    // Compute the result as \sum_{i=0}^{g-1} rho^i(Y_i)
    if (ctxt!=nullptr) {
      *ctxt = acc[0];
      for (long i = 1; i < g; i++) {
        ea.rotate1D(acc[i], dim, i);
        *ctxt += acc[i];
      }
    }
    // "install" the cache if needed
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void matmul1d(Ctxt* ctxt, MatMulBase& mat, long dim, bool oneTransform,
                     MatrixCacheType buildCache)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_impl<PA_GF2> M(mat, dim, locking.getType());
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    case PA_zz_p_tag: {
      matmul1D_impl<PA_zz_p> M(mat, dim, locking.getType());
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    default:
      throw std::logic_error("matmul1d: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4MatMul1D(MatMulBase& mat, long dim,
                         MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, true, buildCache); }

void matMul1D(Ctxt& ctxt, MatMulBase& mat,long dim,
              MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, true, buildCache); }

void buildCache4MatMulti1D(MatMulBase& mat,long dim,
                           MatrixCacheType buildCache)
{ matmul1d(nullptr, mat, dim, false, buildCache); }

void matMulti1D(Ctxt& ctxt,MatMulBase& mat,long dim,
                MatrixCacheType buildCache)
{ matmul1d(&ctxt, mat, dim, false, buildCache); }

/********************************************************************
 ********************************************************************/
// Applying matmul to plaintext, useful for debugging

template<class type>
class matmul1D_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, MatMul<type>& mat,
		       long dim, bool oneTrans)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long D = ea.sizeOfDimension(dim);

    vector< vector<RX> > data1(n/D);
    for (long k = 0; k < n/D; k++)
      data1[k].resize(D);

    // copy the data into a vector of 1D vectors
    vector<RX>& data = pa.getData<type>();
    for (long i = 0; i < n; i++) {
      long k,j;
      std::tie(k,j) = ea.getContext().zMStar.breakIndexByDim(i, dim);
      data1[k][j] = data[i];       // k= along dim, j = the rest of i
    }

    // multiply each one of the vectors by the same matrix
    for (long k = 0; k < n/D; k++) {
      for (long j = 0; j < D; j++) { // simple matrix-vector multiplication
	std::pair<long,long> p(k,j);
	long idx = ea.getContext().zMStar.assembleIndexByDim(p, dim);

	RX acc, val, tmp;
	acc = 0;
        for (long i = 0; i < D; i++) {
          bool zero = oneTrans? mat.get(val, i, j) : mat.multiGet(val,i,j,k);
          if (!zero) {
            NTL::mul(tmp, data1[k][i], val);
            NTL::add(acc, acc, tmp);
          }
        }
        rem(data[idx], acc, ea.getG()); // store the result in the data array
      }
    }
  }
};
// A wrapper around the implementation class
static void matmul1d(NewPlaintextArray& pa, MatMulBase& mat,
		     long dim, bool oneTrans)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      matmul1D_pa_impl<PA_GF2>::multiply(pa,
                      dynamic_cast< MatMul<PA_GF2>& >(mat), dim, oneTrans);
      return;
    }
    case PA_zz_p_tag: {
      matmul1D_pa_impl<PA_zz_p>::multiply(pa,
                      dynamic_cast<MatMul<PA_zz_p>&>(mat), dim, oneTrans);
      return;
    }
    default:
      throw std::logic_error("matMul1D: neither PA_GF2 nor PA_zz_p");
  }
}

void matMul1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, true);
}
void matMulti1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  matmul1d(pa, mat, dim, false);
}

#endif


//
//! Copyright © 2017
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_ARITHMETIC_MATRIX_TRACE_HPP
#define GEOMETRIX_ARITHMETIC_MATRIX_TRACE_HPP

#include <geometrix/tensor/matrix_traits.hpp>
#include <boost/concept/assert.hpp>

namespace geometrix {

    namespace detail{

        template<std::size_t I, typename Sum, typename Matrix>
        struct trace_sum
        {
        private:
            using next_t = trace_sum<I-1, decltype(std::declval<typename type_at<Matrix, I-1, I-1>::type>() + std::declval<Sum>()), Matrix>;
        public:

            using type = typename next_t::type;

            static type apply(const Matrix& m, const Sum& s)
            {
                return next_t::apply(m, s + get<I-1,I-1>(m));
            }
        };

        template<typename Sum, typename Matrix>
        struct trace_sum<0, Sum, Matrix>
        {
            using type = decltype(std::declval<typename type_at<Matrix, 0, 0>::type>() + std::declval<Sum>());

            static type apply(const Matrix&, const Sum& s)
            {
                return s;
            }
        };
    }//! namespace detail;

    //! Trace of a Matrix - Sum of the diagonals.
    namespace result_of {

        template <typename Matrix>
        struct trace
            : ::geometrix::detail::trace_sum
              <
                geometric_traits<Matrix>::row_dimension::value - 2
              , typename type_at<Matrix, geometric_traits<Matrix>::row_dimension::value - 1, geometric_traits<Matrix>::row_dimension::value - 1>::type
              , Matrix
              >
        {};

    }//! namespace result_of;

    template <typename Matrix>
    inline typename result_of::trace<Matrix>::type trace(const Matrix& m)
    {
        BOOST_CONCEPT_ASSERT(( SquareMatrixConcept<Matrix> ));
        auto v = get<geometric_traits<Matrix>::row_dimension::value - 1,geometric_traits<Matrix>::row_dimension::value - 1>(m);
        return detail::trace_sum<geometric_traits<Matrix>::row_dimension::value - 1, decltype(v), Matrix>::apply(m, v);
    }

}//namespace geometrix;

#endif//GEOMETRIX_ARITHMETIC_MATRIX_TRACE_HPP


#include "plane_fitting.h"

#include <Eigen/Dense>
#include "util/mirrored_memory.h"

namespace dart {

void fitPlane(float3 & planeNormal,
              float & planeIntercept,
              const float4 * dObsVertMap,
              const float4 * dObsNormMap,
              const int width,
              const int height,
              const float distanceThreshold,
              const float normalThreshold,
              const int maxIters,
              const float regularization,
              int * dbgAssociated) {

    dart::MirroredVector<float> result(4 + 16 + 1);

    for (int iter=0; iter<maxIters; ++iter) {

        cudaMemset(result.devicePtr(),0,(1+4+16)*sizeof(float));

        fitPlaneIter(planeNormal,
                     planeIntercept,
                     dObsVertMap,
                     dObsNormMap,
                     width,
                     height,
                     distanceThreshold,
                     normalThreshold,
                     regularization,
                     result.devicePtr(),
                     dbgAssociated);

        result.syncDeviceToHost();

        Eigen::MatrixXf JTJ = regularization*Eigen::MatrixXf::Identity(4,4);
        Eigen::VectorXf eJ = Eigen::VectorXf::Zero(4,1);

        for (int i=0; i<4; ++i) {
            eJ(i) = result.hostPtr()[i];
            for(int j=0; j<4; ++j) {
                JTJ(i,j) += result.hostPtr()[4 + j + i*4];
            }
        }

        //std::cout << "JTJ: \n" << JTJ << std::endl;
        //std::cout << "eJ: \n" << eJ << std::endl;

        Eigen::VectorXf update = -JTJ.ldlt().solve(eJ);

        //std::cout << "error: " << result.hostPtr()[4+16] << std::endl;
        //std::cout << "update: " << update << std::endl;

        planeNormal.x += update(0);
        planeNormal.y += update(1);
        planeNormal.z += update(2);
        planeIntercept += update(3);

        planeNormal = normalize(planeNormal);

        //std::cout << "new normal " << planeNormal.x << ", " << planeNormal.y << ", " << planeNormal.z << std::endl;
        //std::cout << "new intercept " << planeIntercept << std::endl;
    }

}

}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include <Eigen/Dense>
#include <algorithm>
#include <iterator>
#include <numeric>
#include <random>
#include <unordered_set>

#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"

namespace open3d {
namespace geometry {

/// \class RANSACResult
///
/// \brief Stores the current best result in the RANSAC algorithm.
class RANSACResult {
public:
    RANSACResult() : fitness_(0), inlier_rmse_(0) {}
    ~RANSACResult() {}

public:
    double fitness_;
    double inlier_rmse_;
};

// Calculates the number of inliers given a list of points and a plane model,
// and the total distance between the inliers and the plane. These numbers are
// then used to evaluate how well the plane model fits the given points.
RANSACResult EvaluateRANSACBasedOnDistance(
        const std::vector<Eigen::Vector3d> &points,
        const Eigen::Vector4d plane_model,
        std::vector<size_t> &inliers,
        double distance_threshold,
        double error) {
    RANSACResult result;

    for (size_t idx = 0; idx < points.size(); ++idx) {
        Eigen::Vector4d point(points[idx](0), points[idx](1), points[idx](2),
                              1);
        double distance = std::abs(plane_model.dot(point));

        if (distance < distance_threshold) {
            error += distance;
            inliers.emplace_back(idx);
        }
    }

    size_t inlier_num = inliers.size();
    if (inlier_num == 0) {
        result.fitness_ = 0;
        result.inlier_rmse_ = 0;
    } else {
        result.fitness_ = (double)inlier_num / (double)points.size();
        result.inlier_rmse_ = error / std::sqrt((double)inlier_num);
    }
    return result;
}

// Find the plane such that the summed squared distance from the
// plane to all points is minimized.
//
// Reference:
// https://www.ilikebigbits.com/2015_03_04_plane_from_points.html
Eigen::Vector4d GetPlaneFromPoints(const std::vector<Eigen::Vector3d> &points,
                                   const std::vector<size_t> &inliers) {
    Eigen::Vector3d centroid(0, 0, 0);
    for (size_t idx : inliers) {
        centroid += points[idx];
    }
    centroid /= double(inliers.size());

    double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;

    for (size_t idx : inliers) {
        Eigen::Vector3d r = points[idx] - centroid;
        xx += r(0) * r(0);
        xy += r(0) * r(1);
        xz += r(0) * r(2);
        yy += r(1) * r(1);
        yz += r(1) * r(2);
        zz += r(2) * r(2);
    }

    double det_x = yy * zz - yz * yz;
    double det_y = xx * zz - xz * xz;
    double det_z = xx * yy - xy * xy;

    Eigen::Vector3d abc;
    if (det_x > det_y && det_x > det_z) {
        abc = Eigen::Vector3d(det_x, xz * yz - xy * zz, xy * yz - xz * yy);
    } else if (det_y > det_z) {
        abc = Eigen::Vector3d(xz * yz - xy * zz, det_y, xy * xz - yz * xx);
    } else {
        abc = Eigen::Vector3d(xy * yz - xz * yy, xy * xz - yz * xx, det_z);
    }

    double norm = abc.norm();
    // Return invalid plane if the points don't span a plane.
    if (norm == 0) {
        return Eigen::Vector4d(0, 0, 0, 0);
    }
    abc /= abc.norm();
    double d = -abc.dot(centroid);
    return Eigen::Vector4d(abc(0), abc(1), abc(2), d);
}

std::tuple<Eigen::Vector4d, std::vector<size_t>> PointCloud::SegmentPlane(
        const double distance_threshold /* = 0.01 */,
        const int ransac_n /* = 3 */,
        const int num_iterations /* = 100 */) const {
    RANSACResult result;
    double error = 0;

    // Initialize the plane model ax + by + cz + d = 0.
    Eigen::Vector4d plane_model = Eigen::Vector4d(0, 0, 0, 0);
    // Initialize the best plane model.
    Eigen::Vector4d best_plane_model = Eigen::Vector4d(0, 0, 0, 0);

    // Initialize consensus set.
    std::vector<size_t> inliers;

    size_t num_points = points_.size();
    std::vector<size_t> indices(num_points);
    std::iota(std::begin(indices), std::end(indices), 0);

    std::random_device rd;
    std::mt19937 rng(rd());

    // Return if ransac_n is less than the required plane model parameters.
    if (ransac_n < 3) {
        utility::LogError(
                "ransac_n should be set to higher than or equal to 3.");
        return std::make_tuple(best_plane_model, inliers);
    }
    if (num_points < size_t(ransac_n)) {
        utility::LogError("There must be at least 'ransac_n' points.");
        return std::make_tuple(best_plane_model, inliers);
    }

    for (int itr = 0; itr < num_iterations; itr++) {
        for (int i = 0; i < ransac_n; ++i) {
            std::swap(indices[i], indices[rng() % num_points]);
        }
        inliers.clear();
        for (int idx = 0; idx < ransac_n; ++idx) {
            inliers.emplace_back(indices[idx]);
        }

        // Fit model to num_model_parameters randomly selected points among the
        // inliers.
        plane_model = TriangleMesh::ComputeTrianglePlane(
                points_[inliers[0]], points_[inliers[1]], points_[inliers[2]]);
        if (plane_model.isZero(0)) {
            continue;
        }

        error = 0;
        inliers.clear();
        auto this_result = EvaluateRANSACBasedOnDistance(
                points_, plane_model, inliers, distance_threshold, error);
        if (this_result.fitness_ > result.fitness_ ||
            (this_result.fitness_ == result.fitness_ &&
             this_result.inlier_rmse_ < result.inlier_rmse_)) {
            result = this_result;
            best_plane_model = plane_model;
        }
    }

    // Find the final inliers using best_plane_model.
    inliers.clear();
    for (size_t idx = 0; idx < points_.size(); ++idx) {
        Eigen::Vector4d point(points_[idx](0), points_[idx](1), points_[idx](2),
                              1);
        double distance = std::abs(best_plane_model.dot(point));

        if (distance < distance_threshold) {
            inliers.emplace_back(idx);
        }
    }

    // Improve best_plane_model using the final inliers.
    best_plane_model = GetPlaneFromPoints(points_, inliers);

    utility::LogDebug("RANSAC | Inliers: {:d}, Fitness: {:e}, RMSE: {:e}",
                      inliers.size(), result.fitness_, result.inlier_rmse_);
    return std::make_tuple(best_plane_model, inliers);
}

}  // namespace geometry
}  // namespace open3d


#include <body_interact_game/body_analizer.hpp>

#include <Eigen/SVD>

namespace
{

constexpr auto pi {3.141592653589793};

inline Eigen::AngleAxisd get_yaw_inverse_matrix(Eigen::Affine3d pos) noexcept
{
  const auto ypr {pos.rotation().eulerAngles(2, 0, 1)};
  const auto yaw_angle {ypr(0) < pi / 2 ? ypr(2) : ypr(2) - pi};
  return {-yaw_angle, Eigen::Vector3d::UnitY()};
}

}

body_analizer::body_analizer(std::string root, std::size_t target_number)
  : root_ {root},
    target_number_ {std::to_string(target_number)},
    base_name_ {"torso_" + target_number_},
    right_hand_name_ {"right_hand_" + target_number_},
    left_hand_name_ {"left_hand_" + target_number_},
    right_knee_name_ {"right_knee_" + target_number_},
    left_knee_name_ {"left_knee_" + target_number_}
{
}

void body_analizer::update()
{
  const auto current_time {ros::Time{0}};
  const auto base_pos {get_position(base_name_, current_time)};
  const auto yaw_canceller {get_yaw_inverse_matrix(base_pos)};
  head_ = yaw_canceller * base_pos.rotation() * Eigen::Vector3d::UnitY();
  const auto base_vec {base_pos.translation()};
  right_hand_ = yaw_canceller * (get_position(right_hand_name_, current_time).translation() - base_vec);
  left_hand_ = yaw_canceller * (get_position(left_hand_name_, current_time).translation() - base_vec);
  right_knee_ = yaw_canceller * (get_position(right_knee_name_, current_time).translation() - base_vec);
  left_knee_ = yaw_canceller * (get_position(left_knee_name_, current_time).translation() - base_vec);
}


#include <utility>

#include <boost/numeric/ublas/matrix.hpp>

#include "total_unimodularity.hpp"

namespace tu
{

  /**
   * Tests if the given matrix contains only -1,0,+1 entries,
   * returning the violating position if this is not the case.
   *
   * @param matrix The given matrix
   * @param position Returns a violating entry
   * @return true if and only if it is a -1,0,+1 matrix
   */

  bool is_zero_plus_minus_one_matrix(const integer_matrix& matrix, std::pair <integer_matrix::size_type, integer_matrix::size_type>& position)
  {
    for (size_t row = 0; row < matrix.size1(); ++row)
    {
      for (size_t column = 0; column < matrix.size2(); ++column)
      {
        const int value = matrix(row, column);
        if (value < -1 || value > 1)
        {
          position.first = row;
          position.second = column;
          return false;
        }
      }
    }

    return true;
  }

  /**
   * Tests if the given matrix contains only -1,0,+1 entries.
   *
   * @param matrix The given matrix
   * @return true if and only if it is a -1,0,+1 matrix
   */

  bool is_zero_plus_minus_one_matrix(const integer_matrix& matrix)
  {
    std::pair <size_t, size_t> result;

    return is_zero_plus_minus_one_matrix(matrix, result);
  }

  /**
   * Tests if the given matrix contains only 0 or 1 entries,
   * returning the violating position if this is not the case.
   *
   * @param matrix The given matrix
   * @param position Returns a violating entry
   * @return true if and only if it is a 0-1 matrix
   */

  bool is_zero_one_matrix(const integer_matrix& matrix, std::pair <integer_matrix::size_type, integer_matrix::size_type>& position)
  {
    for (size_t i = 0; i < matrix.size1(); ++i)
    {
      for (size_t j = 0; j < matrix.size2(); ++j)
      {
        const int value = matrix(i, j);
        if (value < 0 || value > 1)
        {
          position .first = i;
          position.second = j;
          return false;
        }
      }
    }

    return true;
  }

  /**
   * Tests if the given matrix contains only 0 or 1 entries.
   *
   * @param matrix The given matrix
   * @return true if and only if it is a 0-1 matrix
   */

  bool is_zero_one_matrix(const integer_matrix& matrix)
  {
    std::pair <size_t, size_t> result;

    return is_zero_one_matrix(matrix, result);
  }

} /* namespace tu */


/*
   Copyright (c) Marshall Clow 2014.

   Distributed under the Boost Software License, Version 1.0. (See accompanying
   file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

 Revision history:
    2 Dec 2014 mtc First version; power

*/

/// \file algorithm.hpp
/// \brief Misc Algorithms
/// \author Marshall Clow
///

#ifndef BOOST_ALGORITHM_HPP
#define BOOST_ALGORITHM_HPP

#include <functional> // for plus and multiplies

#include <boost/utility/enable_if.hpp> // for boost::disable_if
#include <boost/type_traits/is_integral.hpp>

namespace boost { namespace algorithm {

template <typename T>
BOOST_CXX14_CONSTEXPR T identity_operation ( std::multiplies<T> ) { return T(1); }

template <typename T>
BOOST_CXX14_CONSTEXPR T identity_operation ( std::plus<T> ) { return T(0); }

/// \fn power ( T x, Integer n )
/// \return the value "x" raised to the power "n"
///
/// \param x     The value to be exponentiated
/// \param n     The exponent (must be >= 0)
///
//  \remark Taken from Knuth, The Art of Computer Programming, Volume 2:
//  Seminumerical Algorithms, Section 4.6.3
template <typename T, typename Integer>
BOOST_CXX14_CONSTEXPR typename boost::enable_if<boost::is_integral<Integer>, T>::type
power (T x, Integer n) {
    T y = 1; // Should be "T y{1};"
    if (n == 0) return y;
    while (true) {
        if (n % 2 == 1) {
            y = x * y;
            if (n == 1)
                return y;
            }
        n = n / 2;
        x = x * x;
        }
    return y;
    }

/// \fn power ( T x, Integer n, Operation op )
/// \return the value "x" raised to the power "n"
/// using the operation "op".
///
/// \param x     The value to be exponentiated
/// \param n     The exponent (must be >= 0)
/// \param op    The operation used
///
//  \remark Taken from Knuth, The Art of Computer Programming, Volume 2:
//  Seminumerical Algorithms, Section 4.6.3
template <typename T, typename Integer, typename Operation>
BOOST_CXX14_CONSTEXPR typename boost::enable_if<boost::is_integral<Integer>, T>::type
power (T x, Integer n, Operation op) {
    T y = identity_operation(op);
    if (n == 0) return y;
    while (true) {
        if (n % 2 == 1) {
            y = op(x, y);
            if (n == 1)
                return y;
            }
        n = n / 2;
        x = op(x, x);
        }
    return y;
    }

}}

#endif // BOOST_ALGORITHM_HPP


//
// Created by Сергей Кривонос on 01.09.17.
//
#include "Exponentiation.h"

#include "e.h"
#include "i.h"
#include "Infinity.h"
#include "pi.h"
#include "Integer.h"
#include "Sum.h"
#include "Product.h"

#include <cmath>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

namespace omnn{
namespace math {

    Exponentiation::Exponentiation(const Valuable& _1, const Valuable& _2)
    : base(_1,_2)
    {
        InitVars();
    }

    max_exp_t Exponentiation::getMaxVaExp(const Valuable& b, const Valuable& e)
    {
        if (e.IsInt()) {
            return b.getMaxVaExp() * e.ca();
        } else if (e.FindVa()) {
            auto i = b.getMaxVaExp();
            if (i) {
                auto _ = e;
                const Variable* v;
                while ((v = _.FindVa())) {
                    _.Eval(*v, 0);
                }
                _.optimize();
                i *= static_cast<a_int>(_);
            }
            return i;
        } else {
            auto maxVaExp = e * b.getMaxVaExp();
            if (maxVaExp.IsInt()) {
                return maxVaExp.ca();
            } else if (maxVaExp.IsSimpleFraction()) {
                auto& f = maxVaExp.as<Fraction>();
                return {f.getNumerator().ca(), f.getDenominator().ca()};
            } else if(!optimizations) {
                optimizations = true;
                maxVaExp.optimize();
                optimizations = {};
                if (maxVaExp.IsInt()) {
                    return maxVaExp.ca();
                }
            }
        }

        IMPLEMENT
    }

    max_exp_t Exponentiation::getMaxVaExp() const
    {
        return getMaxVaExp(getBase(), getExponentiation());
    }

	Valuable Exponentiation::operator -() const
    {
        return Product {*this, -1};
    }

    void Exponentiation::InitVars() {
        v.clear();
        if (ebase().IsVa())
            v[ebase().as<Variable>()] = eexp();
    }

    void Exponentiation::optimize()
    {
        if (optimized) {
            return;
        }

        if (!optimizations)
        {
            hash = ebase().Hash() ^ eexp().Hash();
            return;
        }

        ebase().optimize();
        eexp().optimize();

        if (ebase().IsExponentiation() || ebase().IsProduct())
        {
            ebase() ^= eexp();
            Become(std::move(ebase()));
            return;
        }

        if (eexp().IsSum())
        {
            auto& s = eexp().as<Sum>();
            auto sz = s.size();
            auto v = 1_v;
            for(auto it = s.begin(), e = s.end();
                it != e; )
            {
                if (it->IsInt()) {
                    v *= ebase()^*it;
                    s.Delete(it);
                }
                else
                    ++it;
            }
            if (sz != s.size()) {
                Become(*this * v);
                return;
            }
        }

        // todo : check it, comment this and try System test
//        if (ebase().IsSum() && eexp().IsFraction())
//        {
//            auto f = Fraction::cast(eexp());
//            auto& d = f->getDenominator();
//            if (d == ebase().getMaxVaExp()) {
//                auto vars = ebase().Vars();
//                if (vars.size() == 1) {
//                    auto va = *vars.begin();
//                    auto baseToSolve = ebase();
//                    baseToSolve.SetView(View::Solving);
//                    baseToSolve.optimize();
//                    auto eq = va - baseToSolve(va);
//                    auto sq = eq ^ d;
//                    auto check = ebase()/sq;
//                    if (check.IsInt() || check.IsSimpleFraction()) {
//                        ebase() = eq;
//                        eexp() = f->getNumerator();
//                    } else {
//                        // TODO : IMPLEMENT
//                    }
//                }
//            }
//        }
//
//        if (ebase().IsSum() && eexp().IsFraction())
//        {
//            auto f = Fraction::cast(eexp());
//            auto& d = f->getDenominator();
//            auto e = ebase() ^ f->getNumerator();
//            if (d == e.getMaxVaExp()) {
//                auto vars = e.Vars();
//                if (vars.size() == 1) {
//                    auto va = *vars.begin();
//                    auto eq = va - e(va);
//                    auto sq = eq ^ d;
//                    auto check = e / sq;
//                    if (check.IsInt() || check.IsSimpleFraction()) {
//                        Become(std::move(eq));
//                        return;
//                    } else {
//                        // TODO : IMPLEMENT
//                    }
//                }
//            }
//        }

        if (ebase().IsFraction() && eexp().IsMultival()==YesNoMaybe::No) {
            auto& f = ebase().as<Fraction>();
            auto _ = (f.getNumerator() ^ eexp()) / (f.getDenominator() ^ eexp());
            if (_.IsExponentiation()) {
                auto& e = _.as<Exponentiation>();
                if (!(e.ebase()==ebase() && eexp()==e.eexp())) {
                    IMPLEMENT
                }
            } else {
                Become(std::move(_));
                return;
            }
        }

        if (ebase().IsFraction() && eexp().IsInt() && eexp() < 0_v) {
            eexp() = -eexp();
            ebase() = ebase().as<Fraction>().Reciprocal();
        }

        if (ebase().Is_e()) {
            if (eexp().IsProduct()) {
                auto& p = eexp().as<Product>();
                if (p.Has(constant::pi) && p.Has(constant::i)) { // TODO : sequence does metter
//                    ebase() = -1;
//                    eexp() /= constant::i;
//                    eexp() /= constant::pi;
                }
            }
        }
        // todo : check
        if (ebase().IsSimple()) {
            if (eexp().IsProduct()) {
                auto& p = eexp().as<Product>();
                auto it = p.GetFirstOccurence<Integer>();
                auto in = ebase() ^ *it;  // IsExponentiationSimplifiable
                if (in.IsInt()) {
                    ebase() = in;
                    p.Delete(it);
                    if (p.size() == 1) {
                        eexp().optimize();
                    }
                }
            }
            if (ebase()==1) {
                if (eexp().IsInt()) {
                    Become(std::move(ebase()));
                    return;
                } else if (eexp().IsSimpleFraction()) {
                    if (eexp().as<Fraction>().getDenominator().bit(0)) {
                        Become(std::move(ebase()));
                        return;
                    }
                } else if (!!eexp().IsMultival()) {
                } else if (!(eexp().IsInfinity() || eexp().IsMInfinity())) {
                    Become(std::move(ebase()));
                    return;
                } else
                    IMPLEMENT;
            } else if (ebase()==-1 && eexp().IsInt() && eexp() > 0 && eexp()!=1) {
                eexp() = eexp().bit(0);
            } else if (eexp()==-1) {
                Become(Fraction{1,ebase()});
                return;
            } else if (eexp().IsInfinity()) {
                IMPLEMENT
            } else if (eexp().IsFraction()) {
                auto& f = eexp().as<Fraction>();
                auto& n = f.getNumerator();
                if (n != 1) {
                    // TODO: auto is = ebase().IsExponentiationSimplifiable(n);
                    auto newBase = ebase() ^ n;
                    if(!newBase.IsExponentiation()){
                        Become(newBase ^ (1_v / f.getDenominator()));
                        return;
                    }
                }
            }
        }

        bool ebz = ebase() == 0_v;
        bool exz = eexp() == 0_v;
        if(exz)
        {
            if (ebase().IsInfinity() || ebase().IsMInfinity()) {
                IMPLEMENT
            }
            if(ebz)
                throw "NaN";

            Become(1_v);
            return;
        }
        else if(eexp() == 1_v)
        {
            Become(std::move(ebase()));
            return;
        }
        else if (ebz)
        {
            if (exz)
                throw "NaN";
            Become(0_v);
            return;
        }
        else if (ebase().IsInfinity())
        {
            if (eexp() > 0) {
                Become(std::move(ebase()));
            } else
                IMPLEMENT
        }
        else if (ebase().IsMInfinity())
        {
            if (eexp() > 0) {
                if ((eexp() % 2) > 0) // TODO : test with non-ints
                    Become(std::move(ebase()));
                else
                    Become(Infinity());
            } else
                IMPLEMENT
        }
        else if (ebase().IsVa() && eexp().IsSimple())
        {
        }
        else
        {
            switch(view)
            {
                case View::Solving:

                case View::None:
                case View::Calc:
                {
                    if (eexp().IsInt() && eexp()>0) {
                        auto b = 1_v;
                        for (; eexp()--;) {
                            b *= ebase();
                        }
                        Become(std::move(b));
                        return;
                    }
                    if (ebase().IsInt() && eexp().IsInt()) {
                        Become(ebase() ^ eexp());
                        return;
                    }
                    break;
                }
                case View::Flat: {

                    if(eexp().IsInt())
                    {
                        if (ebase().IsVa()) {
                            break;
                        }
                        if (eexp() != 0_v) {
                            if (eexp() > 1) {
                                Valuable x = ebase();
                                Valuable n = eexp();
                                if (n < 0_v)
                                {
                                    x = 1_v / x;
                                    n = -n;
                                }
                                if (n == 0_v)
                                {
                                    Become(1_v);
                                    return;
                                }
                                auto y = 1_v;
                                while(n > 1)
                                {
                                    bool isInt = n.IsInt();
                                    if (!isInt)
                                        IMPLEMENT
                                    if (isInt && n.bit(0) == 0_v)
                                    {
                                        x.sq();
                                        n /= 2;
                                    }
                                    else
                                    {
                                        y *= x;
                                        x.sq();
                                        --n;
                                        n /= 2;
                                    }
                                }
                                x *= y;
                                Become(std::move(x));
                            } else if (eexp()!=-1){
                                // negative
                                Become(1_v/(ebase()^(-eexp())));
                            }
                        }
                        else { // zero
                            if (ebase() == 0_v)
                            {
                                IMPLEMENT
                                throw "NaN"; // feel free to handle this properly
                            }
                            else
                            {
                                Become(1_v);
                            }
                        }
                    }
//                    else
//                    IMPLEMENT
                    break;
                }
                case View::Equation: {
                    if(eexp().IsSimple())
                        Become(std::move(ebase()));
                    break;
                }
                default:
                	LOG_AND_IMPLEMENT(str() << " mode is " << view);
            }
        }

        if(IsExponentiation() && ebase().IsExponentiation())
        {
            auto& e = ebase().as<Exponentiation>();
            auto& eeexp = e.getExponentiation();
            if ((eeexp.FindVa() == nullptr) == (eexp().FindVa() == nullptr)) {
                eexp() *= eeexp;
                // todo : copy if it shared
                ebase() = std::move(const_cast<Valuable&>((e.getBase())));
            }
        }

        if (IsExponentiation()) {
            hash = ebase().Hash() ^ eexp().Hash();
            optimized = true;
            InitVars();
        }
    }

    Valuable& Exponentiation::operator +=(const Valuable& v)
    {
        return Become(Sum {*this, v});
    }

    Valuable& Exponentiation::operator *=(const Valuable& v)
    {
        const Exponentiation* e;
        const Fraction* f;
        const Product* fdn = {};
        const Exponentiation* fdne;
        auto isProdHasExpWithSameBase = [this](const Product* p) -> const Exponentiation*
        {
            for(auto& it : *p){
                if (it.IsExponentiation()) {
                    auto& e = it.as<Exponentiation>();
                    if (ebase() == e.getBase()) {
                        return &e;
                    }
                }
            }
            return {};
        };
        if (v.IsExponentiation()
            && ebase() == (e = &v.as<Exponentiation>())->getBase()
            && (eexp().IsInt() || eexp().IsSimpleFraction()) && eexp() > 0
            && (e->eexp().IsInt() || e->eexp().IsSimpleFraction()) && e->eexp() > 0
            )
        {
            eexp() += e->getExponentiation();
            optimized={};
        }
        else if(v.IsFraction()
                && (f = &v.as<Fraction>())->getDenominator() == ebase())
        {
            --eexp();
            optimized={};
            optimize();
            return *this *= f->getNumerator();
        }
        else if(v.IsFraction()
                && f->getDenominator().IsProduct()
                && (fdn = &f->getDenominator().as<Product>())->Has(ebase()))
        {
            --eexp();
            optimized={};
            optimize();
            return *this *= f->getNumerator() / (*fdn / ebase());
        }
        else if(fdn
                && (fdne = isProdHasExpWithSameBase(fdn)))
        {
            eexp() -= fdne->getExponentiation();
            optimized={};
            optimize();
            return *this *= f->getNumerator() / (*fdn / *fdne);
        }
        else if(ebase() == v && v.FindVa())
        {
            ++eexp();
            optimized={};
        }
        else if(v.IsProduct())
        {
            return Become(v * *this);
        }
        else if(v.IsInt())
        {
            if(v==1)
                return *this;
            else if(eexp()==-1 && ebase().IsInt())
                return Become(v/ebase());
            else
                return Become(Product{v, *this});
        }
        else
            return Become(Product{v, *this});

        optimize();
        return *this;
    }

    bool Exponentiation::MultiplyIfSimplifiable(const Valuable& v)
    {
        auto is = v == getBase();
        if (is) {
            ++eexp();
            optimized = {};
            optimize();
        } else if (v.IsExponentiation()) {
            auto& vexpo = v.as<Exponentiation>();
            is = vexpo.getBase() == getBase();
            if (is) {
                eexp() += vexpo.eexp();
                optimized = {};
                optimize();
            } // TODO : else if ? (base^2 == v.base)
        } else if (v.IsInt()) {
            IMPLEMENT
        } else {
//            std::cout << str() << " * " << v.str() << std::endl;
        }
        return is;
    }

    static auto one = 1_v;
    std::pair<bool,Valuable> Exponentiation::IsMultiplicationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is, expSumSimplifiable = {};
        is.first = v == getBase()
            && (expSumSimplifiable = vo<1>::get().IsSummationSimplifiable(eexp())).first;
        if (is.first) {
            is.second = getBase() ^ expSumSimplifiable.second;
        } else if (v.IsExponentiation()) {
            auto& vexpo = v.as<Exponentiation>();
            is.first = vexpo.getBase() == getBase();
            if (is.first) {
                is.second = ebase() ^ (eexp() + vexpo.eexp());
            } // TODO : else if ? (base^2 == v.base)
        } else if (v.IsSimple()) {
//            if (getBase().IsVa()) {
//            } else if (getExponentiation().IsSimpleFraction()) {
//                auto
//                is.first = IsMultiplicationSimplifiable()
//            } else if (getExponentiation().IsSimple()) {
//                is = base::IsMultiplicationSimplifiable(v);
//            } else {
//                IMPLEMENT
//            }
        } else if (v.IsVa()) {
            // covered by (v==base()) case
        } else {
#ifndef NDEBUG
            std::cout << "IsMultiplication simplifiable?: " << str() << " * " << v.str() << std::endl;
#endif
        }
        return is;
    }

    bool Exponentiation::SumIfSimplifiable(const Valuable& v)
    {
        auto is = !v.IsSimple() && !v.IsFraction() && !v.IsExponentiation();
        if(is){
            auto sumIfSimplifiable = v.IsSummationSimplifiable(*this);
            is = sumIfSimplifiable.first;
            if (is)
                Become(std::move(sumIfSimplifiable.second));
        }
        return is;
    }

    std::pair<bool,Valuable> Exponentiation::IsSummationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is;
        is.first = operator==(v);
        if (is.first) {
            is.second = *this * 2;
        } else if ((is.first = operator==(-v))) {
                is.second = 0;
        } else if (v.IsSimple()
                || v.IsExponentiation()
                || v.IsVa()
                || v.IsFraction())
        {
        } else {
            is = v.IsSummationSimplifiable(*this);
        }
        return is;
    }

    Valuable& Exponentiation::operator /=(const Valuable& v)
    {
        auto isMultival = IsMultival()==YesNoMaybe::Yes;
        auto vIsMultival = v.IsMultival()==YesNoMaybe::Yes;
        if(isMultival && vIsMultival) {
            solutions_t vals, thisValues;
            Values([&](auto& thisVal){
                thisValues.insert(thisVal);
                return true;
            });

            v.Values([&](auto&vVal){
                for(auto& tv:thisValues)
                    vals.insert(tv/vVal);
                return true;
            });

            return Become(Valuable(vals));
        }
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            if(ebase() == e.ebase() && (ebase().IsVa() || !ebase().IsMultival()))
            {
                eexp() -= e.eexp();
            }
            else
            {
                Become(Fraction(*this, v));
                return *this;
            }
        }
        else if(v.IsFraction())
        {
            *this *= v.as<Fraction>().Reciprocal();
            return *this;
        }
        else if(ebase() == v)
        {
            --eexp();
        }
        else
        {
            Become(Fraction(*this, v));
            return *this;
        }

        optimized={};
        optimize();
        return *this;
    }

    Valuable& Exponentiation::operator^=(const Valuable& v)
    {
        eexp() *= v;
        optimized={};
        optimize();
        return *this;
    }

    bool Exponentiation::operator ==(const Valuable& v) const
    {
        auto eq = v.IsExponentiation() && Hash()==v.Hash();
        if(eq){
            auto& e = v.as<Exponentiation>();
            eq = _1.Hash() == e._1.Hash()
                && _2.Hash() == e._2.Hash()
                && _1 == e._1
                && _2 == e._2;
        } else if (v.IsFraction()) {
            eq = eexp().IsInt()
                 && eexp() < 0
                 && ebase() == (v.as<Fraction>().getDenominator() ^ (-eexp()));
        }
        return eq;
    }

    Exponentiation::operator double() const
    {
        return std::pow(static_cast<double>(ebase()), static_cast<double>(eexp()));
    }

    Valuable& Exponentiation::d(const Variable& x)
    {
        optimized={};
        bool bhx = ebase().HasVa(x);
        bool ehx = eexp().HasVa(x);
        if(ehx) {
            IMPLEMENT
            if(bhx){

            }else{

            }
        } else if (bhx) {
            if(ebase() == x)
                Become(eexp() * (ebase() ^ (eexp()-1)));
            else
                IMPLEMENT
        } else
            Become(0_v);
        optimize();
        return *this;
    }

    Valuable& Exponentiation::i(const Variable& x, const Variable& C)
    {
        if ((eexp().IsInt() || eexp().IsSimpleFraction()) && ebase()==x) {
            ++eexp();
            operator/=(eexp());
            operator+=(C);
        } else {
            IMPLEMENT
        }

        optimize();
        return *this;
    }

    Valuable Exponentiation::I(const Variable& x, const Variable& C) const
    {
        if ((eexp().IsInt() || eexp().IsSimpleFraction()) && ebase()==x) {
            auto einc = eexp()+1;
            return (ebase() ^ einc) / einc + C;
        } else {
            IMPLEMENT
        }
    }

    bool Exponentiation::operator <(const Valuable& v) const
    {
        if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            if (e.getBase() == getBase())
                return getExponentiation() < e.getExponentiation();
            if (e.getExponentiation() == getExponentiation())
                return getBase() < e.getBase();
        }

        return base::operator <(v);
    }

    std::ostream& Exponentiation::print_sign(std::ostream& out) const
    {
        return out << "^";
    }

    Valuable::YesNoMaybe Exponentiation::IsMultival() const
    {
        auto is = _1.IsMultival() || _2.IsMultival();
        if (is != YesNoMaybe::Yes && _2.IsFraction())
            is = _2.as<Fraction>().getDenominator().IsEven() || is;
        return is;
    }

    void Exponentiation::Values(const std::function<bool(const Valuable&)>& fun) const
    {
        if (fun) {
            auto cache = optimized; // TODO: multival caching (inspect all optimized and optimization transisions) auto isCached =

            std::set<Valuable> vals;
            {
            std::deque<Valuable> d1;
            _1.Values([&](auto& v){
                d1.push_back(v);
                return true;
            });

            _2.Values([&](auto& v){
                auto vIsFrac = v.IsFraction();
                const Fraction* f;
                if(vIsFrac)
                    f = &v.template as<Fraction>();
                auto vMakesMultival = vIsFrac && f->getDenominator().IsEven()==YesNoMaybe::Yes;

                for(auto& item1:d1){
                    if(vMakesMultival){
                        Variable x;
                        auto& dn = f->getDenominator();
                        auto solutions = (x ^ dn).Equals(*this ^ dn).Solutions(x);
                        for(auto&& s:solutions)
                            vals.insert(s);
                    } else {
                        auto value=item1^v;
                        if(value.IsMultival()==YesNoMaybe::No)
                            vals.insert(value);
                        else {
                            IMPLEMENT
                        }
                    }
                }
                return true;
            });
            }

            for(auto& v:vals)
                fun(v);
        }
    }

    std::ostream& Exponentiation::code(std::ostream& out) const
    {
        if(!getExponentiation().IsInt())
            IMPLEMENT;

        out << "(1";
        for (auto i=getExponentiation(); i-->0;) {
            out << '*' << getBase();
        }
        out << ')';

        return out;
    }

    bool Exponentiation::IsComesBefore(const Valuable& v) const
    {
        auto mve = getMaxVaExp();
        auto vmve = v.getMaxVaExp();
        auto is = mve > vmve;
        if (mve != vmve)
        {}
        else if (v.IsExponentiation())
        {
            auto& e = v.as<Exponentiation>();
            bool baseIsVa = getBase().IsVa();
            bool vbaseIsVa = e.getBase().IsVa();
            if (baseIsVa && vbaseIsVa)
                is = getExponentiation() == e.getExponentiation() ? getBase().IsComesBefore(e.getBase()) : getExponentiation() > e.getExponentiation();
            else if(baseIsVa)
                is = false;
            else if(vbaseIsVa)
                is = true;
            else if(getBase() == e.ebase())
                is = getExponentiation().IsComesBefore(e.getExponentiation());
            else if(getExponentiation() == e.getExponentiation())
                is = getBase().IsComesBefore(e.getBase());
            else
            {
                auto c = Complexity();
                auto ec = e.Complexity();
                if (c != ec)
                    is = c > ec;
                else {
                    is = getBase().IsComesBefore(e.getBase()) ||
                        (!e.ebase().IsComesBefore(ebase()) && getExponentiation().IsComesBefore(e.getExponentiation())); //  || str().length() > e->str().length();
    //                auto expComesBefore = eexp().IsComesBefore(e->eexp());
    //                auto ebase()ComesBefore = ebase().IsComesBefore(e->ebase());
    //                is = expComesBefore==ebase()ComesBefore || str().length() > e->str().length();
                }
            }
        }
        else if(v.IsProduct())
        {
            is = !v.IsComesBefore(*this);
        }
        else if(v.IsInt())
            is = true;
//        else if(v.IsFraction())
//        {is=}
        else if(v.IsVa())
            is = !!FindVa();
        else if(v.IsSum())
            is = IsComesBefore(*v.as<Sum>().begin());
        else
            IMPLEMENT

        return is;
    }

    Valuable Exponentiation::calcFreeMember() const
    {
        Valuable c;
        if(getBase().IsSum() && getExponentiation().IsInt()){
            c = getBase().calcFreeMember() ^ getExponentiation();
        } else if(getBase().IsVa()) {
            c = 0_v;
        } else
            IMPLEMENT;
        return c;
    }

    Valuable & Exponentiation::sq()
    {
        eexp() *= 2;
        optimized = {};
        optimize();
        return *this;
    }

    const Valuable::vars_cont_t& Exponentiation::getCommonVars() const
    {
        return v;
    }

    Valuable Exponentiation::InCommonWith(const Valuable& v) const
    {
        auto c = 1_v;
        if (v.IsProduct()) {
            for(auto& m: v.as<Product>()){
                c = InCommonWith(m);
                if (c != 1_v) {
                    break;
                }
            }
        } else if (v.IsExponentiation()) {
            auto& e = v.as<Exponentiation>();
            if (e.getBase() == getBase()) {
                if (e.getExponentiation() == getExponentiation()) {
                    c = e;
                } else if (getExponentiation().IsSimple() && e.getExponentiation().IsSimple()) {
                    if (getExponentiation() > 0 || e.getExponentiation() > 0) {
                        if (e.getExponentiation() >= getExponentiation()) {
                            c = *this;
                        } else
                            c = e;
                    } else if (getExponentiation() < 0 || e.getExponentiation() < 0) {
                        if (e.getExponentiation() >= getExponentiation()) {
                            c = e;
                        } else
                            c = *this;
                    } else {
                        IMPLEMENT
                    }
                } else if (getExponentiation().IsSimpleFraction() && e.getExponentiation().IsSimpleFraction()) {
                    if (getExponentiation()<0 == e.getExponentiation()<0) {
                        c = getBase() ^ getExponentiation().InCommonWith(e.getExponentiation());
                    }
                } else if (getExponentiation().IsSum()) {
                    auto sz = getExponentiation().as<Sum>().size();
                    auto diff = getExponentiation() - e.getExponentiation();
                    if (!diff.IsSum() || diff.as<Sum>().size() < sz)
                        c = v;
                } else if (e.getExponentiation().IsSum()) {
                    c = e.InCommonWith(*this);
                } else if (e.getExponentiation().IsProduct()) {
                    c = ebase() ^ e.eexp().InCommonWith(eexp());
                } else {
                    IMPLEMENT
                }
            }
        } else if (getExponentiation().IsInt()) {
            if(getExponentiation() > 0)
                c = getBase().InCommonWith(v);
        } else if (getExponentiation().IsFraction()) {
        } else if (v.IsVa()) {
            c = v.InCommonWith(*this);
        } else if (v.IsInt() || v.IsSimpleFraction()) {
        } else if (getExponentiation().IsVa()) {
        } else {
            IMPLEMENT
        }
        return c;
    }

    Valuable Exponentiation::operator()(const Variable& va) const
    {
        return operator()(va, 0_v);
    }

    Valuable Exponentiation::operator()(const Variable& v, const Valuable& augmentation) const
    {
        if (!getExponentiation().FindVa() && getExponentiation()!=0 && augmentation==0) {
            return getBase()(v,augmentation);
        } else if (getExponentiation().IsSimpleFraction()) {
            auto& f = getExponentiation().as<Fraction>();
            return (getBase()^f.getNumerator())(v,augmentation^f.getDenominator());
        } else {
            IMPLEMENT
        }
    }

    Valuable::solutions_t Exponentiation::Distinct() const
    {
        solutions_t branches;
        if (eexp().IsSimpleFraction()){
            auto& f = eexp().as<Fraction>();
            auto& denom = f.denominator();
            if (denom.IsEven() == YesNoMaybe::Yes) {
                // TODO : de-recoursefy:
//                auto branchesSz = boost::multiprecision::msb(denom); // the largest bit
//                branches.reserve(branchesSz);
//                ...
                if(!ebase().IsInt()){
                    LOG_AND_IMPLEMENT("Distinct for " << str());
                } else {
                    for (auto&& branch
                            : (ebase().Sqrt() ^ (f.numerator() / (denom / 2))).Distinct())
                    {
                        branches.emplace(-branch);
                        branches.emplace(std::move(branch));
                    }
                }
            }
        } else {
            branches.emplace(*this);
        }
        return branches;
    }
}}


////////////////////////////////////////////////////////////////////////////////
// TensionFieldEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  The tension-field-theory-based strain energy density used in Skouras 2014:
//  Designing Inflatable Structures.
//
//  This energy is implemented as a function of the right Green-Green
//  deformation tensor.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  04/04/2019 18:21:53
////////////////////////////////////////////////////////////////////////////////
#ifndef TENSIONFIELDENERGY_HH
#define TENSIONFIELDENERGY_HH

#include <Eigen/Dense>
#include <array>
#include "EigSensitivity.hh"
#include "IncompressibleBalloonEnergy.hh"

template<typename Real>
struct TensionFieldEnergy {
    using V2d  = Eigen::Matrix<Real, 2, 1>;
    using M2d  = Eigen::Matrix<Real, 2, 2>;

    TensionFieldEnergy() { }

    template<typename Derived>
    TensionFieldEnergy(const Eigen::MatrixBase<Derived> &C) { setMatrix(C); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &C) {
        m_balloonEnergy   .setMatrix(C);
        m_eigSensitivities.setMatrix(C);
        setEigs(m_eigSensitivities.lambda(0), m_eigSensitivities.lambda(1));
    }
    Real energy() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde)            return m_balloonEnergy.stiffness * (m_l1 + 2.0 * m_l2_tilde - 3.0);
        return m_balloonEnergy.energy();
    }

    Real denergy(const M2d &dC) const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde)            return m_balloonEnergy.stiffness * ((1.0 - m_l2_tilde * m_l2_tilde * m_l2_tilde) * m_eigSensitivities.dLambda(dC)[0]);
        return m_balloonEnergy.denergy(dC);
    }

    M2d denergy() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return M2d::Zero();
        if (m_l2 < m_l2_tilde)            return (m_balloonEnergy.stiffness * ((1.0 - m_l2_tilde * m_l2_tilde * m_l2_tilde))) * m_eigSensitivities.dLambda(0);
        return m_balloonEnergy.denergy();
    }

    Real d2energy(const M2d &dC_a, const M2d &dC_b) const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde) {
            Real inv_l1 = 1.0 / m_l1;
            return m_balloonEnergy.stiffness * ((1.0 - inv_l1 * m_l2_tilde)         * m_eigSensitivities.d2Lambda(dC_a, dC_b)[0]
                                               + 1.5 * inv_l1 * inv_l1 * m_l2_tilde * m_eigSensitivities. dLambda(0, dC_a)
                                                                                * m_eigSensitivities. dLambda(0, dC_b));
        }
        return m_balloonEnergy.d2energy(dC_a, dC_b);
    }

    M2d delta_denergy(const M2d &dC) const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return m_relaxedStiffnessEps * m_balloonEnergy.delta_denergy_undeformed(dC); // add a small artificial stiffness to avoid rank-deficient Hessian in fully compressed regions
        if (m_l2 < m_l2_tilde) {
            Real inv_l1 = 1.0 / m_l1;
            return m_balloonEnergy.stiffness * ((1.0 - inv_l1 * m_l2_tilde)         * m_eigSensitivities.delta_dLambda(0, dC)
                                              + (1.5 * inv_l1 * inv_l1 * m_l2_tilde * m_eigSensitivities.      dLambda(0, dC))
                                                                                * m_eigSensitivities.      dLambda(0)
                                              + (m_relaxedStiffnessEps * m_eigSensitivities.dLambda(1, dC)) // add a small artificial stiffness to avoid rank-deficient Hessian in regions of partial tension
                                                                       * m_eigSensitivities.dLambda(1));
        }
        return m_balloonEnergy.delta_denergy(dC);
    }

    size_t tensionState() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0; // compression in both directions
        if (m_l2 < m_l2_tilde)            return 1; //     tension in one  direction
        return 2;                                   //     tension in both directions
    }

    Real stiffness() const { return m_balloonEnergy.stiffness; }
    void setStiffness(Real val ) { m_balloonEnergy.stiffness = val; }

    // Note: the stiffness added in the relaxed case is also proportional to stiffness()!
    void setRelaxedStiffnessEpsilon(Real val) { m_relaxedStiffnessEps = val; }
    Real getRelaxedStiffnessEpsilon() const { return m_relaxedStiffnessEps; }

    const EigSensitivity<Real> &eigSensitivities() const { return m_eigSensitivities; }

    // Useful for visualizing/debugging the energy and its derivatives at
    // arbitrary (l1, l2)...
    void setEigs(Real l1, Real l2) {
        if (l1 < l2) std::swap(l1, l2);
        m_l1 = l1;
        m_l2 = l2;
        m_l2_tilde = 1.0 / std::sqrt(l1);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Expressions in terms of eigenvalues for debugging/developing smoothed
    // energy density.
    ////////////////////////////////////////////////////////////////////////////
    // Energy expressed in terms of eigenvalues.
    Real psi() const {
        const Real k = m_balloonEnergy.stiffness;
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return 0.0;
        if (m_l2 < m_l2_tilde)            return k * (m_l1 + 2.0 * m_l2_tilde - 3.0);
        // full neo-Hookean energy in terms of eigenvalues
        return k * (m_l1 + m_l2 + 1.0 / (m_l1 * m_l2) - 3.0);
    }

    // Derivatives of energy with respect to eigenvalues l1, l2 (Note: these formulas can
    // not be used for computing denergy(dC) when l1 == l2 since the eigenvalues become
    // non-smooth functions of C in this case).
    V2d dpsi_dl() const {
        const Real k = m_balloonEnergy.stiffness;
        V2d result(V2d::Zero());
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return V2d::Zero();
        if (m_l2 < m_l2_tilde) return k * V2d(1.0 - m_l2_tilde * m_l2_tilde * m_l2_tilde, 0.0);
        return k * V2d(1.0 - 1.0 / (m_l1 * m_l1 * m_l2),
                       1.0 - 1.0 / (m_l1 * m_l2 * m_l2));
    }

    M2d d2psi_dl2() const {
        if ((m_l1 < 1.0) && (m_l2 < 1.0)) return M2d::Zero();
        M2d result;
        const Real k = m_balloonEnergy.stiffness;
        const Real inv_l1 = 1.0 / m_l1;
        if (m_l2 < m_l2_tilde) {
            result << k * 1.5 * inv_l1 * inv_l1 * m_l2_tilde, 0.0, 0.0, 0.0;
        }
        else {
            result << 2.0 / (m_l1 * m_l1 * m_l1 * m_l2), 1.0 / (m_l1 * m_l1 * m_l2 * m_l2),
                      1.0 / (m_l1 * m_l1 * m_l2 * m_l2), 2.0 / (m_l1 * m_l2 * m_l2 * m_l2);
            result *= k;
        }
        return result;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
    IncompressibleBalloonEnergy<Real> m_balloonEnergy;
    EigSensitivity<Real>              m_eigSensitivities;
    Real m_l1 = 0.0, m_l2 = 0.0, m_l2_tilde = 0.0;
    bool useSmoothedTensionField = false;
    Real m_relaxedStiffnessEps = 1e-8;
};

// Optionally use either TensionFieldEnergy or IncompressibleBalloonEnergy
template<typename _Real>
struct OptionalTensionFieldEnergy : public TensionFieldEnergy<Real> {
    using Real = _Real;
    using TFE = TensionFieldEnergy<Real>;
    using V2d = typename TFE::V2d;
    using M2d = typename TFE::M2d;

    OptionalTensionFieldEnergy() { }

    template<typename Derived>
    OptionalTensionFieldEnergy(const Eigen::MatrixBase<Derived> &C) : TFE(C) { }

    Real energy() const {
        if (useTensionField) return TFE::energy();
        return TFE::m_balloonEnergy.energy();
    }

    Real denergy(const M2d &dC) const {
        if (useTensionField) return TFE::denergy(dC);
        return TFE::m_balloonEnergy.denergy(dC);
    }

    M2d denergy() const {
        if (useTensionField) return TFE::denergy();
        return TFE::m_balloonEnergy.denergy();
    }

    M2d delta_denergy(const M2d &dC) const {
        if (useTensionField) return TFE::delta_denergy(dC);
        return TFE::m_balloonEnergy.delta_denergy(dC);
    }

    Real d2energy(const M2d &dC_a, const M2d &dC_b) const {
        if (useTensionField) return TFE::d2energy(dC_a, dC_b);
        return TFE::m_balloonEnergy.d2energy(dC_a, dC_b);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Conform to the C-based energy interface
    // (So we can use the membrane energy density wrapper)
    ////////////////////////////////////////////////////////////////////////////
    OptionalTensionFieldEnergy(const OptionalTensionFieldEnergy &other, UninitializedDeformationTag &&) {
        copyMaterialProperties(other);
    }

    static constexpr EDensityType EDType = EDensityType::CBased;
    static constexpr size_t Dimension = 2;
    static constexpr size_t N         = 2;
    void setC(Eigen::Ref<const M2d> C) { setMatrix(C); }
    M2d PK2Stress() const { return 2.0 * denergy(); }

    template<class Mat_>
    M2d delta_PK2Stress(const Mat_ &dC) const { return 2.0 * delta_denergy(dC.matrix()); }

    template<class Mat_, class Mat2_>
    M2d delta2_PK2Stress(const Mat_ &/* dC_a */, const Mat2_ &/* dC_b */) const {
        throw std::runtime_error("Unimplemented");
    }

    bool useTensionField = true;

    void copyMaterialProperties(const OptionalTensionFieldEnergy &b) {
        setStiffness(b.stiffness());
        setRelaxationEnabled(b.getRelaxationEnabled());
        setRelaxedStiffnessEpsilon(b.getRelaxedStiffnessEpsilon());
    }
    bool getRelaxationEnabled() const { return useTensionField; }
    void setRelaxationEnabled(bool enable) { useTensionField = enable; }
};

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


/*
   (c) Copyright 2012, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    Interpolation search template
*/

#ifndef LINTEL_INTERPOLATION_SEARCH_HPP
#define LINTEL_INTERPOLATION_SEARCH_HPP

#include <inttypes.h>

#include <boost/type_traits/make_unsigned.hpp>

#include <Lintel/AssertBoost.hpp>

// http://www.cs.technion.ac.il/~itai/publications/Algorithms/p550-perl.pdf
// http://en.wikipedia.org/wiki/Interpolation_search

namespace lintel {

    // TODO: redo the interface to have no precondition, i.e. v does not have to be in first..last
    // and that if it is outside that rande, the function can return a value < 0 or > last_pos.
    // but it needs to not crash.  Then clamp the return in the caller using the standard signed
    // compare trick to usually only take one compare to check if 0..last_pos but if it is out of
    // range a second to decide whether we clamp to 0 or last_pos.  A good estimator will estimate
    // <0 if v < first, and >last_pos if v > last.  This approach means that we avoid the compares
    // to re-establish the pre-condition at the cost of compares to clamp the range, but the clamp
    // compares are always going to be fast because they are ssize_t compares, whereas the current
    // compares could be expensive because they are value compares.  Tricky question on what
    // happens if first == last --> value_range == 0.  That can't happen right now because it
    // implies the return value is first and we will have stopped, but with this change it can
    // happen.
template<typename ValueT> struct EstimateOffset {
    typedef typename boost::make_unsigned<ValueT>::type UnsignedValueT;
    // Preconditions: first < v <= last
    // Postconditions: 0 <= return value <= last_pos
    size_t
    operator()(const ValueT &v, const ValueT &first, const ValueT &last, size_t last_pos) const {
        UnsignedValueT value_range = last - first;
        double relative_pos = static_cast<double>(v - first) / value_range; // [0,1]
        return relative_pos * last_pos;
    }
};

template<typename ValueT> struct Compare {
    bool operator()(const ValueT &a, const ValueT &b) const {
        return a < b;
    }
};

namespace detail {

template<size_t interpolation_steps, size_t binary_steps, size_t binary_search_below_length,
         typename ValueT, typename Iterator, class EstimateOffsetT, class CompareT>
Iterator interpolationLowerBound(Iterator begin, Iterator end, const ValueT &v,
                                 const EstimateOffsetT &estimator, const CompareT &comparer) {
    size_t estimated_offset;
    Iterator estimated_pos;

    if (begin == end) {
        return begin;
    }
    if (comparer(*begin, v)) {
        if (comparer(*(end - 1), v)) {
            // v > *end
            return end;
        } else {
            // ok, v \in ]*begin, *end]
        }
    } else {
        return begin;
    }

    // estimateOffset function can now safely assume that v is in range, so we don't have
    // to check each time to see if we are in bounds.

    size_t len = end - begin; // - on struct iterator involves divide, do it once.
    while (len > 0) {
        for (size_t is = 0; len > 0 && is < interpolation_steps; ++is) {
            DEBUG_SINVARIANT(begin < end);
            DEBUG_SINVARIANT((end - begin) == static_cast<ptrdiff_t>(len));
            DEBUG_SINVARIANT(comparer(*begin, v)); // *begin < v
            DEBUG_INVARIANT(!comparer(*(end - 1), v),// v <= *(end - 1)
                            boost::format("%s < %s") % *(end - 1) % v);
            estimated_offset = estimator(v, *begin, *(end - 1), len - 1);
            //            std::cout << boost::format("len = %d, is = %d, eo = %d\n") % len % is % estimated_offset;
            estimated_pos = begin + estimated_offset;
            DEBUG_INVARIANT(estimated_offset < len, boost::format("%d >= %d from %s [%s .. %s]")
                            % estimated_offset % len % v % *begin % *(end - 1));
            DEBUG_SINVARIANT(estimated_pos < end);
            if (comparer(*estimated_pos, v)) {
                begin = estimated_pos;
                ++begin;
                if (!comparer(*begin, v)) {
                    return begin; // this is lower bound since it is not < and one less is
                }
                len = len - (estimated_offset + 1);
            } else { // estimated_pos >= v
                if (comparer(*(estimated_pos - 1), v)) {
                    return estimated_pos; // this is the lower bound since it is not < and one less is
                }
                end = estimated_pos;
                len = estimated_offset;
            }
        }

        if (len == 0) {
            return begin;
        }

        // std::cout << boost::format("len = %d\n") % len;
        if (len <= binary_search_below_length) {
            return std::lower_bound(begin, end, v, comparer);
        }
        bool adjusted_begin = false, adjusted_end = false;
        for (size_t bs = 0; len > 0 && bs < binary_steps; ++bs) {
            DEBUG_SINVARIANT(begin < end);
            DEBUG_SINVARIANT((end - begin) == static_cast<ptrdiff_t>(len));
            estimated_offset = len >> 1;
            estimated_pos = begin + estimated_offset;
            if (comparer(*estimated_pos, v)) {
                begin = estimated_pos;
                ++begin;
                len = len - (estimated_offset + 1);
                adjusted_begin = true;
            } else {
                len = estimated_offset;
                end = estimated_pos;
                adjusted_end = true;
            }
        }
        // Repair invariants used for interpolation function
        if (adjusted_begin) {
            if (!comparer(*begin, v)) {
                return begin;
            }
        }
        if (adjusted_end) {
            if (comparer(*(end - 1), v)) {
                return end;
            }
        }
    }
    return begin;
}

} // namespace detail

// We want to write estimator = EstimateOffset<ValueT>, but that requires C++0x
template<typename ValueT, typename Iterator> Iterator
interpolationLowerBound(Iterator begin, Iterator end, const ValueT &v) {
    EstimateOffset<ValueT> eo;
    return detail::interpolationLowerBound<2,4,256>(begin, end, v, eo, Compare<ValueT>());
}

} // namespace lintel

#endif