/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

//#include <ros/ros.h>
#include <Eigen/Core>
#include <ar_track_alvar/filter/kinect_filtering.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>

namespace ar_track_alvar
{

  namespace gm=geometry_msgs;

  using std::vector;
  using std::cerr;
  using std::endl;
  using std::ostream;

  // Distance threshold for plane fitting: how far are points
  // allowed to be off the plane?
  const double distance_threshold_ = 0.005;

  PlaneFitResult fitPlane (ARCloud::ConstPtr cloud)
  {
    PlaneFitResult res;
    pcl::PointIndices::Ptr inliers=boost::make_shared<pcl::PointIndices>();

    pcl::SACSegmentation<ARPoint> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(distance_threshold_);

    seg.setInputCloud(cloud);
    seg.segment(*inliers, res.coeffs);

    pcl::ExtractIndices<ARPoint> extracter;
    extracter.setInputCloud(cloud);
    extracter.setIndices(inliers);
    extracter.setNegative(false);
    extracter.filter(*res.inliers);

    return res;
  }

  ARCloud::Ptr filterCloud (const ARCloud& cloud, const vector<cv::Point, Eigen::aligned_allocator<cv::Point> >& pixels)
  {
    ARCloud::Ptr out(new ARCloud());
    //ROS_INFO("  Filtering %zu pixels", pixels.size());
    //for (const cv::Point& p : pixels)
    for(size_t i=0; i<pixels.size(); i++)
      {
	const cv::Point& p = pixels[i];
	const ARPoint& pt = cloud(p.x, p.y);
	if (isnan(pt.x) || isnan(pt.y) || isnan(pt.z)){
	  //ROS_INFO("    Skipping (%.4f, %.4f, %.4f)", pt.x, pt.y, pt.z);
	}
	else
	  out->points.push_back(pt);
      }
    return out;
  }

  gm::Point centroid (const ARCloud& points)
  {
    gm::Point sum;
    sum.x = 0;
    sum.y = 0;
    sum.z = 0;
    //for (const Point& p : points)
    for(size_t i=0; i<points.size(); i++)
      {
	sum.x += points[i].x;
	sum.y += points[i].y;
	sum.z += points[i].z;
      }

    gm::Point center;
    const size_t n = points.size();
    center.x = sum.x/n;
    center.y = sum.y/n;
    center.z = sum.z/n;
    return center;
  }

  // Helper function to construct a geometry_msgs::Quaternion
  inline
  gm::Quaternion makeQuaternion (double x, double y, double z, double w)
  {
    gm::Quaternion q;
    q.x = x;
    q.y = y;
    q.z = z;
    q.w = w;
    return q;
  }

  // Extract and normalize plane coefficients
  int getCoeffs (const pcl::ModelCoefficients& coeffs, double* a, double* b,
		 double* c, double* d)
  {
    if(coeffs.values.size() != 4)
      return -1;
    const double s = coeffs.values[0]*coeffs.values[0] +
      coeffs.values[1]*coeffs.values[1] + coeffs.values[2]*coeffs.values[2];
    if(fabs(s) < 1e-6)
      return -1;
    *a = coeffs.values[0]/s;
    *b = coeffs.values[1]/s;
    *c = coeffs.values[2]/s;
    *d = coeffs.values[3]/s;
    return 0;
  }

  // Project point onto plane
  tf::Vector3 project (const ARPoint& p, const double a, const double b,
		     const double c, const double d)
  {
    const double t = a*p.x + b*p.y + c*p.z + d;
    return tf::Vector3(p.x-t*a, p.y-t*b, p.z-t*c);
  }

  ostream& operator<< (ostream& str, const tf::Matrix3x3& m)
  {
    str << "[" << m[0][0] << ", " << m[0][1] << ", " << m[0][2] << "; "
	<< m[1][0] << ", " << m[1][1] << ", " << m[1][2] << "; "
	<< m[2][0] << ", " << m[2][1] << ", " << m[2][2] << "]";
    return str;
  }

  ostream& operator<< (ostream& str, const tf::Quaternion& q)
  {
    str << "[(" << q.x() << ", " << q.y() << ", " << q.z() <<
      "), " << q.w() << "]";
    return str;
  }

  ostream& operator<< (ostream& str, const tf::Vector3& v)
  {
    str << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")";
    return str;
  }

  int extractFrame (const pcl::ModelCoefficients& coeffs,
		    const ARPoint& p1, const ARPoint& p2,
		    const ARPoint& p3, const ARPoint& p4,
            tf::Matrix3x3 &retmat)
  {
    // Get plane coeffs and project points onto the plane
    double a=0, b=0, c=0, d=0;
    if(getCoeffs(coeffs, &a, &b, &c, &d) < 0)
      return -1;

    const tf::Vector3 q1 = project(p1, a, b, c, d);
    const tf::Vector3 q2 = project(p2, a, b, c, d);
    const tf::Vector3 q3 = project(p3, a, b, c, d);
    const tf::Vector3 q4 = project(p4, a, b, c, d);

    // Make sure points aren't the same so things are well-defined
    if((q2-q1).length() < 1e-3)
      return -1;

    // (inverse) matrix with the given properties
    const tf::Vector3 v = (q2-q1).normalized();
    const tf::Vector3 n(a, b, c);
    const tf::Vector3 w = -v.cross(n);
    tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);

    // Possibly flip things based on third point
    const tf::Vector3 diff = (q4-q3).normalized();
    //ROS_INFO_STREAM("w = " << w << " and d = " << diff);
    if (w.dot(diff)<0)
      {
	//ROS_INFO_STREAM("Flipping normal based on p3.  Current value: " << m);
	m[1] = -m[1];
	m[2] = -m[2];
	//ROS_INFO_STREAM("New value: " << m);
      }

    // Invert and return
    retmat = m.inverse();
    //cerr << "Frame is " << retmat << endl;
    return 0;
  }

  int getQuaternion (const tf::Matrix3x3& m, tf::Quaternion &retQ)
  {
    if(m.determinant() <= 0)
      return -1;

    //tfScalar y=0, p=0, r=0;
    //m.getEulerZYX(y, p, r);
    //retQ.setEulerZYX(y, p, r);

    //Use Eigen for this part instead, because the ROS version of bullet appears to have a bug
    Eigen::Matrix3f eig_m;
    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            eig_m(i,j) = m[i][j];
        }
    }
    Eigen::Quaternion<float> eig_quat(eig_m);

    // Translate back to bullet
    tfScalar ex = eig_quat.x();
    tfScalar ey = eig_quat.y();
    tfScalar ez = eig_quat.z();
    tfScalar ew = eig_quat.w();
    tf::Quaternion quat(ex,ey,ez,ew);
    retQ = quat.normalized();

    return 0;
  }

  int extractOrientation (const pcl::ModelCoefficients& coeffs,
			  const ARPoint& p1, const ARPoint& p2,
			  const ARPoint& p3, const ARPoint& p4,
			  gm::Quaternion &retQ)
  {
    tf::Matrix3x3 m;
    if(extractFrame(coeffs, p1, p2, p3, p4, m) < 0)
      return -1;
    tf::Quaternion q;
    if(getQuaternion(m,q) < 0)
      return -1;
    retQ.x = q.x();
    retQ.y = q.y();
    retQ.z = q.z();
    retQ.w = q.w();
    return 0;
  }

} // namespace


#include <cmath>
#include <iostream>
#include <fstream>

#include <boost/shared_ptr.hpp>
#include <gtest/gtest.h>

#include "ces.h"
#include "fdict.h"
#include "hg.h"
#include "kbest.h"
#include "hg_io.h"
#include "filelib.h"
#include "inside_outside.h"
#include "viterbi.h"
#include "viterbi_envelope.h"
#include "line_optimizer.h"
#include "scorer.h"

using namespace std;
using boost::shared_ptr;

class OptTest : public testing::Test {
 protected:
   virtual void SetUp() { }
   virtual void TearDown() { }
};

const char* ref11 = "australia reopens embassy in manila";
const char* ref12 = "( afp , manila , january 2 ) australia reopened its embassy in the philippines today , which was shut down about seven weeks ago due to what was described as a specific threat of a terrorist attack .";
const char* ref21 = "australia reopened manila embassy";
const char* ref22 = "( agence france-presse , manila , 2nd ) - australia reopened its embassy in the philippines today . the embassy was closed seven weeks ago after what was described as a specific threat of a terrorist attack .";
const char* ref31 = "australia to reopen embassy in manila";
const char* ref32 = "( afp report from manila , january 2 ) australia reopened its embassy in the philippines today . seven weeks ago , the embassy was shut down due to so - called confirmed terrorist attack threats .";
const char* ref41 = "australia to re - open its embassy to manila";
const char* ref42 = "( afp , manila , thursday ) australia reopens its embassy to manila , which was closed for the so - called \" clear \" threat of terrorist attack 7 weeks ago .";

TEST_F(OptTest, TestCheckNaN) {
  double x = 0;
  double y = 0;
  double z = x / y;
  EXPECT_EQ(true, isnan(z));
}

TEST_F(OptTest,TestViterbiEnvelope) {
  shared_ptr<Segment> a1(new Segment(-1, 0));
  shared_ptr<Segment> b1(new Segment(1, 0));
  shared_ptr<Segment> a2(new Segment(-1, 1));
  shared_ptr<Segment> b2(new Segment(1, -1));
  vector<shared_ptr<Segment> > sa; sa.push_back(a1); sa.push_back(b1);
  vector<shared_ptr<Segment> > sb; sb.push_back(a2); sb.push_back(b2);
  ViterbiEnvelope a(sa);
  cerr << a << endl;
  ViterbiEnvelope b(sb);
  ViterbiEnvelope c = a;
  c *= b;
  cerr << a << " (*) " << b << " = " << c << endl;
  EXPECT_EQ(3, c.size());
}

