//
//  Copyright (C) 2016 Novartis Institutes for BioMedical Research
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#include <math.h>
#include <boost/format.hpp>

#include "../RDKitBase.h"
#include "../../RDGeneral/types.h"
#include "../../Geometry/point.h"
#include "StructChecker.h"
#include "Utilites.h"
#include "Stereo.h"

namespace RDKit {
namespace StructureCheck {

static const double PI = 3.14159265359;
static const double ANGLE_EPSILON = (5.0 * PI / 180.);  // 5 degrees
static const double EPS = 0.0000001;  // float type precision ???
                                      /*
                                          // constants for bond definitions
                                      //#define  CIS_TRANS_EITHER  0x03
                                      //#define  CIS_TRANS_SWAPPED 0x08
                                      */
struct npoint_t {
  double x, y, z;
  int number;
};

static double Angle(double x1, double y1, double x2, double y2) {
  // Returns the angle between the two vectors (x1,y1) and (x2,y2) at (0,0).
  double l1, l2;
  double cos_alpha, sin_alpha;
  double result;

  l1 = sqrt(x1 * x1 + y1 * y1);
  l2 = sqrt(x2 * x2 + y2 * y2);
  if (l1 < 0.00001 || l2 < 0.00001) return (0.0);

  cos_alpha = (x1 * x2 + y1 * y2) / (l1 * l2);
  if (cos_alpha > 1.0)  // safeguard against round off erros
    cos_alpha = 1.0;
  else if (cos_alpha < -1.0)
    cos_alpha = -1.0;
  sin_alpha = (x1 * y2 - x2 * y1) / (l1 * l2);

  result = acos(cos_alpha);
  if (sin_alpha < 0.0) result = 2 * PI - result;
  return result;
}

static double Volume(struct npoint_t tetra[4]) {
  // Computes the signed volume of the tetrahedron defined by the four points in
  // tetra[].

  double ax, ay, az, bx, by, bz, cx, cy, cz;

  ax = tetra[1].x - tetra[0].x;
  ay = tetra[1].y - tetra[0].y;
  az = tetra[1].z - tetra[0].z;
  bx = tetra[2].x - tetra[0].x;
  by = tetra[2].y - tetra[0].y;
  bz = tetra[2].z - tetra[0].z;
  cx = tetra[3].x - tetra[0].x;
  cy = tetra[3].y - tetra[0].y;
  cz = tetra[3].z - tetra[0].z;

  return (ax * (by * cz - bz * cy) + ay * (bz * cx - bx * cz) +
          az * (bx * cy - by * cx));
}

int DubiousStereochemistry(RWMol &mol) {
  /*
  * Checks if there is some ill-defined stereochemistry in the
  * molecule *mp. The function returns a bit set integer which defines
  * the problems encountered.
  */

  int result = 0;
  //   int is_allene, ndb, jatom;

  std::vector<Neighbourhood> neighbour_array;

  SetupNeighbourhood(mol, neighbour_array);

  // look for EITHER bonds
  for (unsigned i = 0; i < mol.getNumBonds(); i++) {
    const Bond &bond = *mol.getBondWithIdx(i);
    if (RDKit::Bond::UNKNOWN == bond.getBondDir())  //== EITHER
    {
      std::cerr << "bond " << i << ": Either bond found" << std::endl;
      result |= EITHER_BOND_FOUND;
    }
  }
  // look for stereo bonds to non-stereogenic atoms
  for (unsigned i = 0; i < neighbour_array.size(); i++) {
    const Neighbourhood &nbp = neighbour_array[i];
    unsigned nmulti = 0;
    bool is_allene = false;
    for (unsigned j = 0; j < nbp.Bonds.size(); j++) {
      const Bond &bond = *mol.getBondWithIdx(nbp.Bonds[j]);
      if (RDKit::Bond::SINGLE != bond.getBondType()) {
        unsigned ndb = 0;
        nmulti++;
        if (RDKit::Bond::DOUBLE == bond.getBondType()) {
          unsigned jatom = nbp.Atoms[j];
          for (unsigned jj = 0; jj < neighbour_array[jatom].Bonds.size(); jj++)
            if (RDKit::Bond::DOUBLE ==
                mol.getBondWithIdx(neighbour_array[jatom].Bonds[jj])
                    ->getBondType())
              ndb++;
        }
        if (2 == ndb) is_allene = true;
      }
    }
    unsigned element = mol.getAtomWithIdx(i)->getAtomicNum();
    unsigned n_ligands = (unsigned)nbp.Bonds.size();

    if (!((6 == element &&  // "C"
           n_ligands > 2 && n_ligands <= 4 && nmulti == 0) ||
          (6 == element &&  // "C"
           n_ligands >= 2 && n_ligands <= 3 && nmulti == 1 && is_allene) ||
          (16 == element &&  // "S"
           n_ligands > 2 && n_ligands <= 4) ||
          (7 == element &&  // "N"
           n_ligands > 3 && n_ligands <= 4 && nmulti == 0) ||
          (15 == element &&  // "P"
           n_ligands > 2 && n_ligands <= 4) ||
          (14 == element &&  // "Si"
           n_ligands > 2 && n_ligands <= 4) &&
              nmulti == 0))

      for (unsigned j = 0; j < n_ligands; j++) {
        const Bond &bj = *mol.getBondWithIdx(nbp.Bonds[j]);
        if (bj.getBeginAtomIdx() == i  &&
            (RDKit::Bond::BEGINWEDGE == bj.getBondDir() ||   // == UP
             RDKit::Bond::BEGINDASH == bj.getBondDir())) {  // == DOWN))
          std::string name;
          mol.getPropIfPresent(common_properties::_Name, name);
          BOOST_LOG(rdWarningLog) << boost::format("%10s    atom %3d : %s")%
              name % i % "stereobond to non-stereogenic atom" << std::endl;
          result |= STEREO_BOND_AT_NON_STEREO_ATOM;
        }
      }
  }
  return result;
}

int FixDubious3DMolecule(RWMol &mol) {
  /*
  * Checks if the structure has 3D coordinates and/or flat sp3-carbons with
  * stereo-bonds and
  * converts the designation to 2D, clearing away any Z-component of the
  * coordinates.
  * Real 3D structures without stereo designations go through untouched.
  */

  int result = 0;
  bool non_zero_z = false;
  unsigned nstereo = 0;
  std::vector<Neighbourhood> neighbour_array;
  std::vector<RDGeom::Point3D> atomPoint(
      mol.getNumAtoms());  // X,Y,Z coordinates of each atom

  // X,Y,Z coordinates of each atom
  non_zero_z = getMolAtomPoints(mol, atomPoint);
  // At first check if this is a trivial case i.e. designated '2D'
  // and count the number of stereo bonds
  if (!non_zero_z)  // check Z coordinate of each atom
    return 0;

  nstereo = 0;
  for (unsigned i = 0; i < mol.getNumBonds(); i++) {
    const Bond *bond = mol.getBondWithIdx(i);
    if (RDKit::Bond::BEGINWEDGE == bond->getBondDir() ||
        RDKit::Bond::BEGINDASH == bond->getBondDir()
        //???    || RDKit::Bond::EITHERDOUBLE == bond->getBondDir()
        )
      nstereo++;
  }
  if (0 == nstereo) return 0;

  // compute average bond length to use in Volume significance testing
  double length = 0.0;
  for (unsigned i = 0; i < mol.getNumBonds(); i++) {
    const Bond *bond = mol.getBondWithIdx(i);
    unsigned a0 = bond->getBeginAtomIdx();
    unsigned a1 = bond->getEndAtomIdx();

    length += (atomPoint[a0].x - atomPoint[a1].x) *
                  (atomPoint[a0].x - atomPoint[a1].x) +
              (atomPoint[a0].y - atomPoint[a1].y) *
                  (atomPoint[a0].y - atomPoint[a1].y) +
              (atomPoint[a0].z - atomPoint[a1].z) *
                  (atomPoint[a0].z - atomPoint[a1].z);
  }
  length = sqrt(length / mol.getNumBonds());

  // check if there is a flat sp3 carbon
  SetupNeighbourhood(mol, neighbour_array);
  int nflat_sp3 = 0;
  for (unsigned i = 0; i < neighbour_array.size(); i++) {
    if (neighbour_array[i].Atoms.size() < 3) continue;
    RDKit::Atom *atom = mol.getAtomWithIdx(i);
    unsigned element = atom->getAtomicNum();
    if (6 != element &&   // "C"
        7 == element &&   // "N"
        15 == element &&  // "P"
        16 == element)    // "S"
      continue;

    unsigned j;
    for (j = 0; j < mol.getNumBonds(); j++) {
      const Bond *bond = mol.getBondWithIdx(j);
      if (RDKit::Bond::BEGINWEDGE == bond->getBondDir() ||
          RDKit::Bond::BEGINDASH == bond->getBondDir() &&
              i == bond->getBeginAtomIdx())
        break;
    }
    if (j < mol.getNumBonds()) continue;  // no stereo designation

    double vol = 0.0;
    int stereo_triple;
    const Neighbourhood &nbp = neighbour_array[i];
    unsigned n_ligands = (unsigned)nbp.Bonds.size();
    unsigned i1;
    struct npoint_t tetra[4];
    tetra[0].x = atomPoint[i].x;
    tetra[0].y = atomPoint[i].y;
    tetra[0].z = atomPoint[i].z;
    for (i1 = 0; i1 < n_ligands; i1++)
      if (mol.getBondWithIdx(nbp.Bonds[i1])->getBondType() !=
              RDKit::Bond::SINGLE &&
          16 != element && 15 != element)  // "S" "P"
        break;
    if (i1 >= n_ligands) continue;  // multiple bond found => no sp3 carbon
    stereo_triple = 0;
    for (i1 = 0; i1 < n_ligands; i1++) {
      tetra[1].x = atomPoint[i1].x;
      tetra[1].y = atomPoint[i1].y;
      tetra[1].z = atomPoint[i1].z;
      if (mol.getBondWithIdx(nbp.Bonds[i1])->getBondDir() ==
              RDKit::Bond::BEGINWEDGE ||
          mol.getBondWithIdx(nbp.Bonds[i1])->getBondDir() ==
              RDKit::Bond::BEGINDASH)  // UP DOWN
        stereo_triple |= 1;
      unsigned i2;
      for (i2 = i1 + 1; i2 < n_ligands; i2++) {
        tetra[2].x = atomPoint[i2].x;
        tetra[2].y = atomPoint[i2].y;
        tetra[2].z = atomPoint[i2].z;
        if (mol.getBondWithIdx(nbp.Bonds[i2])->getBondDir() ==
                RDKit::Bond::BEGINWEDGE ||
            mol.getBondWithIdx(nbp.Bonds[i2])->getBondDir() ==
                RDKit::Bond::BEGINDASH)  // UP DOWN
          stereo_triple |= 2;
        unsigned i3;
        for (i3 = i2 + 1; i3 < n_ligands; i3++) {
          tetra[3].x = atomPoint[i3].x;
          tetra[3].y = atomPoint[i3].y;
          tetra[3].z = atomPoint[i3].z;
          if (mol.getBondWithIdx(nbp.Bonds[i3])->getBondDir() ==
                  RDKit::Bond::BEGINWEDGE ||
              mol.getBondWithIdx(nbp.Bonds[i3])->getBondDir() ==
                  RDKit::Bond::BEGINDASH)  // UP DOWN
            stereo_triple |= 4;
          vol = Volume(tetra);
          if (vol < 0.) vol = -vol;
          if (!stereo_triple) continue;
          if (vol < 0.01 * length * length * length) {
            nflat_sp3++;
            break;
          }
          stereo_triple &= ~4;
        }
        stereo_triple &= ~2;
        if (i3 >= n_ligands) break;
      }
      stereo_triple &= ~1;
      if (i2 >= n_ligands) break;
    }
  }

  if (non_zero_z && 0 == mol.getConformer().is3D())  // && mol dim is 2D
    for (unsigned i = 0; i < mol.getNumAtoms(); i++)
      // TODO: ???
      atomPoint[i].z = 0.0;  // set in mol !!!
  result |= ZEROED_Z_COORDINATES;
  // Cleared z-coordinates in 2D MOL file
  if (non_zero_z && nstereo > 0 && nflat_sp3 > 0) {
    RDKit::MolOps::removeStereochemistry(mol);
    result |= CONVERTED_TO_2D;
  }
  return result;
}

void RemoveDubiousStereochemistry(RWMol &mol) {
  // Removes ill-defined stereodescriptors.
  std::vector<Neighbourhood> neighbour_array;
  SetupNeighbourhood(mol, neighbour_array);

  // remove EITHER marks
  for (unsigned i = 0; i < mol.getNumBonds(); i++) {
    Bond *bond = mol.getBondWithIdx(i);
    if (RDKit::Bond::UNKNOWN == bond->getBondDir())  //== EITHER
      bond->setBondDir(RDKit::Bond::NONE);
  }
  // remove stereo marks to non-stereogenic atoms
  for (unsigned i = 0; i < neighbour_array.size(); i++) {
    const Neighbourhood &nbp = neighbour_array[i];
    unsigned nmulti = 0;
    for (unsigned j = 0; j < nbp.Atoms.size(); j++) {
      const Bond &bond = *mol.getBondWithIdx(nbp.Bonds[j]);
      if (RDKit::Bond::SINGLE != bond.getBondType()) nmulti++;
    }

    unsigned element = mol.getAtomWithIdx(i)->getAtomicNum();
    unsigned n_ligands = (unsigned)nbp.Bonds.size();
    if (!((6 == element &&  // "C"
           n_ligands > 2 && n_ligands <= 4 && nmulti == 0) ||
          (16 == element &&  // "S"
           n_ligands > 2 && n_ligands <= 4) ||
          (7 == element &&  // "N"
           n_ligands > 3 && n_ligands <= 4 && nmulti == 0) ||
          (15 == element &&  // "P"
           n_ligands > 2 && n_ligands <= 4) ||
          (14 == element &&  // "Si"
           n_ligands > 2 && n_ligands <= 4) &&
              nmulti == 0)) {
      for (unsigned j = 0; j < n_ligands; j++) {
        Bond &bj = *mol.getBondWithIdx(nbp.Bonds[j]);
        if (bj.getBeginAtomIdx() == i &&
            (RDKit::Bond::BEGINWEDGE == bj.getBondDir()       // == UP
             || RDKit::Bond::BEGINDASH == bj.getBondDir()))  // == DOWN))
          bj.setBondDir(RDKit::Bond::NONE);
      }
    }
  }
}

//----------------------------------------------------------------------
// CheckStereo():
//----------------------------------------------------------------------

struct stereo_bond_t {
  double x, y;                     // relative 2D coordinates
  RDKit::Bond::BondDir direction;  // stereo direction to this atom coord
  int number;                      // atom number of ligand atom
  double angle;                    // angle in radiants rel. to first bond
                                   // in array (counted counter clockwise)
};

static int Atom3Parity(struct stereo_bond_t ligands[3]) {
  // Computes the stereo parity defined by three ligands.
  int a, b;
  int reference;
  struct npoint_t tetrahedron[4], h;
  double angle;
  int maxnum;

  maxnum = ligands[0].number;
  for (unsigned i = 1; i < 3; i++)
    if (maxnum < ligands[i].number) maxnum = ligands[i].number;

  reference = (-1);
  for (unsigned i = 0; i < 3; i++)
    if (ligands[i].direction != RDKit::Bond::NONE)
      if (reference == (-1))
        reference = i;
      else {
        // stereo_error = "three attachments with more than 2 stereobonds";
        std::cerr << "three attachments with more than 2 stereobonds"
                  << std::endl;
        return (ILLEGAL_REPRESENTATION);
      }

  if (reference == (-1)) return (UNDEFINED_PARITY);

  if (reference == 0) {
    a = 1;
    b = 2;
  } else if (reference == 1) {
    a = 0;
    b = 2;
  } else {
    a = 0;
    b = 1;
  }

  angle = Angle(ligands[a].x, ligands[a].y, ligands[b].x, ligands[b].y);

  if (angle < ANGLE_EPSILON || fabs(PI - angle) < ANGLE_EPSILON) {
    // stereo_error = "three attachments: colinearity violation";
    std::cerr << "three attachments colinearity violation" << std::endl;

    return (ILLEGAL_REPRESENTATION);
  }

  tetrahedron[0].x = 0.0;
  tetrahedron[0].y = 0.0;
  tetrahedron[0].z = 0.0;
  tetrahedron[0].number = maxnum + 1;
  for (unsigned i = 0; i < 3; i++) {
    tetrahedron[i + 1].x = ligands[i].x;
    tetrahedron[i + 1].y = ligands[i].y;
    if (ligands[i].direction == RDKit::Bond::BEGINWEDGE)  // UP)
      tetrahedron[i + 1].z = 1.0;
    else if (ligands[i].direction == RDKit::Bond::BEGINDASH)  // DOWN)
      tetrahedron[i + 1].z = -1.0;
    else if (ligands[i].direction == RDKit::Bond::NONE)
      tetrahedron[i + 1].z = 0.0;
    else {
      // stereo_error = "three attachments: illegal bond symbol";
      std::cerr << "three attachments illegal bond symbol" << std::endl;
      return (ILLEGAL_REPRESENTATION);
    }
    tetrahedron[i + 1].number = ligands[i].number;
  }

  for (unsigned i = 1; i < 4; i++)
    for (unsigned j = i; j > 0; j--)
      if (tetrahedron[j].number < tetrahedron[j - 1].number) {
        h = tetrahedron[j];
        tetrahedron[j] = tetrahedron[j - 1];
        tetrahedron[j - 1] = h;
      } else
        break;

  return (Volume(tetrahedron) > 0.0 ? EVEN_PARITY : ODD_PARITY);
}

static int Atom4Parity(struct stereo_bond_t ligands[4]) {
  /*
  * Computes the stereo parity defined by four ligands.
  * Assumes central atom at 0/0/0.
  */
  struct npoint_t tetrahedron[4], h;
  int nup, ndown, nopposite;
  double angle;

  nup = ndown = 0;
  for (unsigned i = 0; i < 4; i++) {
    tetrahedron[i].x = ligands[i].x;
    tetrahedron[i].y = ligands[i].y;
    tetrahedron[i].z = 0.0;
    tetrahedron[i].number = ligands[i].number;
    if (ligands[i].direction == RDKit::Bond::BEGINWEDGE) {  // UP
      nup++;
      tetrahedron[i].z = 1.0;
    } else if (ligands[i].direction == RDKit::Bond::BEGINDASH) {  // DOWN
      ndown++;
      tetrahedron[i].z = (-1.0);
    } else if (ligands[i].direction != RDKit::Bond::NONE) {
      // stereo_error = "illegal bond symbol";
      std::cerr << "illegal bond symbol" << std::endl;

      return (ILLEGAL_REPRESENTATION);
    }
  }

  if (nup == 0 && ndown == 0) return (UNDEFINED_PARITY);

  if (nup > 2 || ndown > 2) {
    // stereo_error = "too many stereobonds";
    std::cerr << "too many stereobonds" << std::endl;
    return (ILLEGAL_REPRESENTATION);
  }

  if (nup + ndown == 1)  // check for 'umbrellas'
  {
    unsigned ij;
    for (ij = 0; ij < 4; ij++)
      if (ligands[ij].direction == RDKit::Bond::BEGINWEDGE ||
          ligands[ij].direction == RDKit::Bond::BEGINDASH)
        break;
    nopposite = 0;
    for (unsigned j = 0; j < 4; j++)
      if (ij == j)
        continue;
      else if (ligands[ij].x * ligands[j].x + ligands[ij].y * ligands[j].y < 0)
        nopposite++;
    if (nopposite > 2) {
      // stereo_error = "UMBRELLA: all non-stereo bonds opposite to single
      // stereo bond";
      std::cerr << "umbrella" << std::endl;
      return (ILLEGAL_REPRESENTATION);
    }
  }

  for (unsigned i = 0; i < 2; i++)
    if ((ligands[i].direction == RDKit::Bond::BEGINWEDGE &&
         ligands[i + 2].direction == RDKit::Bond::BEGINDASH) ||
        (ligands[i].direction == RDKit::Bond::BEGINDASH &&
         ligands[i + 2].direction == RDKit::Bond::BEGINWEDGE)) {
      // stereo_error = "UP/DOWN opposition";
      std::cerr << "up/down" << std::endl;
      return (ILLEGAL_REPRESENTATION);
    }

  for (unsigned i = 0; i < 4; i++)
    if ((ligands[i].direction == RDKit::Bond::BEGINWEDGE &&
         ligands[(i + 1) % 4].direction == RDKit::Bond::BEGINWEDGE)  // UP
        || (ligands[i].direction == RDKit::Bond::BEGINDASH          // DOWN
            && ligands[(i + 1) % 4].direction == RDKit::Bond::BEGINDASH)) {
      // stereo_error = "Adjacent like stereobonds";
      std::cerr << "adjacent like" << std::endl;
      return (ILLEGAL_REPRESENTATION);
    }

  for (unsigned i = 0; i < 4; i++)
    if (ligands[i].direction == RDKit::Bond::NONE &&
        ligands[(i + 1) % 4].direction == RDKit::Bond::NONE &&
        ligands[(i + 2) % 4].direction == RDKit::Bond::NONE) {
      angle = Angle(ligands[i].x - ligands[(i + 1) % 4].x,
                    ligands[i].y - ligands[(i + 1) % 4].y,
                    ligands[(i + 2) % 4].x - ligands[(i + 1) % 4].x,
                    ligands[(i + 2) % 4].y - ligands[(i + 1) % 4].y);
      if (angle < (185 * PI / 180)) {
        // stereo_error = "colinearity or triangle rule violation";
        std::cerr << "colinearity or triangle rule" << std::endl;
        return (ILLEGAL_REPRESENTATION);
      }
    }

  for (unsigned i = 1; i < 4; i++)
    for (unsigned j = i; j > 0; j--)
      if (tetrahedron[j].number < tetrahedron[j - 1].number) {
        h = tetrahedron[j];
        tetrahedron[j] = tetrahedron[j - 1];
        tetrahedron[j - 1] = h;
      } else
        break;

  return (Volume(tetrahedron) > 0.0 ? EVEN_PARITY : ODD_PARITY);
}

int AtomParity(const ROMol &mol, unsigned iatom, const Neighbourhood &nbp) {
  /*
  * Computes the stereo parity of atom number iatom in *mp relative
  * to its numbering. The immediate neighbours are defined by *nbp
  * to speed up processing.
  */
  struct stereo_bond_t stereo_ligands[4], h;
  bool multiple = false, allene = false, stereo = false;

  if (nbp.Atoms.size() < 3 || nbp.Atoms.size() > 4)
    return ILLEGAL_REPRESENTATION;

  std::vector<RDGeom::Point3D> atomPoint(
      mol.getNumAtoms());  // X,Y,Z coordinates of each atom
  getMolAtomPoints(mol, atomPoint);

  for (unsigned i = 0; i < nbp.Bonds.size(); i++) {
    const Bond &bi = *mol.getBondWithIdx(i);
    if (bi.getBondType() != RDKit::Bond::SINGLE) {
      multiple = true;
      // check if the multiple bond is part of an allene
      unsigned jatom = nbp.Atoms[i] + 1;
      unsigned ndb = 0;
      for (unsigned j = 0; j < mol.getNumBonds(); j++) {
        const Bond &bond = *mol.getBondWithIdx(j);
        if (bond.getBeginAtomIdx() == jatom || bond.getEndAtomIdx() == jatom)
          if (bond.getBondType() == RDKit::Bond::DOUBLE) ndb++;
      }
      if (ndb == 2) allene = true;
    }

    stereo_ligands[i].x = atomPoint[i].x - atomPoint[iatom].x;
    stereo_ligands[i].y = atomPoint[i].y - atomPoint[iatom].y;
    stereo_ligands[i].number = nbp.Atoms[i] + 1;
    if (bi.getBeginAtomIdx() == iatom) {
      stereo_ligands[i].direction = bi.getBondDir();
      if (stereo_ligands[i].direction == RDKit::Bond::BEGINWEDGE ||  // UP ||
          stereo_ligands[i].direction == RDKit::Bond::BEGINDASH)    // DOWN
        stereo = true;
    } else
      stereo_ligands[i].direction = RDKit::Bond::NONE;
  }
  unsigned element = mol.getAtomWithIdx(iatom)->getAtomicNum();
  if (multiple && stereo && 15 != element && 16 != element) {  // "P" && "S"
    if (allene)
      return (ALLENE_PARITY);
    else {
      // stereo_error = "AtomParity: Stereobond at unsaturated atom";
      std::cerr << "stereobond at unsaturated atom" << std::endl;
      return (ILLEGAL_REPRESENTATION);
    }
  } else if (multiple && 16 != element)  // "S"
    return (UNDEFINED_PARITY);

  stereo_ligands[0].angle = 0.0; /* comp. angle rel. to first ligand */
  for (unsigned i = 1; i < nbp.Atoms.size(); i++)
    stereo_ligands[i].angle = Angle(stereo_ligands[0].x, stereo_ligands[0].y,
                                    stereo_ligands[i].x, stereo_ligands[i].y);
  for (unsigned i = 2; i < nbp.Atoms.size(); i++) /* sort ligands */
    for (unsigned j = i; j > 1; j--)
      if (stereo_ligands[j].angle < stereo_ligands[j - 1].angle) {
        h = stereo_ligands[j];
        stereo_ligands[j] = stereo_ligands[j - 1];
        stereo_ligands[j - 1] = h;
      } else
        break;

  return (nbp.Atoms.size() == 3 ? Atom3Parity(stereo_ligands)
                                : Atom4Parity(stereo_ligands));
}

bool CheckStereo(const ROMol &mol) {
  /*
  * Checks if all potential stereocenters are either completely undefined
  * or attributed with hashes and wedges according to MDL rules.
  */
  bool result = true;
  int parity;
  bool center_defined = false;
  std::vector<Neighbourhood> neighbour_array;

  SetupNeighbourhood(mol, neighbour_array);

  for (unsigned i = 0; i < neighbour_array.size(); i++) {
    const Neighbourhood &nbp = neighbour_array[i];
    unsigned element = mol.getAtomWithIdx(i)->getAtomicNum();
    unsigned n_ligands = (unsigned)nbp.Bonds.size();
    if ((n_ligands > 2 && n_ligands <= 4) && (6 == element       // "C"
                                              || 16 == element   // "S"
                                              || 7 == element    // "N"
                                              || 8 == element    // "O"
                                              || 15 == element   // "P"
                                              || 14 == element)  // "Si"
        ) {
      parity = AtomParity(mol, i, nbp);

      const Atom *atom = mol.getAtomWithIdx(i);
      if (atom->getChiralTag() == Atom::CHI_TETRAHEDRAL_CW ||
          atom->getChiralTag() == Atom::CHI_TETRAHEDRAL_CCW)
        center_defined = true;

      if (parity == ILLEGAL_REPRESENTATION) {  // stereo_error
        result = false;
      } else if (parity == EVEN_PARITY || parity == ODD_PARITY ||
                 parity == ALLENE_PARITY) {
        center_defined = true;
      }
      else {
        for (unsigned j = 0; j < nbp.Bonds.size(); j++) {
          const Bond &bond = *mol.getBondWithIdx(j);
          if (bond.getBeginAtomIdx() == i &&
              (RDKit::Bond::BEGINWEDGE == bond.getBondDir()         // == UP
               || RDKit::Bond::BEGINDASH == bond.getBondDir())) {  // == DOWN))
            // stereobond to non-stereogenic atom
            std::cerr << "stereobond to nonstereogenic" << std::endl;
            result = false;
          }
        }
      }
    }
  }

  unsigned int chiralFlag = 0;
  mol.getPropIfPresent(RDKit::common_properties::_MolFileChiralFlag,
                       chiralFlag);

  if (chiralFlag && !center_defined) {  // no stereocenter defined
    std::cerr << "chiral flag, no stereocenter" << std::endl;
    result = false;
  }
  return result;
}

bool AtomClash(RWMol &mol, double clash_limit) {
  /*
  * Checks if any two atoms in *mp come closer than 10% of the
  * average bond length or if an atom is too close the line
  * between two bonded atoms.
  */
  double bond_square_median, dist, min_dist;
  double rr, rb, bb, h;
  std::vector<RDGeom::Point3D> atomPoint(
      mol.getNumAtoms());  // X,Y,Z coordinates of each atom

  bool twod=true;
  getMolAtomPoints(mol, atomPoint, twod);

  // compute median of square of bond lenght (quick/dirty)
  if (mol.getNumBonds() == 0) return false;
  std::vector<double> blengths(mol.getNumBonds());
  blengths[0] = 1.0;

  for (unsigned i = 0; i < mol.getNumBonds(); i++) {
    const Bond *bond = mol.getBondWithIdx(i);
    unsigned a1 = bond->getBeginAtomIdx();
    unsigned a2 = bond->getEndAtomIdx();
    blengths[i] = (atomPoint[a1].x - atomPoint[a2].x) *
                      (atomPoint[a1].x - atomPoint[a2].x) +
                  (atomPoint[a1].y - atomPoint[a2].y) *
                      (atomPoint[a1].y - atomPoint[a2].y);
  }
  for (unsigned i = 1; i < mol.getNumBonds(); i++) {
    for (int j = rdcast<int>(i) - 1; j >= 0; j--) {
      if (blengths[j] > blengths[j + 1]) {
        h = blengths[j];
        blengths[j] = blengths[j + 1];
        blengths[j + 1] = h;
      } else
        break;
    }
  }
  bond_square_median = blengths[mol.getNumBonds() / 2];

  // Check if two atoms get too close to each other
  min_dist = bond_square_median;
  for (unsigned i = 0; i < mol.getNumAtoms(); i++)
    for (unsigned j = i + 1; j < mol.getNumAtoms(); j++) {
      dist =
          (atomPoint[i].x - atomPoint[j].x) *
              (atomPoint[i].x - atomPoint[j].x) +
          (atomPoint[i].y - atomPoint[j].y) * (atomPoint[i].y - atomPoint[j].y);
      if (dist < clash_limit * clash_limit * bond_square_median) {
        std::cerr << "clash 1" << std::endl;

        return true;
      }
      if (dist < min_dist) min_dist = dist;
    }

  // check if atom lies on top of some bond
  for (unsigned i = 0; i < mol.getNumBonds(); i++) {
    const Bond *bond = mol.getBondWithIdx(i);
    unsigned a1 = bond->getBeginAtomIdx();
    unsigned a2 = bond->getEndAtomIdx();
    for (unsigned j = 0; j < mol.getNumAtoms(); j++) {
      if (a1 == j || a2 == j) continue;
      rr = (atomPoint[j].x - atomPoint[a1].x) *
               (atomPoint[j].x - atomPoint[a1].x) +
           (atomPoint[j].y - atomPoint[a1].y) *
               (atomPoint[j].y - atomPoint[a1].y);
      bb = (atomPoint[a2].x - atomPoint[a1].x) *
               (atomPoint[a2].x - atomPoint[a1].x) +
           (atomPoint[a2].y - atomPoint[a1].y) *
               (atomPoint[a2].y - atomPoint[a1].y);
      rb = (atomPoint[j].x - atomPoint[a1].x) *
               (atomPoint[a2].x - atomPoint[a1].x) +
           (atomPoint[j].y - atomPoint[a1].y) *
               (atomPoint[a2].y - atomPoint[a1].y);

      if (0 <= rb &&   // cos alpha > 0
          rb <= bb &&  // projection of r onto b does not exceed b
          (rr * bb - rb * rb) / (bb + EPS) <  // distance from bond < limit
              clash_limit * clash_limit * bond_square_median) {
        BOOST_LOG(rdWarningLog) << "atom " << j << " " <<
            100*sqrt((rr*bb-rb*rb)/(bb*bond_square_median+EPS)) <<
            "% of average bond length " << a1 << "-" << a2 << std::endl;
        std::cerr << "clash 2" << std::endl;
        return true;
      }
    }
  }
  return false;  // no clash
}
//--------------------------------------------------------------------------

/*
* Sets the color field of the defined double bonds in *mp to CIS,
* TRANS, or NONE depending on the ligands with the lowest numbering[].
* It returns the number of defined double bonds found.
*/
int CisTransPerception(const ROMol &mol,
                       const std::vector<RDGeom::Point3D> &points,
                       const std::vector<unsigned> &numbering,
                       std::vector<unsigned> &bondColor) {
  int result = 0;
  unsigned int maxnum = 0;
  std::vector<Neighbourhood> nba(mol.getNumAtoms());
  SetupNeighbourhood(mol, nba);

  for (unsigned i = 0; i < bondColor.size(); i++) bondColor[i] = 0;
  for (unsigned i = 0; i < nba.size(); i++)  // n_atoms
    if (numbering[i] > maxnum) maxnum = numbering[i];

  for (unsigned i = 0; i < bondColor.size(); i++)
    if (RDKit::Bond::DOUBLE == mol.getBondWithIdx(i)->getBondType()
        // FIX:         && mol.getBondWithIdx(i)->getBondDir() !=
        // RDKit::Bond::ENDDOWNRIGHT
        // FIX:         && mol.getBondWithIdx(i)->getBondDir() !=
        // RDKit::Bond::ENDUPRIGHT
        &&
        mol.getBondWithIdx(i)->getBondDir() !=
            RDKit::Bond::EITHERDOUBLE) {  // != CIS_TRANS_EITHER
      unsigned j1 = mol.getBondWithIdx(i)->getBeginAtomIdx();
      unsigned j2 = mol.getBondWithIdx(i)->getEndAtomIdx();
      if (6 != mol.getAtomWithIdx(j1)->getAtomicNum())  // C
        continue;
      if (16 != mol.getAtomWithIdx(j1)->getAtomicNum())  // N
        continue;
      if (6 != mol.getAtomWithIdx(j2)->getAtomicNum())  // C
        continue;
      if (16 != mol.getAtomWithIdx(j2)->getAtomicNum())  // N
        continue;
      // n_ligands :
      if (nba[j1].Atoms.size() <= 1 ||  // no subst.
          nba[j2].Atoms.size() <= 1)
        continue;
      if (nba[j1].Atoms.size() > 3 ||  // valence error in mol
          nba[j2].Atoms.size() > 3)
        continue;

      bool equal = false;  // find lowest numbered neighbour of j1
      unsigned at1 = 0;
      for (unsigned k = 0, nmin = maxnum; k < nba[j1].Atoms.size(); k++)
        if (nba[j1].Atoms[k] != j2)  // no loop back
        {
          if (numbering[nba[j1].Atoms[k]] <
              nmin) {  // numbering[nba[j1].atoms[k]]
            at1 = nba[j1].Atoms[k];
            nmin = numbering[at1];
          } else if (numbering[nba[j1].Atoms[k]] == nmin)
            equal = true;
        }
      if (equal)   // identical substituents
        continue;  // no stereochemistry

      equal = false;  // find lowest numbered neighbour of j1
      unsigned at2 = 0;
      for (unsigned k = 0, nmin = maxnum; k < nba[j2].Atoms.size(); k++)  //
        if (nba[j2].Atoms[k] != j1) {  // no loop back
          if (numbering[nba[j2].Atoms[k]] < nmin) {
            at2 = nba[j2].Atoms[k];
            nmin = numbering[at2];
          } else if (numbering[nba[j2].Atoms[k]] == nmin)
            equal = true;
        }
      if (equal)   // identical substituents
        continue;  // no stereochemistry

      // Now, bp points to a double bond, at1 and at2 are the
      // indices (not numbers) of the atoms with lowest numbering
      // attached to each end of the bond, and the bond is
      // guaranteed to be either CIS or TRANS

      double x21, y21;
      double x23, y23;
      double x32, y32;
      double x34, y34;
      double sign;
      x21 = points[at1].x - points[j1].x;
      y21 = points[at1].y - points[j1].y;
      x23 = points[j2].x - points[j1].x;
      y23 = points[j2].y - points[j1].y;
      x32 = (-x23);
      y32 = (-y23);
      x34 = points[at2].x - points[j2].x;
      y34 = points[at2].y - points[j2].y;
      sign = (x21 * y23 - x23 * y21) * (x32 * y34 - x34 * y32);
      if (fabs(sign) < 0.001) continue;
      result++;
      bondColor[i] = (sign > 0.0 ? CIS : TRANS);
    }
  return (result);
}

}  // namespace StructureCheck
}  // namespace RDKit


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CUMMIN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CUMMIN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-reduction
    This function object computes the cumulate minimum of the argument elements

    @par Header <boost/simd/function/cummin.hpp>

    @par Example:

      @snippet cummin.cpp cummin

    @par Possible output:

      @snippet cummin.txt cummin

  **/
  Value cummin(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/cummin.hpp>
#include <boost/simd/function/simd/cummin.hpp>

#endif


/* Copyright (C) 2012-2020 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.
 */
/**
 * @file binaryArith.cpp
 * @brief Implementing integer addition, multiplication in binary representation
 */
#include <numeric>
#include <climits>
#include <map>
#include <algorithm>
#include <stdexcept>
#include <atomic>
#include <mutex> // std::mutex, std::unique_lock

#include <NTL/BasicThreadPool.h>
#include <helib/binaryArith.h>

#ifdef HELIB_DEBUG
#include <cstdio>
#include <helib/debugging.h>
#endif

#define BPL_ESTIMATE (30)
// FIXME: this should really be dynamic

namespace helib {

#ifdef HELIB_DEBUG
void decryptAndSum(std::ostream& s,
                   const CtPtrMat& numbers,
                   bool twosComplement = false);
#endif

typedef std::pair<long, long> NodeIdx; // nodes are indexed by a pair (i,j)

/**
 * @class DAGnode
 * @brief A node in an addition-DAG structure.
 *
 **/
class DAGnode
{
public:
  NodeIdx idx; // the indexes of this node
  bool isQ;    // if not then isP
  long level;  // The level at the time of computation

  std::atomic_long childrenLeft; // how many children were not computed yet
  DAGnode *parent1, *parent2;

  std::mutex ct_mtx; // controls access to ctxt pointer (and the ctxt itself)
  Ctxt* ct;          // points to the actual ciphertext (or nullptr)

  DAGnode(NodeIdx ii,
          bool qq,
          long lvl,
          long chl = 0,
          DAGnode* pt1 = nullptr,
          DAGnode* pt2 = nullptr) :
      idx(ii),
      isQ(qq),
      level(lvl),
      childrenLeft(chl),
      parent1(pt1),
      parent2(pt2),
      ct(nullptr)
  {}

  DAGnode(DAGnode&& other) :
      // move constructor
      idx(other.idx),
      isQ(other.isQ),
      level(other.level),
      childrenLeft(long(other.childrenLeft)), // copy value of atomic_long
      parent1(other.parent1),
      parent2(other.parent2),
      ct(other.ct)
  {}

  std::string nodeName() const
  {
    return (std::string(isQ ? "Q(" : "P(") + std::to_string(idx.first) + ',' +
            std::to_string(idx.second) + ')');
  }
};

//! A class to help manage the allocation of temporary Ctxt objects
class ScratchCell
{
public:
  std::atomic_bool used;
  std::unique_ptr<Ctxt> ct; // scratch space owns this pointer
  ScratchCell(const Ctxt& c) : used(true), ct(new Ctxt(ZeroCtxtLike, c)) {}
  ScratchCell(ScratchCell&& other) :
      // move constructor
      used(bool(other.used)),
      ct(std::move(other.ct))
  {}
};

/**
 * @class AddDAG
 * @brief A class representing the logic of the order of bit products when
 *        adding two integers.
 *
 * Given two input arrays a[], b[], we build a DAG with each node representing
 * a term either of the form p_{i,j} = \prod_{t=j}^i (a[t]+b[t]), or of the
 * form q_{i,j} = (a[j]*b[j]) * \prod_{t=j+1}^i (a[t]+b[t]). The source nodes
 * are of the forms (a[i]*b[i]) and (a[i]+b[i]), and each non-source node has
 * exactly two parents, whose product yields that node.
 *
 * When building the DAG, we keep the level of each node as high as possible.
 * For example we can set q_{i,j}=p_{i,k}*q_{k-1,j} or q_{i,j}=p_{i,k+1}*q_{k,j}
 * (among other options), and we choose the option that results in the highest
 * level. In addition, we try to minimize the number of nodes in the DAG that
 * actually need to be computed while adding the two numbers (subject to still
 * consuming as few levels as possible).
 **/
class AddDAG
{
  std::mutex scratch_mtx;           // controls access to scratch vector
  std::vector<ScratchCell> scratch; // scratch space for ciphertexts
  std::map<NodeIdx, DAGnode> p;     // p[i,j]= prod_{t=j}^i (a[t]+b[t])
  std::map<NodeIdx, DAGnode> q; // q[i,j]= a[j]b[j]*prod_{t=j+1}^i (a[t]+b[t])
  long aSize, bSize;

  Ctxt* allocateCtxtLike(const Ctxt& c); // Allocate a new ciphertext if needed
  void markAsAvailable(DAGnode* node);   // Mark temporary Ctxt object as unused
  const Ctxt& getCtxt(DAGnode* node,     // Compute a new Ctxt if need
                      const CtPtrs& a,
                      const CtPtrs& b);

  // Add to c the Ctxt from the given node
  void addCtxtFromNode(Ctxt& c, DAGnode* node, const CtPtrs& a, const CtPtrs& b)
  {
    std::unique_lock<std::mutex> lck(node->ct_mtx);
    c += getCtxt(node, a, b);
    if (--(node->childrenLeft) == 0)
      markAsAvailable(node);
  }

public:
  //! Build a plan to add a and b
  void init(const CtPtrs& a, const CtPtrs& b);

  // Build the addition DAG
  AddDAG(const CtPtrs& a, const CtPtrs& b) { init(a, b); }

  //! Perform the actual addition
  void apply(CtPtrs& sum, const CtPtrs& a, const CtPtrs& b, long sizeLimit = 0);

  //! Returns the lowest level in this DAG
  long lowLvl() const
  {
    if (aSize < 1)
      return 0;
    return findQ(bSize - 1, 0)->level;
  }
  //! Returns a pointer to the a 'p' node of index (i,j)
  DAGnode* findP(long i, long j) const
  { // returns nullptr if not exists
    auto it = p.find(NodeIdx(i, j));
    if (it == p.end()) {
      std::cerr << "  findP(" << i << ',' << j << ") not found" << std::endl;
      return nullptr; // not found
    }
    return (DAGnode*)&(it->second);
  }
  //! Returns a pointer to the a 'q' node of index (i,j)
  DAGnode* findQ(long i, long j) const
  { // returns nullptr if not exists
    auto it = q.find(NodeIdx(i, j));
    if (it == q.end()) {
      std::cerr << "  findQ(" << i << ',' << j << ") not found" << std::endl;
      return nullptr; // not found
    }
    return (DAGnode*)&(it->second);
  }
#ifdef HELIB_DEBUG
  void printAddDAG(bool printCT = false);
#endif
};

// When searching for a good middle point, we use a "good default", so that
// in the cases covered by that default solution we get the smallest number
// of nodes with childrenLeft>0.
// When initializing p[i,j] the default should be p[i,i+1-2^e]*p[i-2^e,j]
// where e is the largest exponent with 2^e <= i-j.
inline long defaultPmiddle(long delta)
{
  return 1 << (NTL::NumBits(delta) - 1);
}
// When initializing q[i,j] the default should be p[i,i+1-2^e]*q[i-2^e,j]
// where e is the largest exponent with 2^e+2^{e-1} <= i-j.
inline long defaultQmiddle(long delta)
{
  delta = (2 * delta + 2) / 3; // ceil(2 delta / 3)
  return 1 << (NTL::NumBits(delta) - 1);
}

//! Build a plan to add a and b
void AddDAG::init(const CtPtrs& aa, const CtPtrs& bb)
{
  // make sure that lsize(b) >= lsize(a)
  const CtPtrs& a = (lsize(bb) >= lsize(aa)) ? aa : bb;
  const CtPtrs& b = (lsize(bb) >= lsize(aa)) ? bb : aa;

  aSize = lsize(a);
  bSize = lsize(b);
  assertTrue<InvalidArgument>(aSize >= 1, "a must not be empty");

  // Initialize the p[i,i]'s and q[i,i]'s
  p.clear();
  q.clear();
  for (long i = 0; i < bSize; i++) {
    NodeIdx idx(i, i);
    // The level of b[i]
    long lvl =
        (b.isSet(i) && !(b[i]->isEmpty())) ? b[i]->bitCapacity() : LONG_MAX;
    if (i < aSize) {
      // The level of a[i]
      long aLvl =
          (a.isSet(i) && !(a[i]->isEmpty())) ? a[i]->bitCapacity() : LONG_MAX;
      lvl = std::min(lvl, aLvl);
      if (lvl == LONG_MAX ||
          aLvl == LONG_MAX) // is either a[i] or b[i] is empty
        q.emplace(idx, DAGnode(idx, true, LONG_MAX, 1));
      else
        q.emplace(idx, DAGnode(idx, true, lvl - 1, 1));
    }
    p.emplace(idx, DAGnode(idx, false, lvl, 1));
  }

  // Initialize p[i,j] for bSize>=i>j>0
  for (long delta = 1; delta < bSize; delta++)
    for (long i = bSize - 1; i >= delta; --i) {
      long j = i - delta;
      long mid = i - defaultPmiddle(delta); // initialize to a "good default"
      DAGnode* prnt2 = findP(i, mid + 1);
      DAGnode* prnt1 = findP(mid, j);
      long maxLvl = std::min(prnt1->level, prnt2->level) - BPL_ESTIMATE;
      if (prnt1->level == LONG_MAX ||
          prnt2->level == LONG_MAX) // parent is empty
        maxLvl = LONG_MAX;
      long maxN =
          std::min(long(prnt1->childrenLeft), long(prnt2->childrenLeft));
      for (long m = j; m < i; m++) { // find middle point maximizing lvl(p[i,j])
        if (m == mid)
          continue;
        DAGnode* p2 = findP(i, m + 1);
        DAGnode* p1 = findP(m, j);
        long lvl = std::min(p1->level, p2->level) - BPL_ESTIMATE;
        if (p1->level == LONG_MAX || p2->level == LONG_MAX) // parent is empty
          lvl = LONG_MAX;
        long n = std::min(long(p1->childrenLeft), long(p2->childrenLeft));
        if (lvl > maxLvl || (lvl == maxLvl && n > maxN)) {
          maxLvl = lvl;
          maxN = n;
          prnt1 = p1;
          prnt2 = p2;
        }
      }
      NodeIdx idx(i, j);
      p.emplace(idx, DAGnode(idx, false, maxLvl, 0, prnt1, prnt2));
      prnt1->childrenLeft++;
      prnt2->childrenLeft++;
    }

  // Initialize q[i,j] for bSize>=i>j>=0
  for (long delta = 1; delta < bSize; delta++)
    for (long i = bSize - 1; i >= delta; --i) {
      long j = i - delta;
      if (j >= aSize)
        continue;
      long maxLvl = 0, maxN = 0;
      long mid = i - defaultQmiddle(delta); // initialize to a "good default"
      DAGnode* prnt2 = findP(i, mid + 1);
      DAGnode* prnt1 = findQ(mid, j);
      if (prnt1 != nullptr) {
        maxLvl = std::min(prnt1->level, prnt2->level) - BPL_ESTIMATE;
        if (prnt1->level == LONG_MAX ||
            prnt2->level == LONG_MAX) // parent is empty
          maxLvl = LONG_MAX;
        maxN = long(prnt2->childrenLeft);
      }
      for (long m = j; m < i; m++) { // find middle point maximizing lvl(p[i,j])
        if (m == mid)
          continue;
        DAGnode* p2 = findP(i, m + 1);
        DAGnode* p1 = findQ(m, j);
        if (p1 == nullptr)
          continue;
        long lvl = std::min(p1->level, p2->level) - BPL_ESTIMATE;
        if (p1->level == LONG_MAX || p2->level == LONG_MAX) // parent is empty
          lvl = LONG_MAX;
        long n = long(p2->childrenLeft);
        if (lvl > maxLvl || (lvl == maxLvl && n > maxN)) {
          maxLvl = lvl;
          maxN = n;
          prnt1 = p1;
          prnt2 = p2;
        }
      }
      if (prnt1 == nullptr)
        continue; // cannot create node
      NodeIdx idx(i, j);
      q.emplace(idx, DAGnode(idx, true, maxLvl, 1, prnt1, prnt2));
      prnt1->childrenLeft++;
      prnt2->childrenLeft++;
    }
}

//! Apply the DAG to actually compute the sum
void AddDAG::apply(CtPtrs& sum,
                   const CtPtrs& aa,
                   const CtPtrs& bb,
                   long sizeLimit)
{
  // make sure that lsize(b) >= lsize(a)
  const CtPtrs& a = (lsize(bb) >= lsize(aa)) ? aa : bb;
  const CtPtrs& b = (lsize(bb) >= lsize(aa)) ? bb : aa;
  if (aSize != lsize(a) || bSize != lsize(b))
    throw LogicError("DAG applied to wrong vectors");

  if (sizeLimit == 0)
    sizeLimit = bSize + 1;
  if (lsize(sum) != sizeLimit)
    sum.resize(sizeLimit, &b); // allocate space for the output
  for (long i = 0; i < lsize(sum); i++)
    sum[i]->clear();

  // Allow multi-threading in this loop
  NTL_EXEC_RANGE(sizeLimit, first, last)
  for (long i = first; i < last; i++) { //  for (long i=0; i<sizeLimit; i++) {
    if (i < bSize)
      addCtxtFromNode(*(sum[i]), this->findP(i, i), a, b);
    for (long j = std::min(i - 1, aSize - 1); j >= 0; --j) {
      DAGnode* node = this->findQ(i - 1, j);
      if (node != nullptr)
        addCtxtFromNode(*(sum[i]), node, a, b);
    }
  }
  NTL_EXEC_RANGE_END
}

//! Get the ciphertext for a node, computing it as needed
const Ctxt& AddDAG::getCtxt(DAGnode* node, const CtPtrs& a, const CtPtrs& b)
{
  // NOTE: node->ct_mtx should be locked before calling this function

  if (node->ct == nullptr) { // ciphertext not computed yet, do it now
    if (node->parent1 != nullptr && node->parent2 != nullptr) { // internal node
      // Obtain locks and ciphertexts for both parents. Also reduce the
      // number of children of that parents that still need to be computed
      std::unique_lock<std::mutex> pt1_lck(node->parent1->ct_mtx);
      const Ctxt& c1 = getCtxt(node->parent1, a, b);
      long n1 = --(node->parent1->childrenLeft);

      std::unique_lock<std::mutex> pt2_lck(node->parent2->ct_mtx);
      const Ctxt& c2 = getCtxt(node->parent2, a, b);
      long n2 = --(node->parent2->childrenLeft);

      if (n1 == 0) { // reuse space from parent1
        node->parent1->ct = nullptr;
        node->ct = (Ctxt*)&c1;
        if (c1.isEmpty() || c2.isEmpty())
          node->ct->clear(); // ct is zero if any of the parents is
        else
          node->ct->multiplyBy(c2);
        if (n2 == 0)
          markAsAvailable(node->parent2);
      } else if (n2 == 0) { // reuse space from parent2
        node->parent2->ct = nullptr;
        node->ct = (Ctxt*)&c2;
        if (c1.isEmpty() || c2.isEmpty())
          node->ct->clear(); // ct is zero if any of the parents is
        else
          node->ct->multiplyBy(c1);
      } else { // allocate new space
        node->ct = allocateCtxtLike(c2);
        if (c1.isEmpty() || c2.isEmpty())
          node->ct->clear(); // ct is zero if any of the parents is
        else {
          *(node->ct) = c2;
          node->ct->multiplyBy(c1);
        }
      }
    } else { // no parents, either a[i]+b[i] or a[i]*b[i]
      long i = node->idx.first;
      long j = node->idx.second; // we expect i==j
      const Ctxt* ct_ptr = b.ptr2nonNull();
      assertNotNull(ct_ptr, "ct_ptr must not be null");
      node->ct = allocateCtxtLike(*ct_ptr);

      if (node->isQ) { // This is b[i]*a[j]
        if (b.isSet(i) && !(b[i]->isEmpty()) && a.isSet(j) &&
            !(a[j]->isEmpty())) {
          *(node->ct) = *(b[i]);
          node->ct->multiplyBy(*(a[j]));
        } // if a[j] or b[i] is empty then node->ct is a zero ciphertext
        else
          node->ct->clear();
      } else { // This is b[i]+a[i]
        if (!b.isSet(i) || b[i]->isEmpty())
          node->ct->clear();
        else
          *(node->ct) = *(b[i]);
        if (a.isSet(j) && !(a[j]->isEmpty()))
          *(node->ct) += *(a[j]);
      }
    } // end of no-parents case
  }
  return *(node->ct);
}

//! Adds another cell to scratch space, or use an existing one that's free
Ctxt* AddDAG::allocateCtxtLike(const Ctxt& c)
{
  // look for an unused cell in the scratch array
  for (long i = 0; i < lsize(scratch); i++)
    if (scratch[i].used == false) { // found a free one, try to use it
      bool used = scratch[i].used.exchange(true); // mark it as used
      if (used == false) // make sure no other thread got there first
        return scratch[i].ct.get();
    }

  // If not found, allocate a new cell
  ScratchCell sc(c);      // cell points to new ctxt, with used=true
  Ctxt* pt = sc.ct.get(); // remember the raw pointer
  std::unique_lock<std::mutex> lck(scratch_mtx); // protect scratch vector
  scratch.emplace_back(std::move(sc));           // scratch now owns the pointer
  return pt;                                     // return the raw pointer
}

// Mark a scratch ciphertext as unused. We assume that no two nodes
// ever share a ciphertext object, so if this node is done with the
// object then the object is unused.
void AddDAG::markAsAvailable(DAGnode* node)
{
  // NOTE: node->ct_mtx should be locked before calling this function
  // NOTE: somewhat inefficient, use linear search for the raw pointer
  for (long i = 0; i < (long)scratch.size(); i++)
    if (scratch[i].ct.get() == node->ct)
      scratch[i].used = false;
  node->ct = nullptr;
}

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

// Use packed bootstrapping, so we can bootstrap all in just one go.
void packedRecrypt(const CtPtrs& a,
                   const CtPtrs& b,
                   std::vector<zzX>* unpackSlotEncoding)
{
  const Ctxt* ct = b.ptr2nonNull(); // find some non-null Ctxt
  if (ct == nullptr)
    ct = a.ptr2nonNull();
  if (ct == nullptr)
    return; // nothing to do

  assertNotNull<InvalidArgument>(unpackSlotEncoding,
                                 "unpackSlotEncoding must not be null");
  assertTrue(ct->getPubKey().isBootstrappable(),
             "public key must be bootstrappable for recryption");

  struct CtPtrs_pair : CtPtrs
  {
    const CtPtrs& a;
    const CtPtrs& b;
    CtPtrs_pair(const CtPtrs& _a, const CtPtrs& _b) : a(_a), b(_b) {}
    Ctxt* operator[](long i) const override
    {
      return (i < lsize(a)) ? a[i] : b[i - lsize(a)];
    }
    long size() const override { return lsize(a) + lsize(b); }
  };
  const CtPtrs_pair ab(a, b);

  packedRecrypt(ab, *unpackSlotEncoding, *(ct->getContext().ea));
}

// Return a number as a vector of bits with little endian-ness
std::vector<long> longToBitVector(long num, long bitSize)
{
  assertTrue<InvalidArgument>(bitSize >= 0, "bitSize must be non-negative.");
  std::vector<long> result;
  for (long i = 0; i < bitSize; num >>= 1, ++i)
    result.push_back(num & 1);
  return result;
}

//! Apply mask across the vector of bits slot-wise.
void binaryMask(CtPtrs& bits, const Ctxt& mask)
{
  for (long i = 0; i < bits.size(); ++i)
    bits[i]->multiplyBy(mask);
}

//! Implementation of output = cond ? trueValue : falseValue
void binaryCond(CtPtrs& output,
                const Ctxt& cond,
                const CtPtrs& trueValue,
                const CtPtrs& falseValue)
{
  assertEq(trueValue.size(),
           falseValue.size(),
           "trueValue and falseValue must have the same size.");
  assertEq(output.size(),
           falseValue.size(),
           "output and input vectors must have the same size.");
  vecCopy(output, trueValue);
  std::vector<Ctxt> falseCopy;
  vecCopy(falseCopy, falseValue);
  binaryMask(output, cond);
  Ctxt negated_cond(cond);
  negated_cond.addConstant(NTL::ZZX(1L));
  CtPtrs_vectorCt falseCopyWrapper(falseCopy);
  binaryMask(falseCopyWrapper, negated_cond);
  // TODO: Change this to use bit-wise XOR
  for (long i = 0; i < output.size(); ++i)
    *output[i] += falseCopy[i];
}

//! Concatenate two binary numbers into a single `CtPtrs` object.
void concatBinaryNums(CtPtrs& output, const CtPtrs& a, const CtPtrs& b)
{
  assertEq(output.size(),
           a.size() + b.size(),
           "output must be of size a.size() + b.size()");
  for (long i = 0; i < a.size(); ++i)
    *output[i] = *a[i];
  for (long i = 0; i < b.size(); ++i)
    *output[i + a.size()] = *b[i];
}

//! Split a binary number into two separate binary numbers.
void splitBinaryNums(CtPtrs& leftSplit, CtPtrs& rightSplit, const CtPtrs& input)
{
  assertEq(leftSplit.size() + rightSplit.size(),
           input.size(),
           "Output sizes must sum to input.size()");
  for (long i = 0; i < leftSplit.size(); ++i)
    *leftSplit[i] = *input[i];
  for (long i = 0; i < rightSplit.size(); ++i)
    *rightSplit[i] = *input[i + leftSplit.size()];
}

//! Shift binary numbers to the left by `shamt`
void leftBitwiseShift(CtPtrs& output, const CtPtrs& input, const long shamt)
{
  assertTrue(shamt >= 0, "Shift amount must be positive.");
  assertEq(output.size(),
           input.size(),
           "output and input must have the same size.");
  for (long i = 0; i < output.size() - shamt; ++i)
    *output[i + shamt] = *input[i];
  for (long i = 0; i < shamt; ++i)
    output[i]->clear();
}

//! Rotate binary numbers by `rotamt`.
void bitwiseRotate(CtPtrs& output, const CtPtrs& input, long rotamt)
{
  assertEq(output.size(),
           input.size(),
           "output and input must be the same size.");
  long bitSize = input.size();
  rotamt = mcMod(rotamt, bitSize);
  for (long i = 0; i < output.size(); ++i)
    *output[i] = *input[mcMod(i - rotamt, bitSize)];
}

//! Compute a bitwise XOR between `lhs` and `rhs`.
void bitwiseXOR(CtPtrs& output, const CtPtrs& lhs, const CtPtrs& rhs)
{
  assertEq(output.size(), lhs.size(), "output and lhs must be the same size.");
  assertEq(lhs.size(), rhs.size(), "lhs and rhs must be the same size.");
  vecCopy(output, lhs);
  for (long i = 0; i < rhs.size(); ++i)
    *output[i] += *rhs[i];
}

//! Compute a bitwise OR between `lhs` and `rhs`.
void bitwiseOr(CtPtrs& output, const CtPtrs& lhs, const CtPtrs& rhs)
{
  assertEq(output.size(), lhs.size(), "output and lhs must be the same size.");
  assertEq(lhs.size(), rhs.size(), "lhs and rhs must be the same size.");
  // a OR b is equivalent to (ab + a + b)
  // Start output off as lhs & rhs
  bitwiseAnd(output, lhs, rhs);
  // Now add on lhs and rhs
  for (long i = 0; i < rhs.size(); ++i) {
    *output[i] += *lhs[i];
    *output[i] += *rhs[i];
  }
}

//! Compute a bitwise AND between `lhs` and `rhs`.
void bitwiseAnd(CtPtrs& output, const CtPtrs& lhs, const CtPtrs& rhs)
{
  assertEq(output.size(), lhs.size(), "output and lhs must be the same size.");
  assertEq(lhs.size(), rhs.size(), "lhs and rhs must be the same size.");
  vecCopy(output, lhs);
  for (long i = 0; i < rhs.size(); ++i)
    output[i]->multiplyBy(*rhs[i]);
}

//! Compute a bitwise AND between `input` and `mask`.
void bitwiseAnd(CtPtrs& output,
                const CtPtrs& input,
                const std::vector<long> mask)
{
  assertEq(output.size(),
           input.size(),
           "output and input must be the same size.");
  vecCopy(output, input);
  for (long i = 0; i < output.size(); ++i)
    if (!mask[i])
      output[i]->clear();
}

//! Compute a bitwise NOT of `input`.
void bitwiseNot(CtPtrs& output, const CtPtrs& input)
{
  assertEq(output.size(),
           input.size(),
           "input and output must have the same size");
  vecCopy(output, input);
  for (long i = 0; i < output.size(); ++i)
    output[i]->addConstant(NTL::ZZ(1L));
}

//! Add two integers in binary representation
void addTwoNumbers(CtPtrs& sum,
                   const CtPtrs& lhs,
                   const CtPtrs& rhs,
                   long sizeLimit,
                   std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  if (lsize(lhs) < 1) {
    vecCopy(sum, rhs, sizeLimit);
    return;
  } else if (lsize(rhs) < 1) {
    vecCopy(sum, lhs, sizeLimit);
    return;
  }

  // Work out the order of multiplications to compute all the carry bits
  AddDAG addPlan(lhs, rhs);

#ifdef HELIB_DEBUG // print plan
  addPlan.printAddDAG();
#endif

  // Ensure that we have enough levels to compute everything,
  // bootstrap otherwise
  if (addPlan.lowLvl() < BPL_ESTIMATE) {
    packedRecrypt(lhs, rhs, unpackSlotEncoding);
    addPlan.init(lhs, rhs);                // Re-compute the DAG
    if (addPlan.lowLvl() < BPL_ESTIMATE) { // still not enough levels
      throw LogicError("not enough levels for addition DAG");
    }
  }
  addPlan.apply(sum, lhs, rhs, sizeLimit); // perform the actual addition
}

// Negate a binary number that is already in 2's complement. Note: input must
// not alias negation.
void negateBinary(CtPtrs& negation, const CtPtrs& input)
{
  assertEq(negation.size(), input.size(), "Arguments must have matching size.");
  std::vector<Ctxt> bitFlippedInput;
  vecCopy(bitFlippedInput, input);
  // First flip all bits of the input.
  for (auto& bit : bitFlippedInput)
    bit.addConstant(NTL::ZZX(1L));
  // Deep copy of input into negation.
  vecCopy(negation, bitFlippedInput);
  // Now add one.
  negation[0]->addConstant(NTL::ZZX(1L));
  // Calculate the resultant carry bits.
  std::vector<Ctxt>& carryBits = bitFlippedInput;
  incrementalProduct(carryBits);
  for (std::size_t i = 1; i < bitFlippedInput.size(); ++i)
    *(negation[i]) += carryBits[i - 1];
}

// Subtract rhs from lhs and put the result in difference.
void subtractBinary(CtPtrs& difference,
                    const CtPtrs& lhs,
                    const CtPtrs& rhs,
                    std::vector<zzX>* unpackSlotEncoding)
{
  assertEq(lhs.size(), rhs.size(), "Size of lhs and rhs must be the same.");
  assertEq(difference.size(),
           rhs.size(),
           "Size of output vector must equal the size of the input vectors.");
  // Negate the rhs and then use the existing add function.
  std::vector<Ctxt> negated_rhs(rhs.size(), *rhs[0]);
  CtPtrs_vectorCt negated_wrapper(negated_rhs);
  negateBinary(negated_wrapper, rhs);
  addTwoNumbers(difference,
                lhs,
                negated_wrapper,
                lhs.size(),
                unpackSlotEncoding);
}

// Return pointers to the three inputs, ordered by size
static std::tuple<const CtPtrs*, const CtPtrs*, const CtPtrs*> orderBySize(
    const CtPtrs& a,
    const CtPtrs& b,
    const CtPtrs& c)
{
  if (lsize(a) <= lsize(b)) {
    if (lsize(b) <= lsize(c))
      return std::make_tuple(&a, &b, &c); // a <= b <= c
    else if (lsize(a) <= lsize(c))
      return std::make_tuple(&a, &c, &b); // a <= c < b
    else
      return std::make_tuple(&c, &a, &b); // c < a <= b
  } else {                                // lsize(b) < lsize(a)
    if (lsize(a) <= lsize(c))
      return std::make_tuple(&b, &a, &c); // b < a <= c
    else if (lsize(b) <= lsize(c))
      return std::make_tuple(&b, &c, &a); // b <= c < a
    else
      return std::make_tuple(&c, &b, &a); // c < b < a
  }
}

// Implementing the basic 3-for-2 trick: u,v,w encrypt bits, return two bits
// x,y such that x+2y = u+v+w over the integers. Outputs can alias the inputs.
static void three4Two(Ctxt& lsb,
                      Ctxt& msb,
                      const Ctxt& u,
                      const Ctxt& v,
                      const Ctxt& w)
{
  Ctxt tmp_v = v;
  Ctxt tmp_w = w;
  lsb = u;
  msb = u;

  lsb += tmp_v;          // u+v
  msb.multiplyBy(tmp_v); // u*v

  tmp_v = lsb; // u+v

  tmp_v.multiplyBy(tmp_w); // (u+v)*w

  lsb += tmp_w; // u+v+w
  msb += tmp_v; // u*v + (u+v)*w = u*v + u*w + v*w
}

// Same as three4Two above, but some of the inputs could be null.
// Returns the number of output bits that are not identically zero.
static long three4Two(Ctxt* lsb, Ctxt* msb, Ctxt* u, Ctxt* v, Ctxt* w)
{
  if (u != nullptr && !u->isEmpty() && v != nullptr && !v->isEmpty() &&
      w != nullptr && !w->isEmpty()) { // if none are empty
    three4Two(*lsb, *msb, *u, *v, *w); // call the function above
    return 2;
  }
  if ((u == nullptr || u->isEmpty()) && (v == nullptr || v->isEmpty()) &&
      (w == nullptr || w->isEmpty())) { // if all are empty
    lsb->clear();                       // result is empty too
    msb->clear();
    return 0;
  }

  // Some are empty, others are not, arrange so that empty are at the end
  if (u == nullptr || u->isEmpty()) {
    if (v == nullptr || v->isEmpty())
      u = w; // only w was non-empty
    else {
      u = v;
      v = w;
    } // v,w were non-empty
  } else if (v == nullptr || v->isEmpty())
    v = w;     // u is non-empty, v was empty
  w = nullptr; // we don't use w anymore

  if (v == nullptr || v->isEmpty()) { // only u is non-empty
    *lsb = *u;
    msb->clear();
    return 1;
  }

  // both u,v are non-empty
  Ctxt tmp = *v;
  *lsb = *u;
  *msb = *u;
  *lsb += tmp;
  msb->multiplyBy(tmp);
  return 2;
}

// Apply the 3-for-2 routine to integers (i.e., an array of bits). The
// inputs need not be of the same size, and size of the output x is
// equal to the largest of them, and the size of the output y is one
// larger. This is safe even when the outputs alias some of the inputs
static void three4Two(CtPtrs& lsb,
                      CtPtrs& msb,
                      const CtPtrs& u,
                      const CtPtrs& v,
                      const CtPtrs& w,
                      long sizeLimit)
{
  HELIB_TIMER_START;
  // Arrange u,v,w by size from smallest to largest
  const CtPtrs *p1, *p2, *p3;
  std::tie(p1, p2, p3) = orderBySize(u, v, w); // size(p3)>=size(p2)>=size(p1)

  if (p3->size() <= 0) { // empty input
    setLengthZero(lsb);
    setLengthZero(msb);
    return;
  }
  if (p1->size() <= 0) { // two or less inputs
    std::vector<Ctxt> tmp;
    vecCopy(tmp, *p2, sizeLimit); // just in case p2, msb share pointers
    vecCopy(msb, *p3, sizeLimit);
    vecCopy(lsb, tmp);
    return;
  }
  if (sizeLimit == 0)
    sizeLimit = p3->size() + 1;

  // Allocate space in the output vectors

  const Ctxt* ctptr = p3->ptr2nonNull();
  std::vector<Ctxt> tmpMsb, tmpLsb;

  long lsbSize = std::min(sizeLimit, lsize(*p3));
  long msbSize = lsbSize;
  if (lsize(*p2) == lsize(*p3) && lsbSize < sizeLimit)
    msbSize++; // possible carry out of last position

  resize(tmpLsb, lsbSize, Ctxt(ZeroCtxtLike, *ctptr));
  resize(tmpMsb, msbSize, Ctxt(ZeroCtxtLike, *ctptr));

  NTL_EXEC_RANGE(msbSize - 1, first, last)
  for (long i = first; i < last; i++) {
    if (i < lsize(*p1))
      three4Two(&tmpLsb[i], &tmpMsb[i + 1], (*p1)[i], (*p2)[i], (*p3)[i]);
    else if (i < lsize(*p2)) {
      three4Two(&tmpLsb[i], &tmpMsb[i + 1], (*p2)[i], (*p3)[i], nullptr);
    } else if (p3->isSet(i))
      tmpLsb[i] = *((*p3)[i]);
  }
  NTL_EXEC_RANGE_END

  if (msbSize == lsbSize) { // we only computed upto lsbSize-1, do the last LSB
    if (p1->isSet(lsbSize - 1))
      tmpLsb[lsbSize - 1] = *((*p1)[lsbSize - 1]);
    if (p2->isSet(lsbSize - 1))
      tmpLsb[lsbSize - 1] += *((*p2)[lsbSize - 1]);
    if (p3->isSet(lsbSize - 1))
      tmpLsb[lsbSize - 1] += *((*p3)[lsbSize - 1]);
  }
  vecCopy(lsb, tmpLsb);
  vecCopy(msb, tmpMsb);
}

//! @brief An implementation of PtrMatrix using vector< PtrVector<T>* >
template <typename T>
struct PtrMatrix_PtPtrVector : PtrMatrix<T>
{
  std::vector<PtrVector<T>*>& rows;
  PtrMatrix_PtPtrVector(std::vector<PtrVector<T>*>& mat) : rows(mat) {}
  PtrVector<T>& operator[](long i) override // returns a row
  {
    return *rows[i];
  }
  const PtrVector<T>& operator[](long i) const override // returns a row
  {
    return *rows[i];
  }
  long size() const override { return lsize(rows); } // How many rows
};

// Calculates the sum of many numbers using the 3-for-2 method
void addManyNumbers(CtPtrs& sum,
                    CtPtrMat& numbers,
                    long sizeLimit,
                    std::vector<zzX>* unpackSlotEncoding)
{
#ifdef HELIB_DEBUG
  std::cout << " addManyNumbers: " << numbers.size()
            << " numbers with size-limit=" << sizeLimit << std::endl;
#endif
  HELIB_TIMER_START;
  const Ctxt* ct_ptr = numbers.ptr2nonNull();
  if (lsize(numbers) < 1 || ct_ptr == nullptr) { // nothing to add
    setLengthZero(sum);
    return;
  }
  if (lsize(numbers) == 1) {
    vecCopy(sum, numbers[0]);
    return;
  }

  bool bootstrappable = ct_ptr->getPubKey().isBootstrappable();
  const EncryptedArray& ea = *(ct_ptr->getContext().ea);

  long leftInQ = lsize(numbers);
  std::vector<CtPtrs*> numPtrs(leftInQ);
  for (long i = 0; i < leftInQ; i++)
    numPtrs[i] = &(numbers[i]);

  // use 3-for-2 repeatedly until only two numbers are leff to add
  while (leftInQ > 2) {
    // If any number is too low level, then bootstrap everything
    PtrMatrix_PtPtrVector<Ctxt> wrapper(numPtrs);
    if (findMinBitCapacity(wrapper) < 3 * ct_ptr->getContext().BPL()) {
      assertNotNull<InvalidArgument>(unpackSlotEncoding,
                                     "unpackSlotEncoding must not be null");
      assertTrue(bootstrappable,
                 "public key must be bootstrappable for recryption");

      packedRecrypt(wrapper, *unpackSlotEncoding, ea, /*belowLvl=*/10);
    }
    // Prepare a vector for pointers to the output of this iteration
    long nTriples = leftInQ / 3;
    long leftOver = leftInQ - (3 * nTriples);
    std::vector<CtPtrs*> numPtrs2(2 * nTriples + leftOver);

    if (leftOver > 0) { // copy the leftover pointers
      numPtrs2[0] = numPtrs[3 * nTriples];
      if (leftOver > 1)
        numPtrs2[1] = numPtrs[3 * nTriples + 1];
    }
    // Allow multi-threading in this loop
    //    NTL_EXEC_RANGE(nTriples, first, last)
    //    for (long i=first; i<last; i++) {   // call the three-for-two
    //    procedure
    for (long i = 0; i < nTriples; i++) { // call the three-for-two procedure
      three4Two(*numPtrs[3 * i],
                *numPtrs[3 * i + 1], // three4Two works in-place
                *numPtrs[3 * i],
                *numPtrs[3 * i + 1],
                *numPtrs[3 * i + 2],
                sizeLimit);

      numPtrs2[leftOver + 2 * i] = numPtrs[3 * i]; // copy the output pointers
      numPtrs2[leftOver + 2 * i + 1] = numPtrs[3 * i + 1];
    }
    //    NTL_EXEC_RANGE_END
    numPtrs.swap(numPtrs2);   // swap input/output vectors
    leftInQ = lsize(numPtrs); // update the size
  }
  // final addition
  addTwoNumbers(sum, *numPtrs[0], *numPtrs[1], sizeLimit, unpackSlotEncoding);
}

// Multiply a positive a by a potentially negative b, we need to sign-extend b
static void multByNegative(CtPtrs& product,
                           const CtPtrs& a,
                           const CtPtrs& b,
                           long sizeLimit,
                           std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  long resSize = lsize(a) + lsize(b);
  if (sizeLimit > 0 && sizeLimit < resSize)
    resSize = sizeLimit;

  NTL::Vec<NTL::Vec<Ctxt>> numbers(NTL::INIT_SIZE, std::min(lsize(a), resSize));
  long nNums = lsize(numbers);
  for (long i = 0; i < nNums; i++)
    numbers[i].SetLength(resSize, Ctxt(ZeroCtxtLike, *(a[0])));

  std::vector<std::pair<long, long>> pairs;
  for (long i = 0; i < nNums; i++)
    for (long j = i; j < resSize; j++)
      if (j < i + lsize(b) && a.isSet(i) && !a[i]->isEmpty() &&
          b.isSet(j - i) && !b[j - i]->isEmpty()) {
        pairs.push_back(std::pair<long, long>(i, j));
      }
  long nPairs = lsize(pairs);

  NTL_EXEC_RANGE(nPairs, first, last)
  for (long idx = first; idx < last; idx++) {
    long i, j;
    std::tie(i, j) = pairs[idx];
    numbers[i][j] = *(b[j - i]);
    numbers[i][j].multiplyBy(*(a[i])); // multiply by the bit of a
  }
  NTL_EXEC_RANGE_END

  // sign extension
  for (long i = 0; i < nNums; i++)
    for (long j = i + lsize(b); j < resSize; j++) {
      numbers[i][j] = numbers[i][i + lsize(b) - 1]; // sign extension
    }

  CtPtrMat_VecCt nums(numbers); // Wrapper around numbers
#ifdef HELIB_DEBUG
  long pa, pb;
  std::vector<long> slots;
  decryptBinaryNums(slots, a, *dbgKey, *dbgEa, false);
  pa = slots[0];
  decryptBinaryNums(slots, b, *dbgKey, *dbgEa, true);
  pb = slots[0];
  decryptAndSum((std::cout << " multByNegative: " << pa << '*' << pb << " = "),
                nums,
                true);
#endif
  addManyNumbers(product, nums, resSize, unpackSlotEncoding);
}

// Multiply two integers (i.e. an array of bits) lhs, rhs.
// Computes the pairwise products x_{i,j} = lhs_i * rhs_j
// then sums the prodcuts using the 3-for-2 method.
void multTwoNumbers(CtPtrs& product,
                    const CtPtrs& lhs,
                    const CtPtrs& rhs,
                    bool rhsTwosComplement,
                    long sizeLimit,
                    std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  long lhsSize = lsize(lhs);
  long rhsSize = lsize(rhs);
  long resSize = lhsSize + rhsSize;
  if (sizeLimit > 0 && sizeLimit < resSize)
    resSize = sizeLimit;

  if (lhs.numNonNull() < 1 || rhs.numNonNull() < 1) {
    setLengthZero(product);
    return; // return 0
  }

#ifdef HELIB_DEBUG
  std::cout << " before multiplication, capacity="
            << findMinBitCapacity({&lhs, &rhs}) << std::endl;
#endif
  // Edge case, if lhs or rhs is 1 bit
  if (lhsSize == 1) {
    if (lhs[0]->isEmpty()) {
      setLengthZero(product);
      return;
    }
    vecCopy(product, rhs, resSize);
    for (long i = 0; i < resSize; i++)
      product[i]->multiplyBy(*(lhs[0]));
    return;
  }
  if (rhsTwosComplement) { // somewhat different implementation for 2s
                           // complement
    multByNegative(product, lhs, rhs, sizeLimit, unpackSlotEncoding);
    return;
  }
  if (rhsSize == 1) {
    if (rhs[0]->isEmpty()) {
      setLengthZero(product);
      return;
    }
    vecCopy(product, lhs, resSize);
    for (long i = 0; i < resSize; i++)
      lhs[i]->multiplyBy(*(rhs[0]));
    return;
  }

  // We make sure temp_lhs is the larger of the two integers
  // to keep the number of additions to a minimum
  const CtPtrs& temp_lhs = (lhsSize >= rhsSize) ? lhs : rhs;
  const CtPtrs& temp_rhs = (lhsSize >= rhsSize) ? rhs : lhs;
  lhsSize = lsize(temp_lhs);
  rhsSize = lsize(temp_rhs);

  NTL::Vec<NTL::Vec<Ctxt>> numbers(NTL::INIT_SIZE,
                                   std::min(lsize(rhs), resSize));
  const Ctxt* ct_ptr = lhs.ptr2nonNull();
  long nNums = lsize(numbers);
  for (long i = 0; i < nNums; i++)
    numbers[i].SetLength(std::min((i + lhsSize), resSize),
                         Ctxt(ZeroCtxtLike, *ct_ptr));
  std::vector<std::pair<long, long>> pairs;
  for (long i = 0; i < nNums; i++)
    for (long j = i; j < lsize(numbers[i]); j++) {
      if (lhs.isSet(j - i) && !(lhs[j - i]->isEmpty()) && rhs.isSet(i) &&
          !(rhs[i]->isEmpty()))
        pairs.push_back(std::pair<long, long>(i, j));
    }
  long nPairs = lsize(pairs);
  NTL_EXEC_RANGE(nPairs, first, last)
  for (long idx = first; idx < last; idx++) {
    long i, j;
    std::tie(i, j) = pairs[idx];
    numbers[i][j] = *(lhs[j - i]);
    numbers[i][j].multiplyBy(*(rhs[i])); // multiply by the bit of rhs
  }
  NTL_EXEC_RANGE_END

  CtPtrMat_VecCt nums(numbers); // A wrapper around numbers
#ifdef HELIB_DEBUG
  long plaintext_lhs, plaintext_rhs;
  std::vector<long> slots;
  decryptBinaryNums(slots, lhs, *dbgKey, *dbgEa, false);
  plaintext_lhs = slots[0];
  decryptBinaryNums(slots, rhs, *dbgKey, *dbgEa, false);
  plaintext_rhs = slots[0];
  decryptAndSum((std::cout << " multTwoNumbers: " << plaintext_lhs << '*'
                           << plaintext_rhs << " = "),
                nums,
                false);
#endif
  addManyNumbers(product, nums, resSize, unpackSlotEncoding);
}

/* seven4Three: adding seven input bits, getting a 3-bit counter
 *
 * input: in[6..0]
 * ----------------
 *         in[6]
 *       b2 b1 = sum of in[2..0] (b2=msb, b1=lsb)
 *       b4 b3 = sum of in[5..3]
 * ------------
 *       c2 c1 = sum of in[6],b1,b3
 *    c4 c3    = sum of b2,b4
 * ------------
 *    d2 d1 c1 = out[2..0]
 */
// The output Ctxts[0..2] must be initialized, can alias the inputs
static void seven4Three(const CtPtrs& out, const CtPtrs& in, long sizeLimit)
{
  // we need 4 scratch ciphertexts
  std::vector<Ctxt> tmp(4, *out[0]);

  // Aliases, referring to the scheme above. Aliases for temporary
  // vars chosen so that inputs, outputs of three4two are distinct

  Ctxt& c1 = *out[0];
  Ctxt& d1 = *out[1];
  Ctxt& d2 = *out[2];

  Ctxt& b1 = tmp[0];
  Ctxt& b2 = tmp[1];
  Ctxt& b3 = tmp[2];
  Ctxt& b4 = tmp[3];
  Ctxt& c2 = d1;
  Ctxt& c3 = b1;
  Ctxt& c4 = b3;

  three4Two(&b1, &b2, in[0], in[1], in[2]); // b2 b1 = 3for2(in[0..2])
  three4Two(&b3, &b4, in[3], in[4], in[5]); // b4 b3 = 3for2(in[3..5])

  three4Two(&c1, &c2, in[6], &b1, &b3); // c2 c1 = 3for2(in[6],b1,b3)
  if (sizeLimit < 2)
    return;
  c3 = b2;

  c3 += b4; // c3 = b2 ^ b4
  c4 = b2;

  c4.multiplyBy(b4); // c4 = b2 * b4
  d2 = c2;

  d1 += c3; // d1 = c2 ^ c3 (d1 alias c2)
  if (sizeLimit < 3)
    return;
  d2.multiplyBy(c3);

  d2 += c4; // d2 = c4 ^ (c2*c3)
}

/* fifteen4Four: adding fifteen input bits, getting a 4-bit counter
 *
 * input: in[14..0]
 * ----------------
 *       b2 b1 = sum of in[2..0] (b2=msb, b1=lsb)
 *       b4 b3 = sum of in[5..3]
 *       b6 b5 = sum of in[8..6]
 *       b8 b7 = sum of in[11..9]
 *      b10 b9 = sum of in[14..12]
 * ------------
 *       b8 b7
 *      b10 b9
 *       c2 c1 = sum of b1,b3,b5
 *    c4 c3    = sum of b2,b4,b6
 * ------------
 *    c4 c3
 *       d2 d1 = sum of b7,b9,c1
 *    d4 d3    = sum of b8,b10,c2
 * ------------
 *          d1
 *    e2 e1    = sum of c3,d2,d3
 * e4 e3       = sum of c4,d4
 * ------------
 * f2 f1 e1 d1 = out[3..0]
 */
// The output Ctxts[0..3] must be initialized, can alias the inputs
static void fifteen4Four(const CtPtrs& out, const CtPtrs& in, long sizeLimit)
{
  // we need 6 scratch ciphertexts
  std::vector<Ctxt> tmp(8, *out[0]);

  // Aliases, referring to the scheme above.

  Ctxt& d1 = *out[0];
  Ctxt& e1 = *out[1];
  Ctxt& f1 = *out[2];
  Ctxt& f2 = *out[3];

  Ctxt& b1 = tmp[0];
  Ctxt& b2 = tmp[1];
  Ctxt& b3 = tmp[2];
  Ctxt& b4 = tmp[3];
  Ctxt& b5 = tmp[4];
  Ctxt& b6 = tmp[5];
  Ctxt& c1 = tmp[6];
  Ctxt& c2 = tmp[7];
  Ctxt& c3 = b1;
  Ctxt& c4 = b3;
  Ctxt& b7 = b5;
  Ctxt& b8 = b2;
  Ctxt& b9 = b4;
  Ctxt& b10 = b6;
  Ctxt& d2 = b7;
  Ctxt& d3 = b9;
  Ctxt& d4 = f2;
  Ctxt& e2 = c1;
  Ctxt& e3 = c2;
  Ctxt& e4 = f2;

  long nThreads = std::min(NTL::AvailableThreads(), 3L);
  NTL_EXEC_INDEX(nThreads, index) // run these three lines in parallel
  switch (index) {
  case 0:
    three4Two(&b1, &b2, in[0], in[1], in[2]); // b2 b1 = 3for2(in[0..2])
    if (nThreads > 1)
      break;
    // FALLTHROUGH
  case 1:
    three4Two(&b3, &b4, in[3], in[4], in[5]); // b4 b3 = 3for2(in[3..5])
    if (nThreads > 2)
      break;
    // FALLTHROUGH
  default:
    three4Two(&b5, &b6, in[6], in[7], in[8]); // b6 b5 = 3for2(in[6..8])
  }
  NTL_EXEC_INDEX_END

  three4Two(c1, c2, b1, b3, b5); // c2 c1 = 3for2(b1,b3,b5)

  three4Two(c3, c4, b2, b4, b6); // c4 c3 = 3for2(b2,b4,b6)

  nThreads = std::min(NTL::AvailableThreads(), 2L);
  NTL_EXEC_INDEX(nThreads, index) // run these two lines in parallel
  switch (index) {
  case 0:
    three4Two(&b7, &b8, in[9], in[10], in[11]); // b8 b7 = 3for2(in[9..11])
    if (nThreads > 1)
      break;
    // FALLTHROUGH
  default:
    three4Two(&b9, &b10, in[12], in[13], in[14]); // b10 b9 = 3for2(in[12..14])
  }
  NTL_EXEC_INDEX_END

  NTL_EXEC_INDEX(nThreads, index) // run these two lines in parallel
  switch (index) {
  case 0:
    three4Two(d1, d2, b7, b9, c1); // d2 d1 = 3for2(b7,b9,c1)
    if (nThreads > 1)
      break;
    // FALLTHROUGH
  default:
    if (sizeLimit >= 2)
      three4Two(d3, d4, b8, b10, c2); // d4 d3 = 3for2(b8,b10,c2)
  }
  NTL_EXEC_INDEX_END
  if (sizeLimit < 2)
    return;

  NTL_EXEC_INDEX(nThreads, index) // run these two blocks in parallel
  switch (index) {
  case 0:
    three4Two(e1, e2, c3, d2, d3); // e2 e1 = 3for2(c3,d2,d3)
    if (nThreads > 1)
      break;
    // FALLTHROUGH
  default:
    if (sizeLimit >= 3) {
      e3 = c4;
      e3 += d4;          // e3 = c4 ^ d4
      e4.multiplyBy(c4); // e4 = c4 * d4 (e4 alias d4)
    }
  }
  NTL_EXEC_INDEX_END
  if (sizeLimit < 3)
    return;

  f1 = e2;
  f1 += e3; // f1 = e2 ^ e3
  if (sizeLimit < 4)
    return;
  e2.multiplyBy(e3);
  f2 += e2; // f2 = e4^(e2*e3)  (f2 alias e4)
}

// Same as above, but some of the pointers may be null.
// Returns number of output bits that are not identically zero.
long fifteenOrLess4Four(const CtPtrs& out, const CtPtrs& in, long sizeLimit)
{
  HELIB_TIMER_START;
  long numNonNull = in.numNonNull();
  if (numNonNull > 7) {
    fifteen4Four(out, in, sizeLimit);
    return 4;
  }

  // At most 7 non-null pointers, collect them in the first entries of a vector
  long lastNonNull = -1;
  std::vector<Ctxt*> inPtrs(7, nullptr);
  for (long i = 0; i < 15; i++)
    if (in.isSet(i))
      inPtrs[++lastNonNull] = in[i];

  if (numNonNull > 3) {
    seven4Three(out, CtPtrs_vectorPt(inPtrs), sizeLimit);
    out[3]->clear(); // msb is zero
    return 3;
  }
  numNonNull = three4Two(out[0], out[1], inPtrs[0], inPtrs[1], inPtrs[2]);
  out[3]->clear(); // msb is zero
  out[2]->clear(); // 2nd msb is zero
  return numNonNull;
}

/********************************************************************/
/***************** test/debugging functions *************************/

// Decrypt the binary numbers that are encrypted in eNums. The bits
// are encrypted in a bit-sliced manner. Namely, encNums[0] contains
// the LSB of all the numbers, encNums[1] the next bits from all, etc.
// If allSlots==false then we only return the subcube with index i=0
// in the last dimension within each ciphertext. Namely, the bit for
// the j'th counter is found in slot of index j*sizeOf(lastDim).
void decryptBinaryNums(std::vector<long>& pNums,
                       const CtPtrs& eNums,
                       const SecKey& sKey,
                       const EncryptedArray& ea,
                       bool twosComplement,
                       bool allSlots)
{
  int offset = 1, size = ea.size();
  if (!allSlots) { // only slots of index i=0 in the last dimension
    offset = ea.sizeOfDimension(ea.dimension() - 1);
    size /= offset;
  }
  pNums.assign(size, 0); // initialize to zero

  for (int i = 0; i < lsize(eNums); i++)
    if (eNums.isSet(i)) {
      std::vector<long> slots;
      ea.decrypt(*eNums[i], sKey, slots);
      for (int j = 0; j < lsize(pNums); j++)
        if (twosComplement && i == lsize(eNums) - 1)
          pNums[j] -= (slots[j * offset] << i);
        else
          pNums[j] += (slots[j * offset] << i);
    }
}

/********************************************************************/
#ifdef HELIB_DEBUG

void AddDAG::printAddDAG(bool printCT)
{
  std::cout << "aSize=" << aSize << ", bSize=" << bSize << std::endl;
  std::cout << "The p[i,j]'s\n============\n";
  for (long delta = 0; delta < bSize; delta++) {
    std::cout << "delta=" << delta << std::endl;
    for (long j = 0; j < bSize - delta; j++) {
      long i = j + delta;
      DAGnode* node = findP(i, j);
      if (node == nullptr)
        continue;
      std::cout << node->nodeName() << ":{ lvl=";
      if (node->level == LONG_MAX)
        std::cout << "XX";
      else
        std::cout << node->level;
      std::cout << ", chLeft=" << int(node->childrenLeft)
                << ", ct=" << node->ct;
      if (node->parent2)
        std::cout << ", prnt2=" << node->parent2->nodeName();
      if (node->parent1)
        std::cout << ", prnt1=" << node->parent1->nodeName();
      std::cout << " }\n";
      if (printCT && node->ct != nullptr)
        decryptAndPrint(std::cout,
                        *(node->ct),
                        *dbgKey,
                        *dbgEa,
                        FLAG_PRINT_VEC);
    }
  }
  std::cout << "\nThe q[i,j]'s\n============\n";
  for (long delta = 0; delta < bSize; delta++) {
    std::cout << "delta=" << delta << std::endl;
    for (long j = 0; j < std::min(aSize, bSize - delta); j++) {
      long i = j + delta;
      DAGnode* node = findQ(i, j);
      if (node == nullptr)
        continue;
      std::cout << node->nodeName() << ":{ lvl=";
      if (node->level == LONG_MAX)
        std::cout << "XX";
      else
        std::cout << node->level;
      std::cout << ", chLeft=" << long(node->childrenLeft)
                << ", ct=" << node->ct;
      if (node->parent2)
        std::cout << ", prnt2=" << node->parent2->nodeName();
      if (node->parent1)
        std::cout << ", prnt1=" << node->parent1->nodeName();
      std::cout << " }\n";
      if (printCT && node->ct != nullptr)
        decryptAndPrint(std::cout,
                        *(node->ct),
                        *dbgKey,
                        *dbgEa,
                        FLAG_PRINT_VEC);
    }
  }
  std::cout << std::endl;
}

void decryptAndSum(std::ostream& s,
                   const CtPtrMat& numbers,
                   bool twosComplement)
{
  s << "sum(";
  long sum = 0;
  for (long i = 0; i < numbers.size(); i++) {
    std::vector<long> slots;
    const CtPtrs& num = numbers[i];
    decryptBinaryNums(slots, num, *dbgKey, *dbgEa, twosComplement);
    s << slots[0] << ' ';
    sum += slots[0];
  }
  s << ")=" << sum << std::endl;
}

#endif // ifdef HELIB_DEBUG

} // namespace helib


// Copyright 2017 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
//
// Endian-dependent test

#include <boost/hash2/hash_append.hpp>
#include <boost/hash2/fnv1a.hpp>
#include <boost/core/lightweight_test.hpp>
#include <vector>
#include <list>
#include <deque>
#include <set>
#if !defined(BOOST_NO_CXX11_HDR_FORWARD_LIST)
# include <forward_list>
#endif

template<class H, class T, class R> void test_sized_range( R r )
{
    int w[] = { 1, 2, 3, 4 };
    T v( w, w + sizeof(w) / sizeof(w[0]) );

    H h;

    hash_append( h, v );

    BOOST_TEST_EQ( h.result(), r );
}

template<class H, class R> void test( R r )
{
    test_sized_range< H, std::vector<int> >( r );
    test_sized_range< H, std::list<int> >( r );
    test_sized_range< H, std::deque<int> >( r );
    test_sized_range< H, std::set<int> >( r );
    test_sized_range< H, std::multiset<int> >( r );
#if !defined(BOOST_NO_CXX11_HDR_FORWARD_LIST)
    test_sized_range< H, std::forward_list<int> >( r );
#endif
}

int main()
{
    test<boost::hash2::fnv1a_32>( 1745310485ul );
    test<boost::hash2::fnv1a_64>( 8602726560470947781ull );

    return boost::report_errors();
}


#include <iostream>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <Eigen/Geometry>
#include <boost/format.hpp>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

int main() {

  std::vector<cv::Mat> color_imgs;
  std::vector<cv::Mat> depth_imgs;

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;

  ifstream fin("./pose.txt");
  if (!fin) {
    std::cerr << "no post.txt!" << std::endl;
    return 1;
  }

  for (int i = 0; i < 5; i++) {
    boost::format fmt("./%s/%d.%s");
    color_imgs.push_back(cv::imread((fmt%"color"%(i + 1)%"png").str()));
    depth_imgs.push_back(cv::imread((fmt%"depth"%(i + 1)%"pgm").str(), -1));

    double data[7] = { 0 };
    for (auto& d : data) {
      fin >> d;
    }

    Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
    Eigen::Isometry3d T(q);
    T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
    poses.push_back(T);
  }

  double cx = 325.5;
  double cy = 253.5;
  double fx = 518.0;
  double fy = 519.0;
  double depth_scale = 1000.0;

  std::cout << "image -> point cloud... ... ..." << std::endl;

  typedef pcl::PointXYZRGB PointT;
  typedef pcl::PointCloud<PointT> PointCloud;

  PointCloud::Ptr point_cloud(new PointCloud);

  for (int i = 0; i < 5; i++) {
    std::cout << "convert img......" << std::endl;

    cv::Mat color = color_imgs[i];
    cv::Mat depth = depth_imgs[i];
    Eigen::Isometry3d T = poses[i];

    for (int v = 0; v < color.rows; v++) {
      for (int u = 0; u < color.cols; u++) {
        unsigned int d = depth.ptr<unsigned short>(v)[u];
        if (d == 0) {
          continue;
        }

        Eigen::Vector3d point;
        point[2] = double(d) / depth_scale;
        point[0] = (u - cx) * point[2] / fx;
        point[1] = (v - cy) * point[2] / fy;
        Eigen::Vector3d point_world = T * point;

        PointT p;
        p.x = point_world[0];
        p.y = point_world[1];
        p.z = point_world[2];
        p.b = color.data[v * color.step + u * color.channels()];
        p.g = color.data[v * color.step + u * color.channels() + 1];
        p.r = color.data[v * color.step + u * color.channels() + 2];

        point_cloud->points.push_back(p);
      }
    }

  }

  point_cloud->is_dense = false;
  std::cout << "point sum = " << point_cloud->size() << std::endl;
  pcl::io::savePCDFileBinary("map2.pcd", *point_cloud);

  return 0;
}


#include <boost/graph/undirected_graph.hpp>

int main()
{
	 boost::undirected_graph<int> g;
}


//
// $Id: WhittakerSmoother.cpp 1191 2009-08-14 19:33:05Z chambm $
//
//
// Original author: Matt Chambers <matt.chambers <a.t> vanderbilt.edu>
//
// Copyright 2008 Vanderbilt University - Nashville, TN 37232
//
// 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.
//

#define PWIZ_SOURCE

#include "WhittakerSmoother.hpp"
#include "pwiz/utility/misc/Export.hpp"
#include "pwiz/utility/misc/Std.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include "pwiz/utility/math/MatrixInverse.hpp"
#include <iostream>

namespace pwiz {
namespace analysis {

PWIZ_API_DECL
WhittakerSmoother::WhittakerSmoother(double lambdaCoefficient)
    : lambda(lambdaCoefficient)
{
    if (lambdaCoefficient < 2.0)
        throw std::runtime_error("[WhittakerSmoother::ctor()] Invalid value for lamda coefficient; valid range is [2, infinity)");
}

PWIZ_API_DECL
void WhittakerSmoother::smooth_copy(vector<double>& x, vector<double>& y)
{
    smooth(x, y, x, y);
}

PWIZ_API_DECL
void WhittakerSmoother::smooth(const std::vector<double>& x,
                               const std::vector<double>& y,
                               std::vector<double>& xSmoothed,
                               std::vector<double>& ySmoothed)
{
    using namespace boost::numeric::ublas;
    using std::cerr;
    using std::endl;

    typedef compressed_matrix<double> Matrix;
    typedef compressed_matrix<double>::iterator1 Iterator1;
    typedef compressed_matrix<double>::iterator2 Iterator2;

    size_t m = y.size();
    identity_matrix<double> E(m);
    Matrix D(m-1, m, (m-1)*2);

    for (size_t row = 0; row < m-1; ++row)
        for (size_t col = 0; col < m; ++col)
            if (row == col)
                D(row,col) = -1;
            else if (row == col - 1)
                D(row,col) = 1;

    Matrix transD, t1, temp;
    transD.resize(m, m-1, false);
    t1.resize(m, m, false);
    temp.resize(m, m, false);
    noalias(transD) = trans(D);
    noalias(t1) = prod(transD, D);
    noalias(temp) = E + (lambda * t1);
    boost::numeric::ublas::vector<double> yv(y.size());
    std::copy(y.begin(), y.end(), yv.begin());
    bool singular;
    Matrix temp2 = gjinverse<double>(temp, singular);
    boost::numeric::ublas::vector<double> z = prod(temp2, yv);
    ySmoothed.resize(z.size());
    std::copy(z.begin(), z.end(), ySmoothed.begin());

    /*for (Iterator1 i1 = D.begin1(); i1 != D.end1(); ++i1)
    {
        for (Iterator2 i2 = i1.begin(); i2 != i1.end(); ++i2)
            cerr << "(" << i2.index1() << "," << i2.index2()
                 << ":" << *i2 << ")  ";
        cerr << endl;
    }*/
}

} // namespace analysis
} // namespace msdata


#include <boost/mpl/assert.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/meta/make_integer.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::is_same;
using boost::mpl::_;
using nt2::meta::make_integer;

using nt2::uint32_t;
using nt2::uint64_t;

template<typename T>
struct test {};

int main()
{
  BOOST_MPL_ASSERT(( is_same<uint32_t,       make_integer<4>::type>                     ));
  BOOST_MPL_ASSERT(( is_same<int64_t,        make_integer<8, signed>::type>             ));
  BOOST_MPL_ASSERT(( is_same<test<uint64_t>, make_integer<8, unsigned, test<_> >::type> ));
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::algorithm::failure_times.hpp                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_FAILURE_TIMES_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_FAILURE_TIMES_HPP_ER_2009
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/matrix_view/algorithm/transform_column.hpp>
#include <boost/statistics/survival/data/data/record.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

    template<unsigned k,typename ItR,typename ItO>
    ItO
    failure_times(
        ItR b,              // records
        ItR e,
        unsigned stride,
        ItO i_o             // output
    )
    {
        typedef typename iterator_value<ItR>::type record_;
        return matrix_view::transform_column<k>(
            b,
            e,
            stride,
            lambda::bind(&record_::failure_time,lambda::_1),
            i_o
        );
    }

}// data
}// survival
}// statistics
}// boost