TEST_F(OptTest,TestViterbiEnvelopeInside) {
  const string json = "{\"rules\":[1,\"[X] ||| a\",2,\"[X] ||| A [1]\",3,\"[X] ||| c\",4,\"[X] ||| C [1]\",5,\"[X] ||| [1] B [2]\",6,\"[X] ||| [1] b [2]\",7,\"[X] ||| X [1]\",8,\"[X] ||| Z [1]\"],\"features\":[\"f1\",\"f2\",\"Feature_1\",\"Feature_0\",\"Model_0\",\"Model_1\",\"Model_2\",\"Model_3\",\"Model_4\",\"Model_5\",\"Model_6\",\"Model_7\"],\"edges\":[{\"tail\":[],\"feats\":[],\"rule\":1}],\"node\":{\"in_edges\":[0]},\"edges\":[{\"tail\":[0],\"feats\":[0,-0.8,1,-0.1],\"rule\":2}],\"node\":{\"in_edges\":[1]},\"edges\":[{\"tail\":[],\"feats\":[1,-1],\"rule\":3}],\"node\":{\"in_edges\":[2]},\"edges\":[{\"tail\":[2],\"feats\":[0,-0.2,1,-0.1],\"rule\":4}],\"node\":{\"in_edges\":[3]},\"edges\":[{\"tail\":[1,3],\"feats\":[0,-1.2,1,-0.2],\"rule\":5},{\"tail\":[1,3],\"feats\":[0,-0.5,1,-1.3],\"rule\":6}],\"node\":{\"in_edges\":[4,5]},\"edges\":[{\"tail\":[4],\"feats\":[0,-0.5,1,-0.8],\"rule\":7},{\"tail\":[4],\"feats\":[0,-0.7,1,-0.9],\"rule\":8}],\"node\":{\"in_edges\":[6,7]}}";
  Hypergraph hg;
  istringstream instr(json);
  HypergraphIO::ReadFromJSON(&instr, &hg);
  SparseVector<double> wts;
  wts.set_value(FD::Convert("f1"), 0.4);
  wts.set_value(FD::Convert("f2"), 1.0);
  hg.Reweight(wts);
  vector<pair<vector<WordID>, prob_t> > list;
  std::vector<SparseVector<double> > features;
  KBest::KBestDerivations<vector<WordID>, ESentenceTraversal> kbest(hg, 10);
  for (int i = 0; i < 10; ++i) {
    const KBest::KBestDerivations<vector<WordID>, ESentenceTraversal>::Derivation* d =
      kbest.LazyKthBest(hg.nodes_.size() - 1, i);
    if (!d) break;
    cerr << log(d->score) << " ||| " << TD::GetString(d->yield) << " ||| " << d->feature_values << endl;
  }
  SparseVector<double> dir; dir.set_value(FD::Convert("f1"), 1.0);
  ViterbiEnvelopeWeightFunction wf(wts, dir);
  ViterbiEnvelope env = Inside<ViterbiEnvelope, ViterbiEnvelopeWeightFunction>(hg, NULL, wf);
  cerr << env << endl;
  const vector<boost::shared_ptr<Segment> >& segs = env.GetSortedSegs();
  dir *= segs[1]->x;
  wts += dir;
  hg.Reweight(wts);
  KBest::KBestDerivations<vector<WordID>, ESentenceTraversal> kbest2(hg, 10);
  for (int i = 0; i < 10; ++i) {
    const KBest::KBestDerivations<vector<WordID>, ESentenceTraversal>::Derivation* d =
      kbest2.LazyKthBest(hg.nodes_.size() - 1, i);
    if (!d) break;
    cerr << log(d->score) << " ||| " << TD::GetString(d->yield) << " ||| " << d->feature_values << endl;
  }
  for (int i = 0; i < segs.size(); ++i) {
    cerr << "seg=" << i << endl;
    vector<WordID> trans;
    segs[i]->ConstructTranslation(&trans);
    cerr << TD::GetString(trans) << endl;
  }
}

TEST_F(OptTest, TestS1) {
  int fPhraseModel_0 = FD::Convert("PhraseModel_0");
  int fPhraseModel_1 = FD::Convert("PhraseModel_1");
  int fPhraseModel_2 = FD::Convert("PhraseModel_2");
  int fLanguageModel = FD::Convert("LanguageModel");
  int fWordPenalty = FD::Convert("WordPenalty");
  int fPassThrough = FD::Convert("PassThrough");
  SparseVector<double> wts;
  wts.set_value(fWordPenalty, 4.25);
  wts.set_value(fLanguageModel, -1.1165);
  wts.set_value(fPhraseModel_0, -0.96);
  wts.set_value(fPhraseModel_1, -0.65);
  wts.set_value(fPhraseModel_2, -0.77);
  wts.set_value(fPassThrough, -10.0);

  vector<int> to_optimize;
  to_optimize.push_back(fWordPenalty);
  to_optimize.push_back(fLanguageModel);
  to_optimize.push_back(fPhraseModel_0);
  to_optimize.push_back(fPhraseModel_1);
  to_optimize.push_back(fPhraseModel_2);

  Hypergraph hg;
  ReadFile rf("./test_data/0.json.gz");
  HypergraphIO::ReadFromJSON(rf.stream(), &hg);
  hg.Reweight(wts);

  Hypergraph hg2;
  ReadFile rf2("./test_data/1.json.gz");
  HypergraphIO::ReadFromJSON(rf2.stream(), &hg2);
  hg2.Reweight(wts);

  vector<vector<WordID> > refs1(4);
  TD::ConvertSentence(ref11, &refs1[0]);
  TD::ConvertSentence(ref21, &refs1[1]);
  TD::ConvertSentence(ref31, &refs1[2]);
  TD::ConvertSentence(ref41, &refs1[3]);
  vector<vector<WordID> > refs2(4);
  TD::ConvertSentence(ref12, &refs2[0]);
  TD::ConvertSentence(ref22, &refs2[1]);
  TD::ConvertSentence(ref32, &refs2[2]);
  TD::ConvertSentence(ref42, &refs2[3]);
  ScoreType type = ScoreTypeFromString("ibm_bleu");
  ScorerP scorer1 = SentenceScorer::CreateSentenceScorer(type, refs1);
  ScorerP scorer2 = SentenceScorer::CreateSentenceScorer(type, refs2);
  vector<ViterbiEnvelope> envs(2);

  RandomNumberGenerator<boost::mt19937> rng;

  vector<SparseVector<double> > axes; // directions to search
  LineOptimizer::CreateOptimizationDirections(
     to_optimize,
     10,
     &rng,
     &axes);
  assert(axes.size() == 10 + to_optimize.size());
  for (int i = 0; i < axes.size(); ++i)
    cerr << axes[i] << endl;
  const SparseVector<double>& axis = axes[0];

  cerr << "Computing Viterbi envelope using inside algorithm...\n";
  cerr << "axis: " << axis << endl;
  clock_t t_start=clock();
  ViterbiEnvelopeWeightFunction wf(wts, axis);  // wts = starting point, axis = search direction
  envs[0] = Inside<ViterbiEnvelope, ViterbiEnvelopeWeightFunction>(hg, NULL, wf);
  envs[1] = Inside<ViterbiEnvelope, ViterbiEnvelopeWeightFunction>(hg2, NULL, wf);

  vector<ErrorSurface> es(2);
  ComputeErrorSurface(*scorer1, envs[0], &es[0], IBM_BLEU, hg);
  ComputeErrorSurface(*scorer2, envs[1], &es[1], IBM_BLEU, hg2);
  cerr << envs[0].size() << " " << envs[1].size() << endl;
  cerr << es[0].size() << " " << es[1].size() << endl;
  envs.clear();
  clock_t t_env=clock();
  float score;
  double m = LineOptimizer::LineOptimize(es, LineOptimizer::MAXIMIZE_SCORE, &score);
  clock_t t_opt=clock();
  cerr << "line optimizer returned: " << m << " (SCORE=" << score << ")\n";
  EXPECT_FLOAT_EQ(0.48719698, score);
  SparseVector<double> res = axis;
  res *= m;
  res += wts;
  cerr << "res: " << res << endl;
  cerr << "ENVELOPE PROCESSING=" << (static_cast<double>(t_env - t_start) / 1000.0) << endl;
  cerr << "  LINE OPTIMIZATION=" << (static_cast<double>(t_opt - t_env) / 1000.0) << endl;
  hg.Reweight(res);
  hg2.Reweight(res);
  vector<WordID> t1,t2;
  ViterbiESentence(hg, &t1);
  ViterbiESentence(hg2, &t2);
  cerr << TD::GetString(t1) << endl;
  cerr << TD::GetString(t2) << endl;
}

TEST_F(OptTest,TestZeroOrigin) {
  const string json = "{\"rules\":[1,\"[X7] ||| blA ||| without ||| LHSProb=3.92173 LexE2F=2.90799 LexF2E=1.85003 GenerativeProb=10.5381 RulePenalty=1 XFE=2.77259 XEF=0.441833 LabelledEF=2.63906 LabelledFE=4.96981 LogRuleCount=0.693147\",2,\"[X7] ||| blA ||| except ||| LHSProb=4.92173 LexE2F=3.90799 LexF2E=1.85003 GenerativeProb=11.5381 RulePenalty=1 XFE=2.77259 XEF=1.44183 LabelledEF=2.63906 LabelledFE=4.96981 LogRuleCount=1.69315\",3,\"[S] ||| [X7,1] ||| [1] ||| GlueTop=1\",4,\"[X28] ||| EnwAn ||| title ||| LHSProb=3.96802 LexE2F=2.22462 LexF2E=1.83258 GenerativeProb=10.0863 RulePenalty=1 XFE=0 XEF=1.20397 LabelledEF=1.20397 LabelledFE=-1.98341e-08 LogRuleCount=1.09861\",5,\"[X0] ||| EnwAn ||| funny ||| LHSProb=3.98479 LexE2F=1.79176 LexF2E=3.21888 GenerativeProb=11.1681 RulePenalty=1 XFE=0 XEF=2.30259 LabelledEF=2.30259 LabelledFE=0 LogRuleCount=0 SingletonRule=1\",6,\"[X8] ||| [X7,1] EnwAn ||| entitled [1] ||| LHSProb=3.82533 LexE2F=3.21888 LexF2E=2.52573 GenerativeProb=11.3276 RulePenalty=1 XFE=1.20397 XEF=1.20397 LabelledEF=2.30259 LabelledFE=2.30259 LogRuleCount=0 SingletonRule=1\",7,\"[S] ||| [S,1] [X28,2] ||| [1] [2] ||| Glue=1\",8,\"[S] ||| [S,1] [X0,2] ||| [1] [2] ||| Glue=1\",9,\"[S] ||| [X8,1] ||| [1] ||| GlueTop=1\",10,\"[Goal] ||| [S,1] ||| [1]\"],\"features\":[\"PassThrough\",\"Glue\",\"GlueTop\",\"LanguageModel\",\"WordPenalty\",\"LHSProb\",\"LexE2F\",\"LexF2E\",\"GenerativeProb\",\"RulePenalty\",\"XFE\",\"XEF\",\"LabelledEF\",\"LabelledFE\",\"LogRuleCount\",\"SingletonRule\"],\"edges\":[{\"tail\":[],\"spans\":[0,1,-1,-1],\"feats\":[5,3.92173,6,2.90799,7,1.85003,8,10.5381,9,1,10,2.77259,11,0.441833,12,2.63906,13,4.96981,14,0.693147],\"rule\":1},{\"tail\":[],\"spans\":[0,1,-1,-1],\"feats\":[5,4.92173,6,3.90799,7,1.85003,8,11.5381,9,1,10,2.77259,11,1.44183,12,2.63906,13,4.96981,14,1.69315],\"rule\":2}],\"node\":{\"in_edges\":[0,1],\"cat\":\"X7\"},\"edges\":[{\"tail\":[0],\"spans\":[0,1,-1,-1],\"feats\":[2,1],\"rule\":3}],\"node\":{\"in_edges\":[2],\"cat\":\"S\"},\"edges\":[{\"tail\":[],\"spans\":[1,2,-1,-1],\"feats\":[5,3.96802,6,2.22462,7,1.83258,8,10.0863,9,1,11,1.20397,12,1.20397,13,-1.98341e-08,14,1.09861],\"rule\":4}],\"node\":{\"in_edges\":[3],\"cat\":\"X28\"},\"edges\":[{\"tail\":[],\"spans\":[1,2,-1,-1],\"feats\":[5,3.98479,6,1.79176,7,3.21888,8,11.1681,9,1,11,2.30259,12,2.30259,15,1],\"rule\":5}],\"node\":{\"in_edges\":[4],\"cat\":\"X0\"},\"edges\":[{\"tail\":[0],\"spans\":[0,2,-1,-1],\"feats\":[5,3.82533,6,3.21888,7,2.52573,8,11.3276,9,1,10,1.20397,11,1.20397,12,2.30259,13,2.30259,15,1],\"rule\":6}],\"node\":{\"in_edges\":[5],\"cat\":\"X8\"},\"edges\":[{\"tail\":[1,2],\"spans\":[0,2,-1,-1],\"feats\":[1,1],\"rule\":7},{\"tail\":[1,3],\"spans\":[0,2,-1,-1],\"feats\":[1,1],\"rule\":8},{\"tail\":[4],\"spans\":[0,2,-1,-1],\"feats\":[2,1],\"rule\":9}],\"node\":{\"in_edges\":[6,7,8],\"cat\":\"S\"},\"edges\":[{\"tail\":[5],\"spans\":[0,2,-1,-1],\"feats\":[],\"rule\":10}],\"node\":{\"in_edges\":[9],\"cat\":\"Goal\"}}";
  Hypergraph hg;
  istringstream instr(json);
  HypergraphIO::ReadFromJSON(&instr, &hg);
  SparseVector<double> wts;
  wts.set_value(FD::Convert("PassThrough"), -0.929201533002898);
  hg.Reweight(wts);

  vector<pair<vector<WordID>, prob_t> > list;
  std::vector<SparseVector<double> > features;
  KBest::KBestDerivations<vector<WordID>, ESentenceTraversal> kbest(hg, 10);
  for (int i = 0; i < 10; ++i) {
    const KBest::KBestDerivations<vector<WordID>, ESentenceTraversal>::Derivation* d =
      kbest.LazyKthBest(hg.nodes_.size() - 1, i);
    if (!d) break;
    cerr << log(d->score) << " ||| " << TD::GetString(d->yield) << " ||| " << d->feature_values << endl;
  }

  SparseVector<double> axis; axis.set_value(FD::Convert("Glue"),1.0);
  ViterbiEnvelopeWeightFunction wf(wts, axis);  // wts = starting point, axis = search direction
  vector<ViterbiEnvelope> envs(1);
  envs[0] = Inside<ViterbiEnvelope, ViterbiEnvelopeWeightFunction>(hg, NULL, wf);

  ScoreType type = ScoreTypeFromString("ibm_bleu");
  vector<vector<WordID> > mr(4);
  TD::ConvertSentence("untitled", &mr[0]);
  TD::ConvertSentence("with no title", &mr[1]);
  TD::ConvertSentence("without a title", &mr[2]);
  TD::ConvertSentence("without title", &mr[3]);
  ScorerP scorer1 = SentenceScorer::CreateSentenceScorer(type, mr);
  vector<ErrorSurface> es(1);
  ComputeErrorSurface(*scorer1, envs[0], &es[0], IBM_BLEU, hg);
}

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#ifndef UTIL_HPP
#define UTIL_HPP

#include <cuda_runtime.h>
#include <cublas_v2.h>
#include <cusolverDn.h>
#include <thrust/device_ptr.h>
#include <thrust/device_malloc.h>
#include <thrust/device_free.h>
#include <thrust/device_vector.h>
#include <thrust/functional.h>
#include <thrust/transform_reduce.h>
#include <thrust/copy.h>
#include <cub/cub.cuh>

#include <Eigen/Dense>

#include "types.hpp"
#include "strided_range.hpp"

#define WARP_SIZE 32
#define FULL_MASK 0xffffffff

// ----------------------------------------------
// ----   Utility functions ---------------------

/// Fail if a recent cusolver call did not succeed
inline void check_cusolver_status(cusolverStatus_t status, int info_h)
{
    if (status || info_h) {
	std::string msg;
	std::stringstream smsg(msg);
	smsg << "Error in potrf: return code " << status << ", info " << info_h;
	throw std::runtime_error(smsg.str());
    }
}

/// Can a usable CUDA capable device be found?
inline bool have_compatible_device(void)
{
    int device_count = 0;
    cudaError_t err = cudaGetDeviceCount(&device_count);

    return (err == cudaSuccess && device_count > 0);

}
// ----------------------------------------------

void identity_device(int N, double *A);
void add_diagonal(int N, double b, double *A);
void sum_log_diag(int N, double *A, double *result, double *work, size_t work_size);
void trace(int N, double *A, double *result, double *work, size_t work_size);

#endif


// bloom_filter class implementation
//

#include "bloom_filter.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#ifndef _M_IX86
#include <smmintrin.h>
#endif

#include <boost/math/special_functions/round.hpp>

#include "../lz4/lz4.h"
#include "sdbf_defines.h"

const uint32_t
bloom_filter::BITS[] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};

const uint32_t
bloom_filter::BIT_MASKS_32[] = {
    0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F, 0xFF,
    0x01FF, 0x03FF, 0x07FF, 0x0FFF, 0x1FFF, 0x3FFF, 0x7FFF, 0xFFFF,
    0x01FFFF, 0x03FFFF, 0x07FFFF, 0x0FFFFF, 0x1FFFFF, 0x3FFFFF, 0x7FFFFF, 0xFFFFFF,
    0x01FFFFFF, 0x03FFFFFF, 0x07FFFFFF, 0x0FFFFFFF, 0x1FFFFFFF, 0x3FFFFFFF, 0x7FFFFFFF, 0xFFFFFFFF
};

/// cutoffs for LARGE filters 16kb
const
uint32_t bloom_filter::CUTOFFS[]={
    86511,86511,86511,86511 , 67010 , 52623 , 42139 , 34377 , 28532 , 24026 , 20499 , 17687 , 15407 , 13535 , 11982 ,
    10685 , 9589 , 8652 , 7846 , 7149 , 6541 , 6008 , 5537 , 5121 , 4745 , 4413 , 4115 , 3850 ,
    3606 , 3388 , 3185 , 3001 , 2834 , 2681 , 2538 , 2407 , 2287 , 2176 , 2072 , 1977 , 1888 , 1802 ,
    1724 , 1651 , 1583 , 1519 , 1458 , 1402 , 1348 , 1298 , 1248 , 1204 , 1161 , 1120 , 1083 , 1047 ,
    1013 , 981 , 949 , 921 , 892 , 866 , 839 , 815 , 791 , 768 , 747 , 726 , 706 , 688 , 669 , 652 ,
    635 , 619 , 603 , 589 , 575 , 561 , 546 , 533 , 521 , 510 , 498 , 487 , 476 , 467 , 456 , 447 ,
    438 , 429 , 420 , 411 , 403 , 395 , 387 , 380 , 373 , 365 , 358 , 351 , 345 , 338 , 332 , 326 ,
    320 , 314 , 309 , 303 , 298 , 293 , 288 , 284 , 279 , 275 , 271 , 266 , 262 , 258 , 254 , 250 ,
    246 , 242 , 238 , 235 , 231 , 228 , 225 , 221 , 218
};

/**
   Create new empty bloom filter
   \param size of bloom filter
   \param hash_count number of hashes for each insertion or query
   \param max_elem max element size (0 ok)
   \param max_fp max false positive rate (0 ok)
*/
bloom_filter::bloom_filter( uint64_t size, uint16_t hash_count, uint64_t max_elem, double max_fp) {
    this->bf_size = size;
    this->hash_count = hash_count;
    this->max_elem = max_elem;
    this->max_fp = max_fp;

    // Make sure size if a power of 2 and at least 64
    if( size && !(size & (size - 1)) && size >= 64) {
        // Find log2(size)
        uint16_t log_size = 0;
        for( uint64_t tmp=size; tmp; tmp >>= 1, log_size++);
        bit_mask = BIT_MASKS_32[log_size+1];
    } else {
        throw -1; // sizes invalid
    }
    bf = (uint8_t*)malloc(size);
    memset( bf, 0, size);
    bf_elem_count = 0;
    created=true;
}

/**
    Read bloom filter(INDEX)  from a file
    \param indexfilename file to read
*/
bloom_filter::bloom_filter(string indexfilename){
    ifstream ifs(indexfilename.c_str(),ifstream::in|ios::binary);
    if (ifs.is_open()) {
        string process;
        // headerbit
        getline(ifs,process,':'); // ignore
        // bf_size
        getline(ifs,process,':');
        bf_size=boost::lexical_cast<uint64_t>(process);
        // elem_count
        getline(ifs,process,':');
        bf_elem_count=boost::lexical_cast<uint64_t>(process);
        // hash_count
        getline(ifs,process,':');
        hash_count=boost::lexical_cast<uint16_t>(process);
        // bit_mask
        getline(ifs,process,':');
        bit_mask=boost::lexical_cast<uint64_t>(process);
        // compressed_size
        getline(ifs,process,':');
        uint64_t comp_size=boost::lexical_cast<uint64_t>(process);
        // setname
        getline(ifs,setname);
        // endl
        // compressed data, size from header
        char* bf_comp =(char*)malloc(comp_size+1);
        bf = (uint8_t*) malloc(bf_size);
        ifs.read((char*)bf_comp,comp_size);
        // decompress.
        int32_t result=this->decompress(bf_comp);
        if (result < 0) // failed to parse
            throw -2;
        free(bf_comp);
    } else
       throw -1; // failed to read
    created=true;
    //created=false;
}