#endif


#include<falkolib/Matching/NNMatcher.h>
#include <boost/graph/graph_concepts.hpp>
#include <falkolib/Feature/FALKO.h>
#include<falkolib/Feature/FALKOExtractor.h>
#include<flirtlib/feature/RangeDetector.h>
#include"extractgeometryfeature.h"
#include "opencv/highgui.h"
#include "../viz/viz.h"

#include <falkolib/Feature/BSC.h>
#include <falkolib/Feature/FALKOExtractor.h>

#include <falkolib/Feature/BSCExtractor.h>

#include <falkolib/Matching/NNMatcher.h>
#include <falkolib/Matching/CCDAMatcher.h>

using  namespace  geomeasurer;
using namespace falkolib;
using namespace std;
 int main()
 {
        sensor::rangeData ranges1,ranges2;
        std::string filedir="../geomeasurer/data/dataflr4scan001";
        std::string filedir1="../geomeasurer/data/dataflr4scan003";

	ranges1=io::fromFile(filedir);
	ranges2=io::fromFile(filedir1);
        PointCloud scan2pcd1=sensor::fromRangeData(ranges1);
	PointCloud scan2pcd2=sensor::fromRangeData(ranges2);
        featurePointSet ret_keypoints1;
	 featurePointSet ret_keypoints2;
	falkolib::FALKOExtractor fe;
	fe.setMinScoreTh(0);
	fe.setMinExtractionRange(0.5);
	fe.setMaxExtractionRange(30);
	fe.enableSubbeam(false);
	fe.setNMSRadius(0.2);
	fe.setNeighB(0.07);
	fe.setBRatio(3);//2.5
	fe.setGridSectors(36);//16

	falkolib::LaserScan scan1(ranges1.angle_min,6*3.1419f/4, ranges1.ranges.size());
	scan1.fromRanges(ranges1.ranges);
	std::vector<falkolib::FALKO> keypoints1;
	fe.extract(scan1, keypoints1);
	for(falkolib::FALKO keypoint:keypoints1)
	{
	  ret_keypoints1.push_back(scan2pcd1.points[keypoint.index]);
	}

	falkolib::LaserScan scan2(ranges2.angle_min,6*3.1419f/4, ranges2.ranges.size());
	scan2.fromRanges(ranges2.ranges);
        std::vector<falkolib::FALKO> keypoints2;
        fe.extract(scan2, keypoints2);
	for(falkolib::FALKO keypoint:keypoints2)
	{
	  ret_keypoints2.push_back(scan2pcd2.points[keypoint.index]);
	}

	 cv::Mat img1,img2;
         img1=viz::imshowPCDWithKeypoints("faklo", ranges1,ret_keypoints1);
	img2=viz::imshowPCDWithKeypoints("faklo", ranges2,ret_keypoints2);

	cout << "num keypoints1 extracted: " << keypoints1.size() << endl;
	cout << "num keypoints2 extracted: " << keypoints2.size() << endl;

	BSCExtractor<FALKO> bsc(16, 8);
	vector<BSC> bscDesc1;
	vector<BSC> bscDesc2;

	bsc.compute(scan1, keypoints1, bscDesc1);
	bsc.compute(scan2, keypoints2, bscDesc2);

	NNMatcher<FALKO> matcher;
	matcher.setDistanceThreshold(0.1);
	std::vector<std::pair<int, int> > assoNN;
	std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, assoNN) << endl;
	for (auto& match : assoNN) {
		if (match.second >= 0) {
			int i1 = match.first;
			int i2 = match.second;
			std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl;
		}
	}
	/*
     std::vector<std::pair<point3d,point3d> > kppairs;
    for(auto pair:assoNN)
    {
      int index1, index2;
      double score;
      point3d temp1=scan2pcd1.points[pair.first];
      point3d temp2=scan2pcd2.points[pair.second];
      kppairs.push_back(std::make_pair(temp1,temp2));
      std::cout<<"index1:"<<index1<<"      "<<"index2:"<<index2<<"    "<<"score:"<<score<<std::endl;
    }
    viz::imshowMatch(img1,img2,kppairs);*/

	cout << endl;
	CCDAMatcher<FALKO> cc;
	cc.setDistMin(0.05);
	cc.setDistTol(0.1);
	std::vector<std::pair<int, int> > assoCC;
	std::cout << "num matching CC: " << cc.match(keypoints1, keypoints2, assoCC) << endl;
	for (auto& match : assoCC) {
		if (match.second >= 0) {
			int i1 = match.first;
			int i2 = match.second;
			std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl;
		}
	}
    std::vector<std::pair<point3d,point3d> > kppairs1;
    for(auto pair:assoCC)
    {
      int index1, index2;
      double score;
      point3d temp1=scan2pcd1.points[keypoints1[pair.first].index];
      point3d temp2=scan2pcd2.points[keypoints2[pair.first].index];
      kppairs1.push_back(std::make_pair(temp1,temp2));
    }
    viz::imshowMatch(img1,img2,kppairs1);

}