/**
    Creates bloom filter from existing buffer of bloom filter data.
       Experimental: sized for sdbf 256-byte bloom filters at the moment
    \param data buffer of bloom filter data
    \param size of bloom filter data
    \param id identifier for clustering bloom filters
    \param bf_elem_ct # of elements in filter
    \param hamming weight of filter
*/
bloom_filter::bloom_filter(uint8_t* data, uint64_t size, int id, int bf_elem_ct, uint16_t hamming) {
    // this makes several testing assumptions.
    // this is for 256-byte BFs,
    // and elem_ct = 192, bit_mask=2047 (pre-calculated)
    // and hash_count=5
    uint16_t log_size = 0;
    for( uint64_t tmp=size; tmp; tmp >>= 1, log_size++);
    bit_mask = BIT_MASKS_32[log_size+1];
    //bit_mask=2047;
    bf_size=size;
    hash_count=5;
    bf_elem_count=bf_elem_ct; // could also be 160
    bl_id = id;
    this->hamming = hamming;
    bf=(uint8_t*)malloc(size);
    memcpy(bf,data,size);
    // marker for non-destructive destroy?
    created=true;
}

/**
    Destroys bloom filter and frees buffer
*/
bloom_filter::~bloom_filter() {
    if (created)
        free(bf);
}

/**
    Returns number of elements present in bloom filter
    \returns number of elements
*/
uint64_t bloom_filter::elem_count() { return bf_elem_count;}

/**
    Returns estimated false positive rate (not implemented)
    \returns estimate
*/
double  bloom_filter::est_fp_rate() { return -1.0;}

/**
    Returns bits per element in bloom filter
    \returns estimate
*/
double bloom_filter::bits_per_elem() { return (double) (bf_size << 3)/bf_elem_count;}

/**
   \internal
   Compresses bloom filter for writing to disk and returns it
*/
char *
bloom_filter::compress() {
    char *dest=(char*)malloc(160*MB);
    int res = LZ4_compress_limitedOutput((const char*)bf,dest,bf_size,160*MB);
    if (res == 0) {
    comp_size = 0;
    free(dest);
        return NULL;
    } else {
        comp_size = res;
        return dest;
    }
}

/**
   \internal
   Decompresses bloom filter as read from file.  Arguments unpacked
   in read function.
   \param src contains source data
*/
int32_t
bloom_filter::decompress(char *src) {
    // bf already allocated to correct size
    int res = LZ4_uncompress((const char*)src,(char*)bf,bf_size);
    return res;
}

/**
   Writes bloom filter out to a file.
   \param filename file to be written
   \returns status -1 if compression fails, -2 if cannot open file
*/
int32_t
bloom_filter::write_out(string filename) {
    char *compressed=this->compress();
    if (compressed==NULL)
        return -1;
    std::filebuf fb;
    fb.open (filename.c_str(),ios::out|ios::binary);
    if (fb.is_open()) {
        std::ostream os(&fb);
        os << "sdbf-idx:" << bf_size << ":" << bf_elem_count << ":"<< hash_count;
        os << ":" << bit_mask << ":" << comp_size << ":";
        os << setname;
        os << endl;
        os.write(compressed,comp_size);
        fb.close();
    } else {
        free(compressed);
        return -2;
    }
    free(compressed);
    return 0;
}

/**
   Returns name associated with bloom filter
   \returns name
*/
string
bloom_filter::name() const{
     return setname;
}

/**
   Changes name associated with bloom filter
   \param name new name
*/
void
bloom_filter::set_name(string name) {
     setname = name;
}

int
bloom_filter::bloom_id() {
    return bl_id;
}

void
bloom_filter::set_bloom_id(int id) {
    this->bl_id = id;
}
/**
   Folds bloom filter by half N times by or'ing the
   second half of the bloom filter onto the first half.
   \param times amount of times to fold filter
*/
void
bloom_filter::fold(uint32_t times){
    // probably also need some sort of mutex while this happens
    // size divided by 8,to use 64-bit chunks, and cast the bf
    uint64_t rsize = bf_size/8;
    uint64_t *bf_64 = (uint64_t *)bf;
    for (uint32_t i=0; i<times; i++) {
        for (uint64_t j=0;j<rsize/2;j++)
            bf_64[j] |= bf_64[j+(rsize/2)];
        rsize=rsize/2;
        if (rsize == 32)
            break; // also error?
    }
    bf_size=rsize*8;
    // recalculate mask
        // Find log2(size)
    uint16_t log_size = 0;
    for( uint64_t tmp=bf_size; tmp; tmp >>= 1, log_size++);
    bit_mask = BIT_MASKS_32[log_size+1];
    uint8_t *oldbf=bf;
    bf = (uint8_t*)malloc(bf_size);
    memset(bf, 0, bf_size);
    // copy in
    memcpy(bf,oldbf,bf_size);
    // delete
    free(oldbf);
}

/**
   Adds another bloom filter to this one
   \param other bloom filter
   \return 0 if successful 1 if not the same size
*/
int
bloom_filter::add(bloom_filter *other) {
    uint64_t *bf_64 = (uint64_t *)bf;
    uint64_t *bf2_64 = (uint64_t *)other->bf;
    if (other->bf_size != bf_size)
        return 1; // must add two of same size
    for (uint32_t j=0;j < bf_size/8;j++)
        bf_64[j]|=bf2_64[j];
    return 0;
}

/**
   Inserts hash data into this bloom filter
   \param sha1 buffer of sha1 hash values
   \returns exists or not exists
*/
bool
bloom_filter::insert_sha1(uint32_t *sha1) {
    return query_and_set(sha1, true);
}

/**
   Queries this bloom filter with hash data
   \param sha1 buffer of sha1 hash values
   \returns exists or not exists
*/
bool
bloom_filter::query_sha1(uint32_t *sha1) {
    return query_and_set(sha1, false);
}

/**
   \internal
    Actual meat of insert/query functionality
   \param sha1
   \param mode_set true to set, false to query
   \returns exists or not exists
*/
bool
bloom_filter::query_and_set(uint32_t *sha1, bool mode_set) {

    uint32_t pos, i, k, bit_cnt=0;
    for( i=0; i<hash_count; i++) {
        pos = sha1[i] & bit_mask;
        k = pos >> 3;
        // Bit is set
        if( (bf[k] & BITS[pos & 0x7])) {
            bit_cnt++;
        // Bit is not set
        } else {
            if( mode_set)
                bf[k] |= BITS[pos & 0x7];
            else
                return false;
        }
    }
    if(mode_set) {
        if(bit_cnt < hash_count) {
            bf_elem_count++;
            return true;
        } else
            return false;
    } else
        return bit_cnt == hash_count;
}

int
bloom_filter::compare(bloom_filter *other, double scale) {
    if (this->bf_size != other->bf_size)
        return -1; // must compare equal sized filters
    uint64_t *bf_64 = (uint64_t *)bf;
    uint64_t *bf2_64 = (uint64_t *)other->bf;
    int64_t res=0;
#ifndef _M_IX86
    for (uint32_t i=0 ; i < bf_size / 8 ; i++) {
        res+=_mm_popcnt_u64(bf_64[i] & bf2_64[i]);
    }
#endif
    int max_est = (this->hamminglg < other->hamminglg) ? this->hamminglg : other->hamminglg;
    double m = bf_size*8;
    //double k = 5;
    //double exp = 1-1.0/m;
    int x=this->bf_elem_count;
    int y=other->bf_elem_count;
    //int min_est = boost::math::round((double)m*(1 - pow(exp,(double)k*x) - pow(exp,(double)k*y) + pow(exp,(double)k*(x+y))) );
    if (( x < 32) ||( y < 32))  // filtering out too-small filters
        return 0;
    int mn=boost::math::round((2*m)/(x+y));
    int cut_off=CUTOFFS[mn];
//cout << "x " << x << " y " << y << endl;
//cout << "scale*max_est-min_est " << scale*(double)(max_est-min_est) << " min_est " << min_est << endl;
//max_est = boost::math::round((double)max_est / (double)(bf_size/256));
if (mn > 128)
    cut_off=CUTOFFS[128]-(mn-128); // setting the cutoff to cutoff -n
if (cut_off < 0){
    return 0;
}
    int32_t result=(res > cut_off) ? boost::math::round<int32_t>(100.0*((double)(res-cut_off)/(double)(max_est-cut_off))) : 0;
//cout << mn << " " << res << " " << cut_off << " " << max_est<< " " << result << endl;
    //int32_t result=res; // TEMP DEBUGGING
    return result;

}
/**
   \internal
   Temporary for output
*/
string
bloom_filter::to_string () const {
    std::stringstream string_bf;
    //string_bf << "sdbf-mr:" << bf_size << ":" << bf_elem_count << ":"<< hash_count;
    //string_bf << ":" << bit_mask << ":0:" ; // 0 is compressed size, we are not compressed
    //string_bf << setname << ":" ;
    char *b64=b64encode((char*)bf,bf_size+3);
    string_bf << b64;
   // string_bf << endl;
    free(b64);
    return string_bf.str();
}

// possibly change folds to target size?
bloom_filter::bloom_filter(string filter, int folds) {
    std::istringstream iss(filter);
    //if (ifs.is_open()) {
    string process;
    // headerbit
    getline(iss,process,':'); // ignore
    // bf_size
    getline(iss,process,':');
    bf_size=boost::lexical_cast<uint64_t>(process);
    // elem_count
    getline(iss,process,':');
    bf_elem_count=boost::lexical_cast<uint64_t>(process);
    // hash_count
    getline(iss,process,':');
    hash_count=boost::lexical_cast<uint16_t>(process);
    // bit_mask
    getline(iss,process,':');
    bit_mask=boost::lexical_cast<uint64_t>(process);
    // compressed_size
    getline(iss,process,':');
    //uint64_t comp_size=boost::lexical_cast<uint64_t>(process);
    // setname
    getline(iss,setname,':');
    // endl
    getline(iss,process); // raw BF left base64
    // allocate size
    bf = new uint8_t[bf_size];
    // decode
    int len;
    bf = (uint8_t *)b64decode((char*)process.c_str(),process.length(),&len);
    created=true;
    hamming=0;
    fold(folds);
    compute_hamming();
}

void
bloom_filter::compute_hamming() {
    hamming=0;
    hamminglg=0;
    uint64_t *b64 = (uint64_t *)this->bf;
#ifndef _M_IX86
    for( uint32_t j=0; j<bf_size/8; j++) {
        hamminglg += _mm_popcnt_u64(b64[j]);
    }
#endif
}


/* -*- c++ -*- */
/*
 * Copyright 2002 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include <gnuradio/atsc/equalizer_lms2_impl.h>
#include <assert.h>
#include <algorithm>
#include <gnuradio/atsc/pnXXX_impl.h>
#include <cmath>
#include <stdlib.h>
#include <gnuradio/math.h>
#include <stdio.h>
#include <boost/math/special_functions/fpclassify.hpp>

using std::min;
using std::max;

static const int	NFFTAPS =  64;
static const int	NFBTAPS = 192;

// the length of the field sync pattern that we know unequivocally
static const int KNOWN_FIELD_SYNC_LENGTH = 4 + 511 + 3 * 63;

// static const float *get_data_seg_sync_training_sequence (int offset);
static int          get_field_sync_training_sequence_length (int offset);
static const float *get_field_sync_training_sequence (int which_field, int offset);

static inline int
wrap (int d)
{
  assert (d >= 0 && d <= (2 * NFBTAPS));

  if(d >= NFBTAPS)
    return d - NFBTAPS;
  return d;
}

static inline float
slice (float d)
{
  if (boost::math::isnan (d))
    return 0.0;

  if (d >= 0.0){
    if (d >= 4.0){
      if (d >= 6.0)
	return 7.0;
      else
	return 5.0;
    }
    if (d >= 2.0)
      return 3.0;
    return 1.0;
  }
  else
    return -slice (-d);
}

atsci_equalizer_lms2::atsci_equalizer_lms2 ()
  : d_taps_ff (NFFTAPS), d_taps_fb (NFBTAPS), d_old_output (NFBTAPS)
{
  for (int i = 0; i < NFFTAPS; i++) {
    d_taps_ff[i] = 0.0;
  }
  for (int i = 0; i < NFBTAPS; i++) {
    d_taps_fb[i] = 0.0;
    d_old_output[i] = 0.0;
  }
  d_output_ptr = 0;
  trainingfile=fopen("taps.txt","w");
}

atsci_equalizer_lms2::~atsci_equalizer_lms2 ()
{
  fclose(trainingfile);
}

void
atsci_equalizer_lms2::reset ()
{
  atsci_equalizer::reset ();		// invoke superclass
  for (int i = 0; i < NFFTAPS; i++) {
    d_taps_ff[i] = 0.0;
  }
  for (int i = 0; i < NFBTAPS; i++) {
    d_taps_fb[i] = 0.0;
    d_old_output[i] = 0.0;
  }
  d_output_ptr = 0;
}

int
atsci_equalizer_lms2::ntaps () const
{
  return NFFTAPS + NFBTAPS;
}

int
atsci_equalizer_lms2::npretaps () const
{
  return NFFTAPS;
}

/*!
 * Input range is known NOT TO CONTAIN data segment syncs
 * or field syncs.  This should be the fast path.  In the
 * non decicion directed case, this just runs the input
 * through the filter without adapting it.
 *
 * \p input_samples has (nsamples + ntaps() - 1) valid entries.
 * input_samples[0] .. input_samples[nsamples - 1 + ntaps() - 1] may be
 * referenced to compute the output values.
 */
void
atsci_equalizer_lms2::filter_normal (const float *input_samples,
				   float *output_samples,
				   int   nsamples)
{
  // handle data
  filterN (input_samples, output_samples, nsamples);
}

/*!
 * Input range is known to consist of only a data segment sync or a
 * portion of a data segment sync.  \p nsamples will be in [1,4].
 * \p offset will be in [0,3].  \p offset is the offset of the input
 * from the beginning of the data segment sync pattern.
 *
 * \p input_samples has (nsamples + ntaps() - 1) valid entries.
 * input_samples[0] .. input_samples[nsamples - 1 + ntaps() - 1] may be
 * referenced to compute the output values.
 */
void
atsci_equalizer_lms2::filter_data_seg_sync (const float *input_samples,
					  float *output_samples,
					  int   nsamples,
					  int   offset)
{
  // handle data
  //  adaptN (input_samples, get_data_seg_sync_training_sequence (offset),
  //	  output_samples, nsamples);
 filterN (input_samples, output_samples, nsamples);

 //  cerr << "Seg Sync: offset " << offset << "\tnsamples\t" << nsamples << "\t pre, 5 -5 -5 5\t" <<
 // output_samples[0] << "\t" << output_samples[1] << "\t" << output_samples[2] << "\t" << output_samples[3] << endl;

}

/*!
 * Input range is known to consist of only a field sync segment or a
 * portion of a field sync segment.  \p nsamples will be in [1,832].
 * \p offset will be in [0,831].  \p offset is the offset of the input
 * from the beginning of the data segment sync pattern.  We consider the
 * 4 symbols of the immediately preceding data segment sync to be the
 * first symbols of the field sync segment.  \p which_field is in [0,1]
 * and specifies which field (duh).
 *
 * \p input_samples has (nsamples + ntaps() - 1) valid entries.
 * input_samples[0] .. input_samples[nsamples - 1 + ntaps() - 1] may be
 * referenced to compute the output values.
 */
void
atsci_equalizer_lms2::filter_field_sync (const float *input_samples,
				       float *output_samples,
				       int   nsamples,
				       int   offset,
				       int   which_field)
{
  // Only the first 4 + 511 + 3 * 63 symbols are completely defined.
  // Those after that the symbols are bilevel, so we could use decision feedback and use
  // that to train, but for now, don't train on them.

  int	n = min (nsamples, get_field_sync_training_sequence_length (offset));

  // handle known training sequence
  adaptN (input_samples, get_field_sync_training_sequence (which_field, offset),
		    output_samples, n);

  // just filter any unknown portion
  if (nsamples > n)
    filterN (&input_samples[n], &output_samples[n], nsamples - n);

  if (offset == 0 && nsamples > 0){
    for (int i = 0; i < NFFTAPS; i++)
      fprintf(trainingfile,"%f ",d_taps_ff[i]);
    for (int i = 0; i < NFBTAPS; i++)
      fprintf(trainingfile,"%f ",d_taps_fb[i]);
    fprintf (trainingfile,"\n");
  }

}

// ----------------------------------------------------------------

//
// filter a single output
//
float
atsci_equalizer_lms2::filter1 (const float input[])
{
  static const int N_UNROLL = 4;

  float	acc0 = 0;
  float	acc1 = 0;
  float	acc2 = 0;
  float	acc3 = 0;
  float acc = 0;

  unsigned	i = 0;
  unsigned	n = (NFFTAPS / N_UNROLL) * N_UNROLL;

  for (i = 0; i < n; i += N_UNROLL){
    acc0 += d_taps_ff[i + 0] * input[i + 0];
    acc1 += d_taps_ff[i + 1] * input[i + 1];
    acc2 += d_taps_ff[i + 2] * input[i + 2];
    acc3 += d_taps_ff[i + 3] * input[i + 3];
  }

  for (; i < (unsigned) NFFTAPS; i++)
    acc0 += d_taps_ff[i] * input[i];

  acc = (acc0 + acc1 + acc2 + acc3);

  d_output_ptr = wrap (d_output_ptr + 1);

  for (int i = 0; i < NFBTAPS; i++) {
    acc -= d_taps_fb[i] * d_old_output[wrap(i + d_output_ptr)];
  }

  if (boost::math::isnan (acc)){
    abort ();
  }

  d_old_output[d_output_ptr] = slice (acc);
  return acc;
}

//
// filter and adapt a single output
//
float kludge ()
{
  return 0.0;
}

float
atsci_equalizer_lms2::adapt1 (const float input[], float ideal_output)
{
  static const double BETA = 0.00005;	// FIXME figure out what this ought to be
					// FIXME add gear-shifting

  double y = filter1 (input);
  double e = y - ideal_output;

  // update taps...
  for (int i = 0; i < NFFTAPS; i++){
    d_taps_ff[i] = d_taps_ff[i] - BETA * e * (double)(input[i]);
  }

  for (int i = 0; i < NFBTAPS; i++){
    // d_taps_fb[i] = d_taps_fb[i] - BETA * e * (double)d_old_output[wrap(i+d_output_ptr)];
    d_taps_fb[i] = d_taps_fb[i] - kludge() * e * (double)d_old_output[wrap(i+d_output_ptr)];
  }

  return y;
}

void
atsci_equalizer_lms2::filterN (const float *input_samples,
			     float *output_samples,
			     int nsamples)
{
  for (int i = 0; i < nsamples; i++)
    output_samples[i] = filter1 (&input_samples[i]);
}

void
atsci_equalizer_lms2::adaptN (const float *input_samples,
			    const float *training_pattern,
			    float *output_samples,
			    int    nsamples)
{
  for (int i = 0; i < nsamples; i++)
    output_samples[i] = adapt1 (&input_samples[i], training_pattern[i]);
}

// ----------------------------------------------------------------

static float
bin_map (int bit)
{
  return bit ? +5 : -5;
}

static void
init_field_sync_common (float *p, int mask)

{
  int  i = 0;

  p[i++] = bin_map (1);			// data segment sync pulse
  p[i++] = bin_map (0);
  p[i++] = bin_map (0);
  p[i++] = bin_map (1);

  for (int j = 0; j < 511; j++)		// PN511
    p[i++] = bin_map (atsc_pn511[j]);

  for (int j = 0; j < 63; j++)		// PN63
    p[i++] = bin_map (atsc_pn63[j]);

  for (int j = 0; j < 63; j++)		// PN63, toggled on field 2
    p[i++] = bin_map (atsc_pn63[j] ^ mask);

  for (int j = 0; j < 63; j++)		// PN63
    p[i++] = bin_map (atsc_pn63[j]);

  assert (i == KNOWN_FIELD_SYNC_LENGTH);
}

#if 0
static const float *
get_data_seg_sync_training_sequence (int offset)
{
  static const float training_data[4] = { +5, -5, -5, +5 };
  return &training_data[offset];
}
#endif

static int
get_field_sync_training_sequence_length (int offset)
{
  return max (0, KNOWN_FIELD_SYNC_LENGTH - offset);
}

static const float *
get_field_sync_training_sequence (int which_field, int offset)
{
  static float *field_1 = 0;
  static float *field_2 = 0;

  if (field_1 == 0){
    field_1 = new float[KNOWN_FIELD_SYNC_LENGTH];
    field_2 = new float[KNOWN_FIELD_SYNC_LENGTH];
    init_field_sync_common (field_1, 0);
    init_field_sync_common (field_2, 1);
  }

  if (which_field == 0)
    return &field_1[offset];
  else
    return &field_2[offset];
}


//
//  Copyright (C) 2007-2013 Greg Landrum
//
//   @@ 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 <GraphMol/RDKitBase.h>
#include <GraphMol/Fingerprints/AtomPairs.h>
#include <GraphMol/Subgraphs/Subgraphs.h>
#include <DataStructs/SparseIntVect.h>
#include <RDGeneral/hash/hash.hpp>
#include <cstdint>
#include <boost/dynamic_bitset.hpp>
#include <boost/foreach.hpp>
#include <GraphMol/Fingerprints/FingerprintUtil.h>

namespace RDKit {
namespace AtomPairs {

template <typename T1, typename T2>
void updateElement(SparseIntVect<T1> &v, T2 elem) {
  v.setVal(elem, v.getVal(elem) + 1);
}

template <typename T1>
void updateElement(ExplicitBitVect &v, T1 elem) {
  v.setBit(elem % v.getNumBits());
}

template <typename T>
void setAtomPairBit(std::uint32_t i, std::uint32_t j,
                    std::uint32_t nAtoms,
                    const std::vector<std::uint32_t> &atomCodes,
                    const double *dm, T *bv, unsigned int minLength,
                    unsigned int maxLength, bool includeChirality) {
  auto dist = static_cast<unsigned int>(floor(dm[i * nAtoms + j]));
  if (dist >= minLength && dist <= maxLength) {
    std::uint32_t bitId =
        getAtomPairCode(atomCodes[i], atomCodes[j], dist, includeChirality);
    updateElement(*bv, static_cast<std::uint32_t>(bitId));
  }
}

SparseIntVect<std::int32_t> *getAtomPairFingerprint(
    const ROMol &mol, const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants, bool includeChirality,
    bool use2D, int confId) {
  return getAtomPairFingerprint(mol, 1, maxPathLen - 1, fromAtoms, ignoreAtoms,
                                atomInvariants, includeChirality, use2D,
                                confId);
};

SparseIntVect<std::int32_t> *getAtomPairFingerprint(
    const ROMol &mol, unsigned int minLength, unsigned int maxLength,
    const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants, bool includeChirality,
    bool use2D, int confId) {
  PRECONDITION(minLength <= maxLength, "bad lengths provided");
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");

  const ROMol *lmol = &mol;
  std::unique_ptr<ROMol> tmol;
  if (includeChirality && !mol.hasProp(common_properties::_StereochemDone)) {
    tmol = std::unique_ptr<ROMol>(new ROMol(mol));
    MolOps::assignStereochemistry(*tmol);
    lmol = tmol.get();
  }

  auto *res = new SparseIntVect<std::int32_t>(
      1 << (numAtomPairFingerprintBits + 2 * (includeChirality ? 2 : 0)));
  const double *dm;
  if (use2D) {
    dm = MolOps::getDistanceMat(*lmol);
  } else {
    dm = MolOps::get3DDistanceMat(*lmol, confId);
  }
  const unsigned int nAtoms = lmol->getNumAtoms();

  std::vector<std::uint32_t> atomCodes;
  for (ROMol::ConstAtomIterator atomItI = lmol->beginAtoms();
       atomItI != lmol->endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      atomCodes.push_back((*atomInvariants)[(*atomItI)->getIdx()] %
                          ((1 << codeSize) - 1));
    }
  }

  for (ROMol::ConstAtomIterator atomItI = lmol->beginAtoms();
       atomItI != lmol->endAtoms(); ++atomItI) {
    unsigned int i = (*atomItI)->getIdx();
    if (ignoreAtoms && std::find(ignoreAtoms->begin(), ignoreAtoms->end(), i) !=
                           ignoreAtoms->end()) {
      continue;
    }
    if (!fromAtoms) {
      for (ROMol::ConstAtomIterator atomItJ = atomItI + 1;
           atomItJ != lmol->endAtoms(); ++atomItJ) {
        unsigned int j = (*atomItJ)->getIdx();
        if (ignoreAtoms && std::find(ignoreAtoms->begin(), ignoreAtoms->end(),
                                     j) != ignoreAtoms->end()) {
          continue;
        }
        setAtomPairBit(i, j, nAtoms, atomCodes, dm, res, minLength, maxLength,
                       includeChirality);
      }
    } else {
      BOOST_FOREACH (std::uint32_t j, *fromAtoms) {
        if (j != i) {
          if (ignoreAtoms && std::find(ignoreAtoms->begin(), ignoreAtoms->end(),
                                       j) != ignoreAtoms->end()) {
            continue;
          }
          setAtomPairBit(i, j, nAtoms, atomCodes, dm, res, minLength, maxLength,
                         includeChirality);
        }
      }
    }
  }
  return res;
}

SparseIntVect<std::int32_t> *getHashedAtomPairFingerprint(
    const ROMol &mol, unsigned int nBits, unsigned int minLength,
    unsigned int maxLength, const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants, bool includeChirality,
    bool use2D, int confId) {
  PRECONDITION(minLength <= maxLength, "bad lengths provided");
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  const ROMol *lmol = &mol;
  std::unique_ptr<ROMol> tmol;
  if (includeChirality && !mol.hasProp(common_properties::_StereochemDone)) {
    tmol = std::unique_ptr<ROMol>(new ROMol(mol));
    MolOps::assignStereochemistry(*tmol);
    lmol = tmol.get();
  }
  auto *res = new SparseIntVect<std::int32_t>(nBits);
  const double *dm;
  try {
    if (use2D) {
      dm = MolOps::getDistanceMat(*lmol);
    } else {
      dm = MolOps::get3DDistanceMat(*lmol, confId);
    }
  } catch (const ConformerException &) {
    delete res;
    throw;
  }

  const unsigned int nAtoms = lmol->getNumAtoms();

  std::vector<std::uint32_t> atomCodes;
  atomCodes.reserve(nAtoms);
  for (ROMol::ConstAtomIterator atomItI = lmol->beginAtoms();
       atomItI != lmol->endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      atomCodes.push_back((*atomInvariants)[(*atomItI)->getIdx()]);
    }
  }

  for (ROMol::ConstAtomIterator atomItI = lmol->beginAtoms();
       atomItI != lmol->endAtoms(); ++atomItI) {
    unsigned int i = (*atomItI)->getIdx();
    if (ignoreAtoms && std::find(ignoreAtoms->begin(), ignoreAtoms->end(), i) !=
                           ignoreAtoms->end()) {
      continue;
    }
    if (!fromAtoms) {
      for (ROMol::ConstAtomIterator atomItJ = atomItI + 1;
           atomItJ != lmol->endAtoms(); ++atomItJ) {
        unsigned int j = (*atomItJ)->getIdx();
        if (ignoreAtoms && std::find(ignoreAtoms->begin(), ignoreAtoms->end(),
                                     j) != ignoreAtoms->end()) {
          continue;
        }
        auto dist = static_cast<unsigned int>(floor(dm[i * nAtoms + j]));
        if (dist >= minLength && dist <= maxLength) {
          std::uint32_t bit = 0;
          gboost::hash_combine(bit, std::min(atomCodes[i], atomCodes[j]));
          gboost::hash_combine(bit, dist);
          gboost::hash_combine(bit, std::max(atomCodes[i], atomCodes[j]));
          updateElement(*res, static_cast<std::int32_t>(bit % nBits));
        }
      }
    } else {
      BOOST_FOREACH (std::uint32_t j, *fromAtoms) {
        if (j != i) {
          if (ignoreAtoms && std::find(ignoreAtoms->begin(), ignoreAtoms->end(),
                                       j) != ignoreAtoms->end()) {
            continue;
          }
          auto dist = static_cast<unsigned int>(floor(dm[i * nAtoms + j]));
          if (dist >= minLength && dist <= maxLength) {
            std::uint32_t bit = 0;
            gboost::hash_combine(bit, std::min(atomCodes[i], atomCodes[j]));
            gboost::hash_combine(bit, dist);
            gboost::hash_combine(bit, std::max(atomCodes[i], atomCodes[j]));
            updateElement(*res, static_cast<std::int32_t>(bit % nBits));
          }
        }
      }
    }
  }
  return res;
}

ExplicitBitVect *getHashedAtomPairFingerprintAsBitVect(
    const ROMol &mol, unsigned int nBits, unsigned int minLength,
    unsigned int maxLength, const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants,
    unsigned int nBitsPerEntry, bool includeChirality, bool use2D, int confId) {
  PRECONDITION(minLength <= maxLength, "bad lengths provided");
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  static int bounds[4] = {1, 2, 4, 8};

  unsigned int blockLength = nBits / nBitsPerEntry;
  SparseIntVect<std::int32_t> *sres = getHashedAtomPairFingerprint(
      mol, blockLength, minLength, maxLength, fromAtoms, ignoreAtoms,
      atomInvariants, includeChirality, use2D, confId);
  auto *res = new ExplicitBitVect(nBits);
  if (nBitsPerEntry != 4) {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second > static_cast<int>(i)) {
          res->setBit(val.first * nBitsPerEntry + i);
        }
      }
    }
  } else {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second >= bounds[i]) {
          res->setBit(val.first * nBitsPerEntry + i);
        }
      }
    }
  }
  delete sres;
  return res;
}

SparseIntVect<boost::int64_t> *getTopologicalTorsionFingerprint(
    const ROMol &mol, unsigned int targetSize,
    const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants, bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  const ROMol *lmol = &mol;
  std::unique_ptr<ROMol> tmol;
  if (includeChirality && !mol.hasProp(common_properties::_StereochemDone)) {
    tmol = std::unique_ptr<ROMol>(new ROMol(mol));
    MolOps::assignStereochemistry(*tmol);
    lmol = tmol.get();
  }
  boost::uint64_t sz = 1;
  sz = (sz << (targetSize *
               (codeSize + (includeChirality ? numChiralBits : 0))));
  // NOTE: this -1 is incorrect but it's needed for backwards compatibility.
  //  hopefully we'll never have a case with a torsion that hits this.
  //
  //  mmm, bug compatible.
  sz -= 1;
  auto *res = new SparseIntVect<boost::int64_t>(sz);

  std::vector<std::uint32_t> atomCodes;
  atomCodes.reserve(lmol->getNumAtoms());
  for (ROMol::ConstAtomIterator atomItI = lmol->beginAtoms();
       atomItI != lmol->endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      // need to add to the atomCode here because we subtract off up to 2 below
      // as part of the branch correction
      atomCodes.push_back(
          (*atomInvariants)[(*atomItI)->getIdx()] % ((1 << codeSize) - 1) + 2);
    }
  }

  boost::dynamic_bitset<> *fromAtomsBV = nullptr;
  if (fromAtoms) {
    fromAtomsBV = new boost::dynamic_bitset<>(lmol->getNumAtoms());
    BOOST_FOREACH (std::uint32_t fAt, *fromAtoms) { fromAtomsBV->set(fAt); }
  }
  boost::dynamic_bitset<> *ignoreAtomsBV = nullptr;
  if (ignoreAtoms) {
    ignoreAtomsBV = new boost::dynamic_bitset<>(mol.getNumAtoms());
    BOOST_FOREACH (std::uint32_t fAt, *ignoreAtoms) {
      ignoreAtomsBV->set(fAt);
    }
  }
  boost::dynamic_bitset<> pAtoms(lmol->getNumAtoms());
  PATH_LIST paths = findAllPathsOfLengthN(*lmol, targetSize, false);
  for (PATH_LIST::const_iterator pathIt = paths.begin(); pathIt != paths.end();
       ++pathIt) {
    bool keepIt = true;
    if (fromAtomsBV) {
      keepIt = false;
    }
    std::vector<std::uint32_t> pathCodes;
    const PATH_TYPE &path = *pathIt;
    if (fromAtomsBV) {
      if (fromAtomsBV->test(static_cast<std::uint32_t>(path.front())) ||
          fromAtomsBV->test(static_cast<std::uint32_t>(path.back()))) {
        keepIt = true;
      }
    }
    if (keepIt && ignoreAtomsBV) {
      BOOST_FOREACH (int pElem, path) {
        if (ignoreAtomsBV->test(pElem)) {
          keepIt = false;
          break;
        }
      }
    }
    if (keepIt) {
      pAtoms.reset();
      for (auto pIt = path.begin(); pIt < path.end(); ++pIt) {
        // look for a cycle that doesn't start at the first atom
        // we can't effectively canonicalize these at the moment
        // (was github #811)
        if (pIt != path.begin() && *pIt != *(path.begin()) && pAtoms[*pIt]) {
          pathCodes.clear();
          break;
        }
        pAtoms.set(*pIt);
        unsigned int code = atomCodes[*pIt] - 1;
        // subtract off the branching number:
        if (pIt != path.begin() && pIt + 1 != path.end()) {
          --code;
        }
        pathCodes.push_back(code);
      }
      if (pathCodes.size()) {
        boost::int64_t code =
            getTopologicalTorsionCode(pathCodes, includeChirality);
        updateElement(*res, code);
      }
    }
  }
  delete fromAtomsBV;
  delete ignoreAtomsBV;

  return res;
}

namespace {
template <typename T>
void TorsionFpCalc(T *res, const ROMol &mol, unsigned int nBits,
                   unsigned int targetSize,
                   const std::vector<std::uint32_t> *fromAtoms,
                   const std::vector<std::uint32_t> *ignoreAtoms,
                   const std::vector<std::uint32_t> *atomInvariants,
                   bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  const ROMol *lmol = &mol;
  std::unique_ptr<ROMol> tmol;
  if (includeChirality && !mol.hasProp(common_properties::_StereochemDone)) {
    tmol = std::unique_ptr<ROMol>(new ROMol(mol));
    MolOps::assignStereochemistry(*tmol);
    lmol = tmol.get();
  }
  std::vector<std::uint32_t> atomCodes;
  atomCodes.reserve(lmol->getNumAtoms());
  for (ROMol::ConstAtomIterator atomItI = lmol->beginAtoms();
       atomItI != lmol->endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      // need to add to the atomCode here because we subtract off up to 2 below
      // as part of the branch correction
      atomCodes.push_back(((*atomInvariants)[(*atomItI)->getIdx()] << 1) + 1);
    }
  }

  boost::dynamic_bitset<> *fromAtomsBV = nullptr;
  if (fromAtoms) {
    fromAtomsBV = new boost::dynamic_bitset<>(lmol->getNumAtoms());
    BOOST_FOREACH (std::uint32_t fAt, *fromAtoms) { fromAtomsBV->set(fAt); }
  }
  boost::dynamic_bitset<> *ignoreAtomsBV = nullptr;
  if (ignoreAtoms) {
    ignoreAtomsBV = new boost::dynamic_bitset<>(lmol->getNumAtoms());
    BOOST_FOREACH (std::uint32_t fAt, *ignoreAtoms) {
      ignoreAtomsBV->set(fAt);
    }
  }

  PATH_LIST paths = findAllPathsOfLengthN(*lmol, targetSize, false);
  for (PATH_LIST::const_iterator pathIt = paths.begin(); pathIt != paths.end();
       ++pathIt) {
    bool keepIt = true;
    if (fromAtomsBV) {
      keepIt = false;
    }
    const PATH_TYPE &path = *pathIt;
    if (fromAtomsBV) {
      if (fromAtomsBV->test(static_cast<std::uint32_t>(path.front())) ||
          fromAtomsBV->test(static_cast<std::uint32_t>(path.back()))) {
        keepIt = true;
      }
    }
    if (keepIt && ignoreAtomsBV) {
      BOOST_FOREACH (int pElem, path) {
        if (ignoreAtomsBV->test(pElem)) {
          keepIt = false;
          break;
        }
      }
    }
    if (keepIt) {
      std::vector<std::uint32_t> pathCodes(targetSize);
      for (unsigned int i = 0; i < targetSize; ++i) {
        unsigned int code = atomCodes[path[i]] - 1;
        // subtract off the branching number:
        if (i > 0 && i < targetSize - 1) {
          --code;
        }
        pathCodes[i] = code;
      }
      size_t bit = getTopologicalTorsionHash(pathCodes);
      updateElement(*res, bit % nBits);
    }
  }
  delete fromAtomsBV;
  delete ignoreAtomsBV;
}
}  // namespace
SparseIntVect<boost::int64_t> *getHashedTopologicalTorsionFingerprint(
    const ROMol &mol, unsigned int nBits, unsigned int targetSize,
    const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants, bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  auto *res = new SparseIntVect<boost::int64_t>(nBits);
  TorsionFpCalc(res, mol, nBits, targetSize, fromAtoms, ignoreAtoms,
                atomInvariants, includeChirality);
  return res;
}

ExplicitBitVect *getHashedTopologicalTorsionFingerprintAsBitVect(
    const ROMol &mol, unsigned int nBits, unsigned int targetSize,
    const std::vector<std::uint32_t> *fromAtoms,
    const std::vector<std::uint32_t> *ignoreAtoms,
    const std::vector<std::uint32_t> *atomInvariants,
    unsigned int nBitsPerEntry, bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  static int bounds[4] = {1, 2, 4, 8};
  unsigned int blockLength = nBits / nBitsPerEntry;
  auto *sres = new SparseIntVect<boost::int64_t>(blockLength);
  TorsionFpCalc(sres, mol, blockLength, targetSize, fromAtoms, ignoreAtoms,
                atomInvariants, includeChirality);
  auto *res = new ExplicitBitVect(nBits);

  if (nBitsPerEntry != 4) {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second > static_cast<int>(i)) {
          res->setBit(val.first * nBitsPerEntry + i);
        }
      }
    }
  } else {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second >= bounds[i]) {
          res->setBit(val.first * nBitsPerEntry + i);
        }
      }
    }
  }
  delete sres;
  return res;
}
}  // end of namespace AtomPairs
}  // end of namespace RDKit


/*
# Copyright (c) 2014-2020 Reed A. Cartwright <reed@cartwright.ht>
# Copyright (c) 2016 Steven H. Wu
# Copyright (c) 2016 Kael Dai
#
# This file is part of the Ultimate Source Code Project.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
*/

#ifndef MUTK_RELATIONSHIP_GRAPH_HPP
#define MUTK_RELATIONSHIP_GRAPH_HPP

#include <mutk/detail/graph.hpp>
#include <mutk/pedigree.hpp>
#include <mutk/memory.hpp>
#include <mutk/mutation.hpp>

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/find.hpp>
#include <boost/range/numeric.hpp>

#include <cmath>
#include <string>
#include <vector>

namespace mutk {

enum struct InheritanceModel {
    Autosomal,    // default option
    Maternal,     // transmitted by mother to child
    Paternal,     // transmitter by father to child
    XLinked,      // females have 2 copies, males have 1; males transmit to daughters, not to sons
    YLinked,      // males have 1 copy, only transmits it to sons
    WLinked,      // females have 1 copy, only transmitted to daughters
    ZLinked,      // males have 2 copies, females have 1; females transmit to sons, not to daughters
    Invalid
};

namespace detail {
extern const std::map<std::string, InheritanceModel> CHR_MODEL_MAP;

struct workspace_t {
    std::vector<mutk::Tensor<1>> stack;
    std::array<mutk::tensor_index_t,3> widths;
    float scale;
};

} //namespace detail

class PeelingVertex;

class RelationshipGraph {
public:
    using samples_t = std::vector<const char*>;
    using workspace_t = detail::workspace_t;
    using Potential = detail::Potential;
    using potential_t = detail::potential_t;

    using graph_t = detail::pedigree_graph::Graph;
    using junction_tree_t = detail::junction_tree::Graph;

    void ConstructGraph(const Pedigree& pedigree, const samples_t& known_samples,
            InheritanceModel inheritance_model,
            double mu, double mu_somatic, bool normalize_somatic_trees);

    void ConstructPeeler();

    float PeelForward(workspace_t *work) const;

    void PrintGraph(std::ostream &os) const;

    samples_t SampleNames() const;

    workspace_t CreateWorkspace() const;

    const std::vector<potential_t>& potentials() const {
        return potentials_;
    }

    const graph_t & graph() const {
        return graph_;
    }

protected:
    InheritanceModel inheritance_model_{InheritanceModel::Autosomal};

    graph_t graph_;
    junction_tree_t junction_tree_;

    using vertex_range_t = std::pair<detail::pedigree_graph::vertex_t,detail::pedigree_graph::vertex_t>;

    vertex_range_t founders_;
    vertex_range_t descendants_;
    vertex_range_t samples_;

    size_t stack_size_;

    // The probability potentials of the relationship graph
    std::vector<potential_t> potentials_;

    // Elimination Rank
    std::vector<std::size_t> elimination_rank_;

    // Peeling Operations
    using peeling_t = std::unique_ptr<PeelingVertex>;
    std::vector<peeling_t> peelers_;

    // location of root data in peelers
    std::vector<size_t> roots_;

private:

};

class PeelingVertex {
public:
    using workspace_t = mutk::RelationshipGraph::workspace_t;
    using tree_t = detail::junction_tree::Graph;
    using vertex_t = detail::junction_tree::vertex_t;
    using label_t = mutk::detail::pedigree_graph::vertex_t;

    virtual ~PeelingVertex() = default;

    virtual void Forward(PeelingVertex::workspace_t *work) const = 0;

    virtual void AddLocal(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) = 0;
    virtual void AddOutput(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) = 0;
    virtual void AddInput(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) = 0;
};

template<std::size_t N>
class GeneralPeelingVertex : public PeelingVertex {
public:
    using tensor_t = mutk::Tensor<N>;

    struct data_t {
        std::size_t index;
        typename tensor_t::Dimensions data_ploidy;
        typename tensor_t::Dimensions broadcast_ploidy;
    };

    GeneralPeelingVertex(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index, const std::vector<int> &buffer_ploidy);

    virtual void AddLocal(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) override;
    virtual void AddOutput(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) override;
    virtual void AddInput(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) override;

    virtual ~GeneralPeelingVertex() = default;

    virtual void Forward(PeelingVertex::workspace_t *work) const override;

protected:
    data_t MakeMetadata(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) const;

    std::array<PeelingVertex::label_t,N> labels_;

    data_t buffer_;

    data_t local_data_;
    data_t output_data_;
    std::vector<data_t> input_data_;
};

namespace detail {

template<typename A>
auto calc_dims(const PeelingVertex::workspace_t &work, A a) {
    typename mutk::Tensor<a.size()>::Dimensions ret;
    std::transform(a.begin(), a.end(), ret.begin(), [&](auto v) {
        return work.widths[v];
    });
    return ret;
}

}

template<std::size_t N>
GeneralPeelingVertex<N>::GeneralPeelingVertex(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index, const std::vector<int> &buffer_ploidy) {
    boost::range::copy(labels, labels_.begin());
    buffer_.index = buffer_index;
    boost::range::copy(buffer_ploidy, buffer_.data_ploidy.begin());
    boost::range::fill(buffer_.broadcast_ploidy, 0);
}

template<std::size_t N>
typename GeneralPeelingVertex<N>::data_t
GeneralPeelingVertex<N>::MakeMetadata(const std::vector<PeelingVertex::label_t> &labels,
    std::size_t buffer_index) const {
    data_t ret;
    ret.index = buffer_index;

    // Go through the labels for this vertex and see if they are in the Metadata labels
    for(size_t i = 0; i < labels_.size(); ++i) {
        auto it = boost::range::find(labels, labels_[i]);
        if(it != labels.end()) {
            // our metadata contains the label, we will not need to broadcast it
            ret.data_ploidy[i] = buffer_.data_ploidy[i];
            ret.broadcast_ploidy[i] = 0;
        } else {
            // we will need to broadcast this axis
            ret.data_ploidy[i] = 0;
            ret.broadcast_ploidy[i] = buffer_.data_ploidy[i];
        }
    }
    return ret;
}

template<std::size_t N>
void GeneralPeelingVertex<N>::AddLocal(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) {
    local_data_ = MakeMetadata(labels, buffer_index);
}

template<std::size_t N>
void GeneralPeelingVertex<N>::AddOutput(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) {
    output_data_ = MakeMetadata(labels, buffer_index);
}

template<std::size_t N>
void GeneralPeelingVertex<N>::AddInput(const std::vector<PeelingVertex::label_t> &labels,
        std::size_t buffer_index) {
    input_data_.push_back(MakeMetadata(labels, buffer_index));
}

template<std::size_t N>
void GeneralPeelingVertex<N>::Forward(PeelingVertex::workspace_t *work) const {
    // copy local data to local buffer
    auto dims = detail::calc_dims(*work, local_data_.data_ploidy);
    if(work->stack[local_data_.index].size() == 0) {
        // unit potentials are empty
        work->stack[buffer_.index].resize(dims.TotalSize());
        work->stack[buffer_.index].setConstant(1.0f);
    } else {
        work->stack[buffer_.index] = work->stack[local_data_.index];
    }

    for(int i=0;i<input_data_.size(); ++i) {
        auto input_dims = detail::calc_dims(*work, input_data_[i].data_ploidy);
        auto broadcast_dims = detail::calc_dims(*work, input_data_[i].broadcast_ploidy);
        work->stack[buffer_.index].reshape(dims) *=
            work->stack[input_data_[i].index].reshape(input_dims).broadcast(broadcast_dims);
    }
    auto msg_dims = detail::calc_dims(*work, output_data_.data_ploidy);
    std::vector<typename tensor_t::Index> sum_dims;

    for(typename tensor_t::Index i=0; i < output_data_.data_ploidy.size(); ++i) {
        if(output_data_.data_ploidy[i] == 0) {
            sum_dims.push_back(i);
        }
    }

    work->stack[output_data_.index].resize(msg_dims.TotalSize());
    switch(sum_dims.size()) {
    case 0:
        work->stack[output_data_.index] = work->stack[buffer_.index];
        break;
    case 1:
        if constexpr( N >= 1 ) {
            work->stack[output_data_.index].reshape(msg_dims) =
                work->stack[buffer_.index].reshape(dims)
                    .sum(Eigen::make_index_list(sum_dims[0])).reshape(msg_dims);
            break;
        }
    case 2:
        if constexpr( N >= 2 ) {
            work->stack[output_data_.index].reshape(msg_dims) =
                work->stack[buffer_.index].reshape(dims)
                    .sum(Eigen::make_index_list(sum_dims[0],sum_dims[1])).reshape(msg_dims);
            break;
        }
    case 3:
        if constexpr( N >= 3 ) {
            work->stack[output_data_.index].reshape(msg_dims) =
                work->stack[buffer_.index].reshape(dims)
                    .sum(Eigen::make_index_list(sum_dims[0],sum_dims[1],sum_dims[2])).reshape(msg_dims);
            break;
        }
    case 4:
        if constexpr( N >= 4 ) {
            work->stack[output_data_.index].reshape(msg_dims) =
                work->stack[buffer_.index].reshape(dims)
                    .sum(Eigen::make_index_list(sum_dims[0],sum_dims[1],sum_dims[2],sum_dims[3])).reshape(msg_dims);
            break;
        }
    default:
        assert(false);
    };
}

}; // namespace mutk

#endif // MUTK_RELATIONSHIP_GRAPH_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/

#include "NewtonEulerFrom1DLocalFrameR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"

//#define NERI_DEBUG

//#define NEFC3D_DEBUG
// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
  printf("contact normal:\n");
  _Nc->display();
  printf("point de contact :\n");
  _Pc1->display();
  printf("center of masse :\n");
  q1->display();
#endif
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  //d1->computeMObjToAbs();
  //SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();
  computeMObjToAbs(q1,_MObjToAbs);

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _Mabs_C->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
  _jachqT->display();
#endif
}

void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

//  d1->computeMObjToAbs();
//  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();

  computeMObjToAbs(q1,_MObjToAbs);

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _Mabs_C->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  _NPG2->zero();
  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

//  d2->computeMObjToAbs();
//  SimpleMatrix& Mobj2_abs = *d2->MObjToAbs();

  computeMObjToAbs(q2,_MObjToAbs);
  prod(*_NPG2, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj + 6, -_Mabs_C->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj + 6, -_AUX2->getValue(0, jj - 3));
}

void NewtonEulerFrom1DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
  NewtonEulerR::initComponents(inter, DSlink, workV, workM);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  _jachq.reset(new SimpleMatrix(1, qSize));

  /* VA 12/04/2016 All of what follows should be put in WorkM*/
  _Mabs_C.reset(new SimpleMatrix(1, 3));
  _MObjToAbs.reset(new SimpleMatrix(3, 3));
  _AUX1.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(1, 3));
  _NPG1.reset(new SimpleMatrix(3, 3));
  _NPG2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}

void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{

  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
  DEBUG_PRINTF("with time =  %f\n",time);
  DEBUG_PRINTF("with inter =  %p\n",&inter);

  _jachq->setValue(0, 0, _Nc->getValue(0));
  _jachq->setValue(0, 1, _Nc->getValue(1));
  _jachq->setValue(0, 2, _Nc->getValue(2));
  if (inter.has2Bodies())
  {
    _jachq->setValue(0, 7, -_Nc->getValue(0));
    _jachq->setValue(0, 8, -_Nc->getValue(1));
    _jachq->setValue(0, 9, -_Nc->getValue(2));
  }

  for (unsigned int iDS =0 ; iDS < q0->getNumberOfBlocks()  ; iDS++)
  {
    SP::SiconosVector q = (q0->getAllVect())[iDS];
    double sign = 1.0;
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS);
    DEBUG_EXPR_WE(q->display(););

    ::boost::math::quaternion<double>    quatGP;
    if (iDS == 0)
    {
      ::boost::math::quaternion<double>    quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1),
                                                   _Pc1->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    else
    {
      sign = -1.0;
      //cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n";
      ::boost::math::quaternion<double>    quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1),
                                                   _Pc2->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4());
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6));
    ::boost::math::quaternion<double>    quat0(1, 0, 0, 0);
    ::boost::math::quaternion<double>    quatBuff;
    ::boost::math::quaternion<double>    _2qiquatGP;
    _2qiquatGP = quatGP;
    _2qiquatGP *= 2 * (q->getValue(3));
    quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP;

    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4());

    _jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                             quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    //cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n";
    //_jachq->setValue(0,7*iDS+3,0);
    for (unsigned int i = 1; i < 4; i++)
    {
      ::boost::math::quaternion<double>    quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0);
      _2qiquatGP = quatGP;
      _2qiquatGP *= 2 * (q->getValue(3 + i));
      quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP;
      _jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                                   quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    }
  }

  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 \n");

}

void NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 )
{
  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 \n")

  if (q0->getNumberOfBlocks()>1)
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0], (q0->getAllVect())[1]);
  }
  else
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0]);
  }

  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0) \n");

}