#define NDEBUG
#include "pca.hh"
#include "pcaoptions.hh"
#include <cctype>
#include <cfloat>
#include <fstream>
#include <iostream>
#include <Eigen/Eigenvalues>

#define FUDGE 10

namespace
{
  using namespace hashpca;
  using veedubparse::HashAll;
  using veedubparse::HashString;

  // http://www0.cs.ucl.ac.uk/staff/d.jones/GoodPracticeRNG.pdf

  /* Implementation of a 32-bit KISS generator which uses no multiply instructions */
  unsigned int JKISS32 ()
  {
    static uint32_t x=123456789,y=234567891,z=345678912,w=456789123,c=0;
    int32_t t;
    y ^= (y<<5); y ^= (y>>7); y ^= (y<<22);
    t = z+w+c; z = w; c = t < 0; w = t&2147483647;
    x += 1411392427;
    return x + y + w;
  }

  double urandom ()
    {
      return .0000000002328306089594001 * JKISS32() + FLT_EPSILON;
    }

  //This is faster than Box-Muller but not benchmarked against inverse cdf
  double
  gensample (double)
    {
      double u,v,x,x2;

      do
        {
          u = urandom();
          v = 1.71552776992141359294*urandom()-.85776388496070679647;
          x = v/u;
          x2 = x*x;
        }
      while(x2>6-8*u+2*u*u && (x2>2/u-2*u || x2>-4*log(u)));

      return x;
    }

  void
  orthonormalize (hashpca::MatrixXd& Y)
    {
      // // http://forum.kde.org/viewtopic.php?f=74&t=91271
      // // Unfortunately this appears memory intensive :(
      // Eigen::HouseholderQR<hashpca::MatrixXd> qr (Y);
      // Y = qr.householderQ () * hashpca::MatrixXd::Identity (Y.rows (), Y.cols ());

      Y.transposeInPlace ();

      // Gram-Schmidt has no space overhead
      for (unsigned int j = 0; j < Y.rows (); ++j)
        {
          for (unsigned int i = 0; i < j; ++i)
            Y.row (j) -= Y.row (i).dot (Y.row (j)) * Y.row (i);

          for (unsigned int i = 0; i < j; ++i)
            Y.row (j) -= Y.row (i).dot (Y.row (j)) * Y.row (i);

          Y.row (j).normalize ();
        }

      Y.transposeInPlace ();
    }

  Eigen::VectorXd
  smallsvd (const hashpca::MatrixXd& Z,
            hashpca::MatrixXd&       V)
    {
      hashpca::MatrixXd ZtZ = Z.transpose () * Z;
      Eigen::SelfAdjointEigenSolver<hashpca::MatrixXd> es (ZtZ);
      Eigen::VectorXd s = es.eigenvalues ();
      hashpca::MatrixXd Upsilon = es.eigenvectors ();

      s = s.unaryExpr ([] (double x) { return x <= 0.0 ? 0.0 : sqrt (x); });

      Eigen::VectorXd pinv = s.unaryExpr ([] (double x) { return 1.0 / (1e-6 + x); });

      V.noalias () = Z * Upsilon * pinv.asDiagonal ();

      s = s.unaryExpr ([] (double x) { return x <= 0.0 ? 0.0 : sqrt (x); });

      return s;
    }

  int
  writemodel (PcaOptions                options,
              std::ostream&             model,
              const hashpca::MatrixXd&  V,
              const Eigen::VectorXd&    s,
              const Eigen::VectorXd&    sum,
              double                    n)
    {
      std::cerr << "Writing model ... ";

      model << V.rows () << " " << V.cols () - FUDGE << " " << options.hashall << std::endl;
      model
        << (options.dashq ? static_cast<unsigned char> (options.dashq[0]) : -1)
        << " "
        << (options.dashq ? static_cast<unsigned char> (options.dashq[1]) : -1)
        << std::endl;

      Eigen::IOFormat HeavyFmt (Eigen::FullPrecision);
      model << s.reverse ().topRows (s.rows () - FUDGE).format (HeavyFmt) << std::endl;

      Eigen::VectorXd mean;

      if (options.center)
        mean = (1.0 / n) * V.transpose () * sum;
      else
        mean = Eigen::VectorXd::Zero (V.cols ());

      mean = mean.reverse ().eval ();

      // http://forum.kde.org/viewtopic.php?f=74&t=107161

      model.write ((char*) & mean (0), sizeof (double) * (mean.rows () - FUDGE));

      for (unsigned int i = 0; i < V.rows (); ++i)
        {
          Eigen::VectorXd tmp = V.row (i).reverse ();
          model.write ((char*) & tmp (0),
                       sizeof (double) * (V.cols () - FUDGE));
        }

      std::cerr << "done." << std::endl;

      return 0;
    }

  char*
  make_dashq (int a,
              int b)
    {
      static char buf[3];

      buf[0] = a;
      buf[1] = b;
      buf[2] = 0;

      return buf;
    }

  int
  readmodel (PcaOptions&        options,
             std::istream&      model,
             hashpca::MatrixXd& V,
             Eigen::VectorXd&   s,
             Eigen::VectorXd&   mean)
    {
      model >> options.hashsize >> options.rank >> options.hashall;

      int a;
      int b;

      model >> a >> b;

      if (a >= 0 && b >= 0)
        options.dashq = make_dashq (a, b);
      else
        options.dashq = 0;

      s.resize (options.rank);
      for (unsigned int i = 0; i < options.rank; ++i)
        model >> s (i);

      char newline;
      model.read (&newline, 1);

      mean.resize (options.rank);
      model.read ((char*) & mean (0), sizeof (double) * options.rank);

      Eigen::VectorXd tmp (options.rank);
      V.resize (options.hashsize, options.rank);

      for (unsigned int i = 0; i < V.rows (); ++i)
        {
          model.read ((char*) & tmp (0), sizeof (double) * options.rank);
          V.row (i) = tmp;
        }

      return 0;
    }

  int
  do_project_two (std::istream& in,
                  PcaOptions    options,
                  int           argc,
                  char*         argv[])
    {
      (void) argc;

      if (! in.good ())
        {
          std::cerr << "ERROR: input is not good" << std::endl;

          return 1;
        }

      std::ifstream model (options.model);

      if (! model.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << options.model << "' for reading: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      hashpca::MatrixXd V;
      Eigen::VectorXd s;
      Eigen::VectorXd mean;

      if (readmodel (options, model, V, s, mean))
        {
          std::cerr << "ERROR: model file is corrupt or invalid." << std::endl;
          return 1;
        }

      Eigen::VectorXd pinv = s.unaryExpr ([] (double x) { return 1.0 / (1e-6 + x); });

      if (options.dashq)
        {
          BiLinearIterator it (options.hashsize,
                               static_cast<unsigned char> (options.dashq[0]),
                               static_cast<unsigned char> (options.dashq[1]));

          return (options.hashall)
            ? computeu<HashAll> (in, std::cout, V, mean, pinv, it, options)
            : computeu<HashString> (in, std::cout, V, mean, pinv, it, options);
        }
      else
        {
          LinearIterator it (options.hashsize);

          return (options.hashall)
            ? computeu<HashAll> (in, std::cout, V, mean, pinv, it, options)
            : computeu<HashString> (in, std::cout, V, mean, pinv, it, options);
        }
    }

  int
  do_project (PcaOptions    options,
              int           argc,
              char*         argv[])
    {
      if (argc < 1)
        {
          return do_project_two (std::cin, options, argc, argv);
        }
      else
        {
          std::ifstream in (argv[0]);

          if (! in.good ())
            {
              std::cerr << "ERROR: failed to open file '"
                        << argv[0] << "' for reading: "
                        << strerror (errno) << std::endl;
              return 1;
            }

          return do_project_two (in, options, argc, argv);
        }
    }

  int
  do_pca (PcaOptions    options,
          int           argc,
          char*         argv[])
    {
      std::ifstream in (argv[0]);

      if (! in.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << argv[0] << "' for reading: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      std::ifstream in2 (argv[argc > 1 ? 1 : 0]);

      if (! in2.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << argv[argc > 1 ? 1 : 0] << "' for reading: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      std::ofstream model (options.model);

      if (! model.good ())
        {
          std::cerr << "ERROR: failed to open file '"
                    << options.model << "' for writing: "
                    << strerror (errno) << std::endl;
          return 1;
        }

      options.rank += FUDGE;

      hashpca::MatrixXd Y; Y.setZero (options.hashsize, options.rank);
      hashpca::MatrixXd Omega =
        hashpca::MatrixXd::Zero (options.hashsize, options.rank)
          .unaryExpr (std::ptr_fun (gensample));
      Eigen::VectorXd sum;

      std::cerr << "Processing examples ... ";
      std::pair<double, uint64_t> lines1;

      if (options.dashq)
        {
          BiLinearIterator it (options.hashsize,
                               static_cast<unsigned char> (options.dashq[0]),
                               static_cast<unsigned char> (options.dashq[1]));

          lines1 = (options.hashall)
            ? pca_accumulate<HashAll> (in, Y, Omega, it, options.center, sum)
            : pca_accumulate<HashString> (in, Y, Omega, it, options.center, sum);
        }
      else
        {
          LinearIterator it (options.hashsize);

          lines1 = (options.hashall)
            ? pca_accumulate<HashAll> (in, Y, Omega, it, options.center, sum)
            : pca_accumulate<HashString> (in, Y, Omega, it, options.center, sum);
        }

      std::cerr << "(" << lines1.first << ", "
                << lines1.second << ") done." << std::endl;

      std::cerr << "Orthogonalizing ... ";
      orthonormalize (Y);
      std::cerr << "done." << std::endl;

      std::cerr << "Reprocessing examples ... ";
      hashpca::MatrixXd& Z (Omega);
      Z.setZero (Z.rows (), Z.cols ());

      std::pair<double, uint64_t> lines2;

      if (options.dashq)
        {
          BiLinearIterator it (options.hashsize,
                               static_cast<unsigned char> (options.dashq[0]),
                               static_cast<unsigned char> (options.dashq[1]));

          lines2 = (options.hashall)
            ? pca_accumulate<HashAll> (in2, Z, Y, it, options.center, sum)
            : pca_accumulate<HashString> (in2, Z, Y, it, options.center, sum);
        }
      else
        {
          LinearIterator it (options.hashsize);

          lines2 = (options.hashall)
            ? pca_accumulate<HashAll> (in2, Z, Y, it, options.center, sum)
            : pca_accumulate<HashString> (in2, Z, Y, it, options.center, sum);
        }

      std::cerr << "(" << lines2.first << ", "
                << lines2.second << ") done." << std::endl;

      std::cerr << "Small svding + postprocessing ... ";
      hashpca::MatrixXd& V (Y);
      Eigen::VectorXd s = smallsvd (Z, V);
      std::cerr << "done." << std::endl;

      options.rank -= FUDGE;
      return writemodel (options, model, V, s, sum, lines2.first);
    }
}

int
main (int   argc,
      char* argv[])
{
  std::ios::sync_with_stdio (false);

  if (argc < 2)
    {
      std::cerr << help << std::endl;
      return 1;
    }

  PcaOptions options = parse_pca_options (argc, argv);

  if (! options.project && argc < 1)
    {
      std::cerr << "ERROR: did not specify input file\n" << std::endl;
      std::cerr << help << std::endl;
      return 1;
    }

  if (! options.model)
    {
      std::cerr << "ERROR: did not specify model file\n" << std::endl;
      std::cerr << help << std::endl;
      return 1;
    }

  return options.project ? do_project (options, argc, argv)
                         : do_pca (options, argc, argv);
}


/*
 * Copyright 2012 CarCV Development Team
 *
 * 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 <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/ml/ml.hpp>

//#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>

#include <iostream>
#include <fstream>
#include <cstdlib>

#include "ocr_train.hpp"

using namespace cv;
using namespace std;

namespace fs = boost::filesystem;

int main() {

	//Loads training image with numbers
	Mat img = imread("train.jpg");
	Mat img_original;
	img.copyTo(img_original);

	//Grayscale
	Mat gray;
	cvtColor(img, gray, CV_BGR2GRAY);

	//Gaussian Blur
	Mat blur;
	Size_<int> ksize(5,5); //Gaussian kernel size
	GaussianBlur(gray, blur, ksize, 0);

	//Builds a threshold making a binary image
	Mat thresh;
	adaptiveThreshold(blur, thresh, 255, 1, 1, 11, 2);

	//Find contours
	vector<vector<Point> > contours;
	vector<Vec4i> hierarchy;
	findContours(thresh, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

	//Prepare arrays
	vector< vector<float> > samples;
	vector<float> responses;

	double cntArea;
	Mat contourMat;
	for (vector< vector<Point> >::iterator it = contours.begin();
		 it != contours.end(); ++it ) {

		vector<Point> cnt = *it;
		cntArea = contourArea(cnt);

		if (cntArea<100 || cntArea>1000) { //Ensures area is large enough
			continue;
		}

		Rect boundbox = boundingRect(cnt);
		int x = boundbox.x;
		int y = boundbox.y;
		int width = boundbox.width;
		int height = boundbox.height;

		if (height<25) { //If it isn't high enough
			continue;
		}

		Point px(x,y);
		Point py(x+width, y+height);
		Scalar color(0, 0, 255); //red

		img.copyTo(contourMat);
		rectangle(contourMat, px, py, color, 2); //draws a rectangle on top of img

		Mat roi;
		roi = thresh(boundbox); //todo could be reverse (x,y) (y,x), am not sure

		//resize it to 10x10
		Mat roismall;
		Size_<int> smallSize(10,10);
		resize(roi, roismall, smallSize);

		imshow("norm", contourMat); //Shows original image
		int key = waitKey(0);

		key -= 0x100000; //hack: fixes keyboard number mapping

		if (key==27) { //Esc key
			//return 0;
			break; // only breaks, to save partial progress atleast
		}
		else if (key >= 48 && key <= 90) { // 48-57 keys are numbers from 0-9 and from 65 to 90 A-Z characters
			responses.push_back(key);
			Mat sample;
			sample = roismall.reshape(0,1);//.t();
			samples.push_back(sample);
		}
		else {
			cout << endl <<  "Error: Invalid key " << key << " pressed!!! Press keys on standard keyboard" << endl << endl;
		}

	}

	//todo: save arrays to file
	string samples_str = "samples";
	string responses_str = "responses";
	saveArray(responses_str, responses);
	save2DArray(samples_str, samples);

	cout << "Samples: " << samples.size() << endl;
	cout << "Responses: " << responses.size() << endl;

	waitKey(0);
	destroyAllWindows();
	return 0;
}


//    |  /           |
//    ' /   __| _` | __|  _ \   __|
//    . \  |   (   | |   (   |\__ `
//   _|\_\_|  \__,_|\__|\___/ ____/
//                   Multi-Physics
//
//  License:		 BSD License
//					 Kratos default license: kratos/license.txt
//
//  Main authors:    Vicente Mataix Ferrandiz
//

// System includes
#include <limits>

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

// Project includes
#include "testing/testing.h"
#include "containers/model.h"

// Utility includes
#include "includes/define.h"
#include "includes/model_part.h"
#include "spaces/ublas_space.h"
#include "solving_strategies/schemes/residual_based_newmark_displacement_scheme.hpp"
#include "solving_strategies/schemes/residual_based_bossak_displacement_scheme.hpp"
#include "solving_strategies/schemes/residual_based_bdf_displacement_scheme.h"

namespace Kratos
{
    namespace Testing
    {
        /// Tests

        typedef UblasSpace<double, CompressedMatrix, Vector> SparseSpaceType;
        typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;

        typedef Scheme< SparseSpaceType, LocalSpaceType >  SchemeType;
        typedef PointerVectorSet<Dof<double>, SetIdentityFunction<Dof<double>>, std::less<SetIdentityFunction<Dof<double>>::result_type>, std::equal_to<SetIdentityFunction<Dof<double>>::result_type>, Dof<double>* > DofsArrayType;

        static inline DofsArrayType BasicTestSchemeDisplacement(
            ModelPart& ModelPart,
            SchemeType::Pointer pScheme,
            std::vector< Dof<double>::Pointer >& DoF,
            const double DeltaTime
            )
        {
            ModelPart.SetBufferSize(3);

            ModelPart.AddNodalSolutionStepVariable(DISPLACEMENT);
            ModelPart.AddNodalSolutionStepVariable(VELOCITY);
            ModelPart.AddNodalSolutionStepVariable(ACCELERATION);

            auto pnode = ModelPart.CreateNewNode(1, 0.0, 0.0, 0.0);

            pnode->AddDof(DISPLACEMENT_X);
            pnode->AddDof(DISPLACEMENT_Y);
            pnode->AddDof(DISPLACEMENT_Z);

            ModelPart.GetProcessInfo()[DELTA_TIME] = DeltaTime;

            DoF.reserve(3);
            DoF.push_back(pnode->pGetDof(DISPLACEMENT_X));
            DoF.push_back(pnode->pGetDof(DISPLACEMENT_Y));
            DoF.push_back(pnode->pGetDof(DISPLACEMENT_Z));

            // Set initial solution
            const array_1d<double, 3> zero_vector = ZeroVector(3);
            pnode->FastGetSolutionStepValue(DISPLACEMENT) = zero_vector;
            pnode->FastGetSolutionStepValue(DISPLACEMENT, 1) = zero_vector;
            pnode->FastGetSolutionStepValue(DISPLACEMENT, 2) = zero_vector;
            pnode->FastGetSolutionStepValue(VELOCITY) = zero_vector;
            pnode->FastGetSolutionStepValue(VELOCITY, 1) = zero_vector;
            pnode->FastGetSolutionStepValue(VELOCITY, 2) = zero_vector;
            pnode->FastGetSolutionStepValue(ACCELERATION) = zero_vector;
            pnode->FastGetSolutionStepValue(ACCELERATION, 1) = zero_vector;
            pnode->FastGetSolutionStepValue(ACCELERATION, 2) = zero_vector;

            DofsArrayType Doftemp;
            Doftemp.reserve(DoF.size());
            for (auto it= DoF.begin(); it!= DoF.end(); it++)
            {
                Doftemp.push_back( it->get() );
            }
            Doftemp.Sort();

			CompressedMatrix A(boost::numeric::ublas::zero_matrix<double>(3, 3));
            Vector Dx = ZeroVector(3);
            Vector b = ZeroVector(3);

            pScheme->Initialize(ModelPart);

            return Doftemp;
        }

        /**
         * Checks if the Newmark scheme performs correctly the integration
         */

        KRATOS_TEST_CASE_IN_SUITE(DisplacementNewmarkSchemeTest, KratosCoreFastSuite)
        {
            Model current_model;

            constexpr double tolerance = 1e-6;

            ModelPart& model_part = current_model.CreateModelPart("Main");

            typedef ResidualBasedNewmarkDisplacementScheme< SparseSpaceType, LocalSpaceType >  ResidualBasedNewmarkDisplacementSchemeType;
            SchemeType::Pointer pscheme = SchemeType::Pointer( new ResidualBasedNewmarkDisplacementSchemeType() );

            const double delta_time = 1.0e-4;

            std::vector< Dof<double>::Pointer > DoF;
            DofsArrayType Doftemp = BasicTestSchemeDisplacement(model_part, pscheme, DoF, delta_time);

            CompressedMatrix A = boost::numeric::ublas::zero_matrix<double>(3, 3);
            Vector Dx = ZeroVector(3);
            Vector b = ZeroVector(3);

            double time = 0;

            Node<3>::Pointer pnode = model_part.pGetNode(1);

            pnode->FastGetSolutionStepValue(DISPLACEMENT_X) = std::cos(time);
            pnode->FastGetSolutionStepValue(DISPLACEMENT_X, 1) = std::cos(time - delta_time);
            pnode->FastGetSolutionStepValue(DISPLACEMENT_X, 2) = std::cos(time - 2.0 * delta_time);
            pnode->FastGetSolutionStepValue(VELOCITY_X) = -std::sin(time);
            pnode->FastGetSolutionStepValue(VELOCITY_X, 1) = -std::sin(time - delta_time);
            pnode->FastGetSolutionStepValue(VELOCITY_X, 2) = -std::sin(time - 2.0 * delta_time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X) = -std::cos(time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X, 1) = -std::cos(time - delta_time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X, 2) = -std::cos(time - 2.0 * delta_time);

            pscheme->Initialize(model_part);

            const unsigned int number_iterations = 10;
            for (unsigned int iter = 0; iter < number_iterations; ++iter)
            {
                time += delta_time;

                model_part.CloneTimeStep(time);

                Dx[0] = std::cos(time) - std::cos(time - delta_time);

                pscheme->InitializeSolutionStep(model_part, A, Dx, b);
                pscheme->Update(model_part, Doftemp, A, Dx, b);

                const double x = pnode->FastGetSolutionStepValue(DISPLACEMENT_X);
                const double v = pnode->FastGetSolutionStepValue(VELOCITY_X);
                const double a = pnode->FastGetSolutionStepValue(ACCELERATION_X);

//                 // Debug
//                 std::cout << time << "\t" << x << "\t" << v << "\t" << a << std::endl;

                KRATOS_CHECK_LESS_EQUAL(std::abs(x - std::cos(time)), tolerance);
                KRATOS_CHECK_LESS_EQUAL(std::abs(v + std::sin(time)), tolerance);
                KRATOS_CHECK_LESS_EQUAL(std::abs(a + std::cos(time)), tolerance);
            }
        }

        /**
         * Checks if the Bossak scheme performs correctly the integration
         */

        KRATOS_TEST_CASE_IN_SUITE(DisplacementBossakSchemeTest, KratosCoreFastSuite)
        {
            Model current_model;

            constexpr double tolerance = 1e-6;

            ModelPart& model_part = current_model.CreateModelPart("Main");

            typedef ResidualBasedBossakDisplacementScheme< SparseSpaceType, LocalSpaceType >  ResidualBasedBossakDisplacementSchemeType;
            SchemeType::Pointer pscheme = SchemeType::Pointer( new ResidualBasedBossakDisplacementSchemeType() );

            const double delta_time = 1.0e-4;

            std::vector< Dof<double>::Pointer > DoF;
            DofsArrayType Doftemp = BasicTestSchemeDisplacement(model_part, pscheme, DoF, delta_time);

            CompressedMatrix A = boost::numeric::ublas::zero_matrix<double>(3, 3);
            Vector Dx = ZeroVector(3);
            Vector b = ZeroVector(3);

            double time = 0;

            Node<3>::Pointer pnode = model_part.pGetNode(1);

            pnode->FastGetSolutionStepValue(DISPLACEMENT_X) = std::cos(time);
            pnode->FastGetSolutionStepValue(DISPLACEMENT_X, 1) = std::cos(time - delta_time);
            pnode->FastGetSolutionStepValue(DISPLACEMENT_X, 2) = std::cos(time - 2.0 * delta_time);
            pnode->FastGetSolutionStepValue(VELOCITY_X) = -std::sin(time);
            pnode->FastGetSolutionStepValue(VELOCITY_X, 1) = -std::sin(time - delta_time);
            pnode->FastGetSolutionStepValue(VELOCITY_X, 2) = -std::sin(time - 2.0 * delta_time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X) = -std::cos(time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X, 1) = -std::cos(time - delta_time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X, 2) = -std::cos(time - 2.0 * delta_time);

            pscheme->Initialize(model_part);

            const unsigned int number_iterations = 10;
            for (unsigned int iter = 0; iter < number_iterations; ++iter)
            {
                time += delta_time;

                model_part.CloneTimeStep(time);

                Dx[0] = std::cos(time) - std::cos(time - delta_time);

                pscheme->InitializeSolutionStep(model_part, A, Dx, b);
                pscheme->Update(model_part, Doftemp, A, Dx, b);

                const double x = pnode->FastGetSolutionStepValue(DISPLACEMENT_X);
                const double v = pnode->FastGetSolutionStepValue(VELOCITY_X);
                const double a = pnode->FastGetSolutionStepValue(ACCELERATION_X);

//                 // Debug
//                 std::cout << time << "\t" << x << "\t" << v << "\t" << a << std::endl;

                KRATOS_CHECK_LESS_EQUAL(std::abs(x - std::cos(time)), tolerance);
                KRATOS_CHECK_LESS_EQUAL(std::abs(v + std::sin(time)), tolerance);
                KRATOS_CHECK_LESS_EQUAL(std::abs(a + std::cos(time)), tolerance);
            }
        }

        /**
         * Checks if the BDF2 scheme performs correctly the integration
         */

        KRATOS_TEST_CASE_IN_SUITE(DisplacementBDF2SchemeTest, KratosCoreFastSuite)
        {
            Model current_model;

            constexpr double tolerance = 1e-6;

            ModelPart& model_part = current_model.CreateModelPart("Main");

            typedef ResidualBasedBDFDisplacementScheme< SparseSpaceType, LocalSpaceType >  ResidualBasedBDFDisplacementSchemeType;
            SchemeType::Pointer pscheme = SchemeType::Pointer( new ResidualBasedBDFDisplacementSchemeType(2) );

            const double delta_time = 1.0e-4;

            std::vector< Dof<double>::Pointer > DoF;
            DofsArrayType Doftemp = BasicTestSchemeDisplacement(model_part, pscheme, DoF, delta_time);

            CompressedMatrix A = boost::numeric::ublas::zero_matrix<double>(3, 3);
            Vector Dx = ZeroVector(3);
            Vector b = ZeroVector(3);

            double time = 0;

            Node<3>::Pointer pnode = model_part.pGetNode(1);

            pnode->FastGetSolutionStepValue(DISPLACEMENT_X) = std::cos(time);
            pnode->FastGetSolutionStepValue(DISPLACEMENT_X, 1) = std::cos(time - delta_time);
            pnode->FastGetSolutionStepValue(DISPLACEMENT_X, 2) = std::cos(time - 2.0 * delta_time);
            pnode->FastGetSolutionStepValue(VELOCITY_X) = -std::sin(time);
            pnode->FastGetSolutionStepValue(VELOCITY_X, 1) = -std::sin(time - delta_time);
            pnode->FastGetSolutionStepValue(VELOCITY_X, 2) = -std::sin(time - 2.0 * delta_time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X) = -std::cos(time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X, 1) = -std::cos(time - delta_time);
            pnode->FastGetSolutionStepValue(ACCELERATION_X, 2) = -std::cos(time - 2.0 * delta_time);

            pscheme->Initialize(model_part);

            const unsigned int number_iterations = 10;
            for (unsigned int iter = 0; iter < number_iterations; ++iter)
            {
                time += delta_time;

                model_part.CloneTimeStep(time);

                Dx[0] = std::cos(time) - std::cos(time - delta_time);

                pscheme->InitializeSolutionStep(model_part, A, Dx, b);
                pscheme->Update(model_part, Doftemp, A, Dx, b);

                const double x = pnode->FastGetSolutionStepValue(DISPLACEMENT_X);
                const double v = pnode->FastGetSolutionStepValue(VELOCITY_X);
                const double a = pnode->FastGetSolutionStepValue(ACCELERATION_X);

//                 // Debug
//                 std::cout << time << "\t" << x << "\t" << v << "\t" << a << std::endl;

                KRATOS_CHECK_LESS_EQUAL(std::abs(x - std::cos(time)), tolerance);
                KRATOS_CHECK_LESS_EQUAL(std::abs(v + std::sin(time)), tolerance);
                KRATOS_CHECK_LESS_EQUAL(std::abs(a + std::cos(time)), tolerance);
            }
        }

    } // namespace Testing
}  // namespace Kratos.


/* Copyright (C) 2017 IBM Corp.
 *  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.
 */
/*****************************************************************************
 * DGaussSampler.cpp - Sampling from non-spherical discrete Gaussian.
 *   A vector x is sampled one entry at a time, each entry sampled from
 *   the marginal distribution, conditioned on all previous entries.
 ******************************************************************************/
#include <NTL/ZZ.h>
#include <NTL/mat_lzz_p.h>
#include <limits>
#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>
#include <NTL/FFT.h>
#include <NTL/SmartPtr.h>

NTL_CLIENT
#include "mat_l.h"
#include "vec_l.h"
#include "DGaussSampler.h"
#include "Gaussian1Dsampler.h"
#include "utils/tools.h"

#ifdef NTL_HAVE_AVX
#warning "HAVE_AVX"

#include <immintrin.h>

#ifdef NTL_HAVE_FMA
#define MUL_ADD(a, b, c) a = _mm256_fmadd_pd(b, c, a)
#else
#define MUL_ADD(a, b, c) a = _mm256_add_pd(a, _mm256_mul_pd(b, c))
#endif
#endif

// Choose a small dimension-n integer vector
void setSmall(vec_l& u, long n, double sigma)
{
    FHE_TIMER_START;
    u.SetLength(n);
    NTL::Pair<bool,long> extra(false,0);

    // Choose a small noise vector
    Gaussian1Dsampler gSampler(sigma);
    for (long i=0; i<n; i++)
      u[i] = gSampler.getSample(/*mu=*/0.0, extra);
}

// Choose a small n-by-m integer matrix
void setSmall(mat_l& E, long n, long m, double sigma)
{
    FHE_TIMER_START;
    E.SetDims(n, m); // the noise n-by-m matrix E

    // Choose a small noise matrix
    Gaussian1Dsampler gSampler(sigma);

    EXEC_RANGE(m, first, last)

    NTL::Pair<bool,long> extra(false,0);
    for (long j=first; j<last; j++)
    for (long i=0; i<n; i++){
        //for (long j=0; j<m; j++){
	E[i][j] = gSampler.getSample(/*mu=*/0.0, extra);
      }
      EXEC_RANGE_END
}

// Initialize to a convariance matrix, returns false on failure. The i'th
// vector in condSigmaV is the top row of the conditional covariance matrix
// of entries i,...,n ocnditioned on 1,...,i-1. Also initializes a 1D sample
// with variance condSigmaV[i][i] for each entry i.

#ifdef NTL_HAVE_AVX
// ******* AVX code
// Most of this was taken verbatim from NTL's mat_lzz_p.c

#define MAT_BLK_SZ (32)

#define PAR_THRESH_SQ (200)
#define PAR_THRESH (40000)

// MUL_ADD(a, b, c): a += b*c
#ifdef NTL_HAVE_FMA
#define MUL_ADD(a, b, c) a = _mm256_fmadd_pd(b, c, a)
#else
#define MUL_ADD(a, b, c) a = _mm256_add_pd(a, _mm256_mul_pd(b, c))
#endif

static
void muladd1_by_32(double *x, const double *a, const double *b, long n)
{
   __m256d acc0=_mm256_load_pd(x + 0*4);
   __m256d acc1=_mm256_load_pd(x + 1*4);
   __m256d acc2=_mm256_load_pd(x + 2*4);
   __m256d acc3=_mm256_load_pd(x + 3*4);
   __m256d acc4=_mm256_load_pd(x + 4*4);
   __m256d acc5=_mm256_load_pd(x + 5*4);
   __m256d acc6=_mm256_load_pd(x + 6*4);
   __m256d acc7=_mm256_load_pd(x + 7*4);

   long i = 0;
   for (; i <= n-4; i +=4) {

      // the following code sequences are a bit faster than
      // just doing 4 _mm256_broadcast_sd's
      // it requires a to point to aligned storage, however

     // this one seems slightly faster
      __m256d a0101 = _mm256_broadcast_pd((const __m128d*)(a+0));
      __m256d a2323 = _mm256_broadcast_pd((const __m128d*)(a+2));

      __m256d avec0 = _mm256_permute_pd(a0101, 0);
      __m256d avec1 = _mm256_permute_pd(a0101, 0xf);
      __m256d avec2 = _mm256_permute_pd(a2323, 0);
      __m256d avec3 = _mm256_permute_pd(a2323, 0xf);

      a += 4;

      __m256d bvec;

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec0, bvec);

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec1, bvec);

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec2, bvec);

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec3, bvec);
   }

   for (; i < n; i++) {
      __m256d avec = _mm256_broadcast_sd(a); a++;
      __m256d bvec;

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec, bvec);
   }

   _mm256_store_pd(x + 0*4, acc0);
   _mm256_store_pd(x + 1*4, acc1);
   _mm256_store_pd(x + 2*4, acc2);
   _mm256_store_pd(x + 3*4, acc3);
   _mm256_store_pd(x + 4*4, acc4);
   _mm256_store_pd(x + 5*4, acc5);
   _mm256_store_pd(x + 6*4, acc6);
   _mm256_store_pd(x + 7*4, acc7);
}

// experiment: process two rows at a time
#ifndef NTL_HAVE_FMA
static
void muladd2_by_32(double *x, const double *a, const double *b, long n)
{
   __m256d avec0, avec1, bvec;
   __m256d acc00, acc01, acc02, acc03;
   __m256d acc10, acc11, acc12, acc13;

   // round 0

   acc00=_mm256_load_pd(x + 0*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 1*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 2*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 3*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 0*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 1*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 2*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 3*4 + 1*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec);
   }

   _mm256_store_pd(x + 0*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 1*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 2*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 3*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 0*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 1*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 2*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 3*4 + 1*MAT_BLK_SZ, acc13);

   // round 1

   acc00=_mm256_load_pd(x + 4*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 5*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 6*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 7*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 4*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 5*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 6*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 7*4 + 1*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4+MAT_BLK_SZ/2]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4+MAT_BLK_SZ/2]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4+MAT_BLK_SZ/2]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4+MAT_BLK_SZ/2]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec);
   }

   _mm256_store_pd(x + 4*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 5*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 6*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 7*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 4*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 5*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 6*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 7*4 + 1*MAT_BLK_SZ, acc13);

}
#endif

// experiment: process three rows at a time
// NOTE: this makes things slower on an AVX1 platform --- not enough registers
// it could be faster on AVX2/FMA, where there should be enough registers

static
void muladd3_by_32(double *x, const double *a, const double *b, long n)
{
   __m256d avec0, avec1, avec2, bvec;
   __m256d acc00, acc01, acc02, acc03;
   __m256d acc10, acc11, acc12, acc13;
   __m256d acc20, acc21, acc22, acc23;

   // round 0

   acc00=_mm256_load_pd(x + 0*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 1*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 2*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 3*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 0*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 1*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 2*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 3*4 + 1*MAT_BLK_SZ);

   acc20=_mm256_load_pd(x + 0*4 + 2*MAT_BLK_SZ);
   acc21=_mm256_load_pd(x + 1*4 + 2*MAT_BLK_SZ);
   acc22=_mm256_load_pd(x + 2*4 + 2*MAT_BLK_SZ);
   acc23=_mm256_load_pd(x + 3*4 + 2*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);
      avec2 = _mm256_broadcast_sd(&a[i+2*MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec); MUL_ADD(acc20, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec); MUL_ADD(acc21, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec); MUL_ADD(acc22, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec); MUL_ADD(acc23, avec2, bvec);
   }

   _mm256_store_pd(x + 0*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 1*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 2*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 3*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 0*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 1*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 2*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 3*4 + 1*MAT_BLK_SZ, acc13);

   _mm256_store_pd(x + 0*4 + 2*MAT_BLK_SZ, acc20);
   _mm256_store_pd(x + 1*4 + 2*MAT_BLK_SZ, acc21);
   _mm256_store_pd(x + 2*4 + 2*MAT_BLK_SZ, acc22);
   _mm256_store_pd(x + 3*4 + 2*MAT_BLK_SZ, acc23);

   // round 1

   acc00=_mm256_load_pd(x + 4*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 5*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 6*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 7*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 4*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 5*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 6*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 7*4 + 1*MAT_BLK_SZ);

   acc20=_mm256_load_pd(x + 4*4 + 2*MAT_BLK_SZ);
   acc21=_mm256_load_pd(x + 5*4 + 2*MAT_BLK_SZ);
   acc22=_mm256_load_pd(x + 6*4 + 2*MAT_BLK_SZ);
   acc23=_mm256_load_pd(x + 7*4 + 2*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);
      avec2 = _mm256_broadcast_sd(&a[i+2*MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4+MAT_BLK_SZ/2]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec); MUL_ADD(acc20, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4+MAT_BLK_SZ/2]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec); MUL_ADD(acc21, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4+MAT_BLK_SZ/2]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec); MUL_ADD(acc22, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4+MAT_BLK_SZ/2]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec); MUL_ADD(acc23, avec2, bvec);
   }

   _mm256_store_pd(x + 4*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 5*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 6*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 7*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 4*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 5*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 6*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 7*4 + 1*MAT_BLK_SZ, acc13);

   _mm256_store_pd(x + 4*4 + 2*MAT_BLK_SZ, acc20);
   _mm256_store_pd(x + 5*4 + 2*MAT_BLK_SZ, acc21);
   _mm256_store_pd(x + 6*4 + 2*MAT_BLK_SZ, acc22);
   _mm256_store_pd(x + 7*4 + 2*MAT_BLK_SZ, acc23);

}

static inline
void muladd_all_by_32(long first, long last, double *x, const double *a, const double *b, long n)
{
   long i = first;
#ifdef NTL_HAVE_FMA
   // processing three rows at a time is faster
   for (; i <= last-3; i+=3)
      muladd3_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
   for (; i < last; i++)
      muladd1_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
#else
   // process only two rows at a time: not enough registers :-(
   for (; i <= last-2; i+=2)
      muladd2_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
   for (; i < last; i++)
      muladd1_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
#endif
}

// this assumes n is a multiple of 16
static inline
void muladd_interval(double * NTL_RESTRICT x, double * NTL_RESTRICT y, double c, long n)
{
   __m256d xvec0, xvec1, xvec2, xvec3;
   __m256d yvec0, yvec1, yvec2, yvec3;

   __m256d cvec = _mm256_broadcast_sd(&c);

   for (long i = 0; i < n; i += 16, x += 16, y += 16) {
      xvec0 = _mm256_load_pd(x+0*4);
      xvec1 = _mm256_load_pd(x+1*4);
      xvec2 = _mm256_load_pd(x+2*4);
      xvec3 = _mm256_load_pd(x+3*4);

      yvec0 = _mm256_load_pd(y+0*4);
      yvec1 = _mm256_load_pd(y+1*4);
      yvec2 = _mm256_load_pd(y+2*4);
      yvec3 = _mm256_load_pd(y+3*4);

      MUL_ADD(xvec0, yvec0, cvec);
      MUL_ADD(xvec1, yvec1, cvec);
      MUL_ADD(xvec2, yvec2, cvec);
      MUL_ADD(xvec3, yvec3, cvec);

      _mm256_store_pd(x + 0*4, xvec0);
      _mm256_store_pd(x + 1*4, xvec1);
      _mm256_store_pd(x + 2*4, xvec2);
      _mm256_store_pd(x + 3*4, xvec3);
   }
}

static
bool Alt_sampler(Vec< Vec<double> >& X, const Mat<long>& A)
{
   FHE_TIMER_START;

   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("sampler: nonsquare matrix");

   if (NTL_OVERFLOW(n, MAT_BLK_SZ, 0)) ResourceError("dimension too large");

   long npanels = (n+MAT_BLK_SZ-1)/MAT_BLK_SZ;

   Vec< AlignedArray<double> > M;
   M.SetLength(npanels);
   for (long panel = 0; panel < npanels; panel++) {
      M[panel].SetLength(n*MAT_BLK_SZ);
      double *panelp = &M[panel][0];

      for (long r = 0; r < n*MAT_BLK_SZ; r++) panelp[r] = 0;
   }

   // copy A into panels
   for (long jj = 0, panel = 0; jj < n; jj += MAT_BLK_SZ, panel++) {
      long j_max = min(jj+MAT_BLK_SZ, n);
      double *panelp = &M[panel][0];

      for (long i = 0; i < n; i++, panelp += MAT_BLK_SZ) {
         const long *ap = A[i].elts() + jj;

         for (long j = jj; j < j_max; j++)
            panelp[j-jj] = ap[j-jj];
      }
   }

   X.SetLength(n);

   for (long kk = 0, kpanel = 0; kk < n; kk += MAT_BLK_SZ, kpanel++) {
      long k_max = min(kk+MAT_BLK_SZ, n);

      double * NTL_RESTRICT kpanelp = &M[kpanel][0];

      for (long k = kk; k < k_max; k++) {

         X[k].SetLength(n-k);
         double * NTL_RESTRICT Xk = X[k].elts();
         for (long i = k; i < n; i++)
            Xk[i-k] = kpanelp[i*MAT_BLK_SZ + (k-kk)];

         double * NTL_RESTRICT y = &kpanelp[k*MAT_BLK_SZ];
         double pivot = y[k-kk];
         y[k-kk] = 1;

         if (pivot <= 0) {
            return false;
         }

         double pivot_inv = 1/pivot;

         for (long i = k+1; i < n; i++) {
            double * NTL_RESTRICT x = &kpanelp[i*MAT_BLK_SZ];
            double t1 = -x[k-kk]*pivot_inv;
            x[k-kk] = 0;
            muladd_interval(x, y, t1, MAT_BLK_SZ);
         }

         for (long j = k; j < k_max; j++) y[j-kk] = 0;
      }

      // finished processing current kpanel
      // next, reduce and apply to all other kpanels

      bool seq = double(npanels-(kpanel+1))*double(n)*double(MAT_BLK_SZ)*double(MAT_BLK_SZ) < PAR_THRESH;

      NTL_GEXEC_RANGE(seq, npanels-(kpanel+1), first, last)
      NTL_IMPORT(n)
      NTL_IMPORT(kpanel)
      NTL_IMPORT(kpanelp)
      NTL_IMPORT(kk)
      NTL_IMPORT(k_max)

      AlignedArray<double> buf_store;
      buf_store.SetLength(MAT_BLK_SZ*MAT_BLK_SZ);
      double *buf = &buf_store[0];

      for (long index = first; index < last; index++) {
         long jpanel = index + kpanel+1;

         double * NTL_RESTRICT jpanelp = &M[jpanel][0];

         // copy block number kpanel (the one on the diagonal)  into buf

         for (long i = 0; i < (k_max-kk)*MAT_BLK_SZ; i++)
            buf[i] = jpanelp[kk*MAT_BLK_SZ+i];

         // jpanel += kpanel*buf

         muladd_all_by_32(kk, n, jpanelp, kpanelp, buf, k_max-kk);
      }

      NTL_GEXEC_RANGE_END

   }

   return true;

}
#endif

//create the nested covariance matrices
static
bool Plain_sampler(Vec< Vec<double> >& X, const Mat<long>& A)
{
    FHE_TIMER_START;

    if (A[0][0] < 0)   //sigma too small
    {
        return false; // failed, variance cannot be negative
    }

    long n = A.NumRows();

    if (A.NumCols() != n)
       LogicError("sampler: nonsquare matrix");

    X.SetLength(n); // allocate space for n vectors

    Mat<double> M;
    conv(M, A); // convert to floating point

    // 1st vector of the conditional covariance = 1st column of covariance
    X[0].SetLength(n);
    for (long k = 0; k < n; k++)
        X[0][k] = M[k][0];

    // Compute the rest of the conditional covariance, one vector at a time

    for (long k = 1; k<n; k++)
    {
        // create the new covariance: sig y|x = sig_yy - sig_yx *sig_xx^{-1} *sig_xy

        //get the subset covariance matrix

        NTL_GEXEC_RANGE(n-k < 100, n-k, first, last)

        NTL_IMPORT(n)
        NTL_IMPORT(k)

        double * NTL_RESTRICT M_0 = &M[k-1][0];
        double pivot = M_0[k-1];
        double pivot_inv = 1.0/pivot;

        for (long row = first; row < last; row++) {
            double * NTL_RESTRICT M_r = &M[k+row][0];
            double fac = -M_r[k-1]*pivot_inv;

            for (long col = k; col < n; col++) {
                M_r[col] += fac * (M_0[col]);
            }
        }

        NTL_GEXEC_RANGE_END

        if (M[k][k] < 0)
        {
            return false; //failed, sigma too small
        }

        // copy the result to the next conditional covariance vector
        X[k].SetLength(n-k);
        for (long i = 0; i < n-k; i++)
            X[k][i] = M[k+i][k];
    }

    return true;
}

bool DiscreteGaussianSampler::InitSampler(const Mat<long> &covMat)
{
    FHE_TIMER_START;

    n = covMat.NumRows();

    bool res = false;

#ifdef NTL_HAVE_AVX
#warning "using AVX code"
   if (n >= 512) {
      res = Alt_sampler(condSigmaV, covMat);
   }
   else
#endif
   {
      res = Plain_sampler(condSigmaV, covMat);
   }

   if (!res) return false;

    //clear if there are previous 1D-samplers
    for (long i = 0; i < oneDsamplers.length(); i++)
        if (oneDsamplers[i] != NULL) delete oneDsamplers[i];

    // create table of 1D-samplers
    oneDsamplers.SetLength(n);

    for (long iVar=0; iVar<n; iVar++)
    {
        if (condSigmaV[iVar][0] < 0)
        {
            cout << "conditional sigma too small" << endl;
            return false; //conditional sigma too small
            }

        oneDsamplers[iVar] =            //create a 1D sampler for this sigma
        (Gaussian1Dsampler*) new Gaussian1Dsampler(sqrt(condSigmaV[iVar][0]));

    }

    return true;
}

// FIXME: better to use UniquePtr or something to manage the entries
// in oneDsamplers, rather than new/delete

//delete the array of sampler instances
DiscreteGaussianSampler::~DiscreteGaussianSampler()
{
    FHE_TIMER_START;
    for (long i = 0; i < oneDsamplers.length(); i++)
        if (oneDsamplers[i]) delete oneDsamplers[i];
}

// Discrete sampling according to the conditional variance and mean. Choose
//each entry in x depending on the conditioned distribution of previous ones.

void DiscreteGaussianSampler::SampleDiscreteGaussian(vec_l& xOut, const Vec<double>& meanVec) const
{
    FHE_TIMER_START;

    //for this val, find the rest of the values

    //x,y are matrices
    //m y|x = m_y + sig_yx * sig ^-1_xx (x - m_x)
    //multiple matrices

    // scratch variables for internal computation
    Vec<double> marginalMeanVec = meanVec;
    Vec<double> nextMarginalMeanVec;

    // work with pointers for easier swapping
    Vec<double>* curMeanVec = &marginalMeanVec;
    Vec<double>* nextMeanVec = &nextMarginalMeanVec;

    //find marginal distribution for each variable

    xOut.SetLength(n);
    for (long iVar = 0; iVar<n; iVar++)
    {
        //get the next sample
        double mean = (*curMeanVec)[0];
        xOut[iVar] = oneDsamplers[iVar]->getSample(mean);

        // update the mean vector:
        //    m(i|j) = m(i) + sigma(i,j)*sigma(j,j)^-1*(x(j) - m(j))
        // only need the first element for the next item

        // Next mean vector has one dimension less
        nextMeanVec->SetLength(curMeanVec->length() -1);

        double diff = xOut[iVar] - mean;  // deviation from mean

        // Look at the next vector from the conditional covariance
        const Vec<double>& covVec = condSigmaV[iVar];

        // newMean = curMean[1..n-1] + covVec * diff / sigma_XX

        double invSig = 1.0 / covVec[0]; // 1/sigma_XX
        for (long row = 0; row<nextMeanVec->length(); row++)
            (*nextMeanVec)[row]
                = (*curMeanVec)[row+1] + (covVec[row+1] *invSig) *diff;

        // Set the updated mean vector as the current one
        swap(nextMeanVec,curMeanVec); // pointer swap
    }
}

long DiscreteGaussianSampler::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fwrite(&n,sizeof(long),1,handle); // write dimension

    long nConvSig = condSigmaV.length();
    count += fwrite(&nConvSig,sizeof(long),1,handle);

    for (long i = 0; i < nConvSig; i++)
    {
    long nconvSigmaVi = condSigmaV[i].length();
    count += fwrite(&nconvSigmaVi,sizeof(long),1,handle);
    count += fwrite(condSigmaV[i].elts(),condSigmaV[i].length()*sizeof(double),1,handle);
    }

    long oLength = oneDsamplers.length();
    count += fwrite(&oLength,sizeof(long),1,handle);

    for (long i = 0; i < oLength; i++)
        count+= oneDsamplers[i]->writeToFile(handle);

    return count;
}

long DiscreteGaussianSampler::readFromFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fread(&n,sizeof(long),1,handle); // read dimension

    long nConvSig;
    count += fread(&nConvSig,sizeof(long),1,handle);

    condSigmaV.SetLength(nConvSig);

    for (long i = 0; i < nConvSig; i++)
    {
    long nconvSigmaVi;
    count += fread(&nconvSigmaVi,sizeof(long),1,handle);
    condSigmaV[i].SetLength(nconvSigmaVi);
    count += fread(condSigmaV[i].elts(),nconvSigmaVi*sizeof(double),1,handle);
    }

    long oLength;
    count += fread(&oLength,sizeof(long),1,handle);
    oneDsamplers.SetLength(oLength);

    for (long iVar=0; iVar<oLength; iVar++)
    {
        oneDsamplers[iVar] =            //create a 1D sampler for this sigma
            (Gaussian1Dsampler*) new Gaussian1Dsampler(sqrt(condSigmaV[iVar][0]));
    }

    for (long i = 0; i < oLength; i++)
        count+= oneDsamplers[i]->readFromFile(handle);

    return count;
}


#pragma once

#include <memory>
#include <set>
#include <chrono>

#include <Eigen/Dense>

#include "lookup.hpp"
#include "group.hpp"
#include "trajectory.hpp"
#include "group_command.hpp"
#include "group_feedback.hpp"
#include "group_info.hpp"

#include "quadruped_parameters.hpp"
#include "quadruped_leg.hpp"

/*
  This class defines a quadruped robot using Hebi's daisy robot (or Mat6)
  The idea is to use leg 1 2 5 6 to run and use 3 4 to grab objects
*/

// Leg Numbering / Chassis Coordinate convention
// This should match ROS wheeled vehicle convention
//
//  1 \   / 2         +x
//     \ /            ^
//  3 ----- 4         |
//     / \      +y <--o
//  5 /   \ 6          +z
// different from hexapod, we use 3 4 to manipulate objects

using namespace Eigen;

namespace hebi {

class Quadruped
{
  public:
    // 1 2 5 6 are locomote legs, 3 4 are manipulate legs
    enum struct CtrlLegType { all, locomote, manipulate };
    enum struct SwingMode {swing_mode_virtualLeg1, swing_mode_virtualLeg2};
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Allow Eigen member variables

    // learn from hebi source code to do this fancy construction method
    static std::unique_ptr<Quadruped> create(const QuadrupedParameters& params);
    virtual ~Quadruped() noexcept;

    Eigen::Vector3d getGravityDirection();
    Eigen::VectorXd getLegJointAngles(int index);

    void computeFootForces(Eigen::MatrixXd& foot_forces);

    // these functions are called in outside state machine, they must be called every iteration
    // and, in all these modes they will call sendCommand. Which is not good because we may want to combine
    // functions in a state, so the cmd will be refreshed twice.
    // I think a better design methdology would either be
    // 1. make sure outside state machine always only call one and only one
    //    function to enter quadruped, and combine state logics all in this function
    // 2. at state machine, explicitly call sendCommand after call functions that set cmd
    //  It is just different levels of abstraction, need more thinking
    bool planStandUpTraj(double duration_time);
    bool execStandUpTraj(double curr_time);
    bool spreadAllLegs();
    bool pushAllLegs(double curr_time, double total_time);
    bool prepareQuadMode();
    void runTest(SwingMode mode, double curr_time, double total_time);
    void prepareTrajectories(SwingMode mode, double leg_swing_time);
    bool reOrient(Matrix3d target_body_R);
    bool rePos(int move_id, double tgt_x, double tgt_y, double curr_time, double total_time);

    bool moveSingleLegTraj(int move_leg_id, double x_distance, double y_distance, double leg_swing_time);
    bool moveSingleLeg(int move_leg_id, double curr_time, double total_time);
    void saveFootPose();
    Eigen::Vector2d getFootPose(int id);
    Eigen::Matrix3d getBodyR() {return body_R;}

    void startBodyRUpdate() {updateBodyR = true;}
    void setStartGait() {startGait = true;}
    void lifeManipulatorLegs();
    void gentleLiftRecover(double curr_time, double total_time);

    bool isExecution() {return is_exec_traj;}

    void setCommand(int index, const VectorXd* angles, const VectorXd* vels, const VectorXd* torques);
    void saveCommand();
    void loadCommand();
    void sendCommand();
    bool setGains();

    // Public Function below is added by Zhaoyuan, use at your own risk
    bool pushAllLegs(double curr_time, double total_time, bool& first_time_enter);

    void moveLegs(double dx, double dy, double dz = 0);
    void moveBody(double dx, double dy, double dz = 0); // move body in cm
    void moveFootRel(int footIndex, double dx = 0, double dy = 0, double dz = 0);
    // virtual void moveFootAbs(int footIndex, double dx, double dy, double dz) = 0; // TODO: make clear about the com coordinate and leg coordinate

    void planFootTraj(int footIndex, double dx, double dy, double dz, double swingTime); // plan a triangle path, dz is height, dx,dx is distance reletive to original foot
    void followFootTraj(int trajFootIndex, double timeSpent);
    void planBodyTraj(double dx, double dy, double body_move_time);
    void followBodyTraj(double timeSpent);
    void freeze(); // stay where the robot was commanded

    // function for wave gait by zhaoyuan
    void followWaveGait(double timeSpent);
    void planWaveGait();
    double getTotalTime(){return totalTime;}
    void planBodyFootTraj(int legIndex, int timeStep, Eigen::MatrixXd &positions);
    void saveStanceCommand();
    void loadStanceCommand();

    // function for dynamic walk by zhaoyuan
    void planDynamicGait(double Ldx, double Rdx, bool swingLeft);
    void followDynamicGait(double timeSpent);
    double getStepTime(){return stepTime;}
    double getDynamicStepSize(){return dynamicStepSize;}
    // double setHalfStepTime(double periodTime){periodTime = periodTime;}
    void recordFootPos(double Ldx = 0, double Rdx = 0){curFootPos = std::make_pair(Ldx, Rdx);}

    // Zhaoyuan's version of IK/FK for Daisy
    void computeIK(Eigen::VectorXd& angles, const Eigen::VectorXd& ee_com_pos, const int legIndex);
    void computeFK(Eigen::VectorXd& ee_com_pos, Eigen::VectorXd angles, const int legIndex);

  private:
    // private constructor, it make sense because before construct must make sure group is successfully created
    Quadruped(std::shared_ptr<Group> group, const QuadrupedParameters& params);

    // private functions
    Eigen::Quaterniond average_quat(Eigen::Quaterniond average_q_, std::vector<Eigen::Quaterniond> q_list_);
    Eigen::Vector3d quat_log(Eigen::Quaterniond q);
    Eigen::Quaterniond quat_exp(Eigen::Vector3d qv);

    // hebi middleware to communicate with real hardware
    std::shared_ptr<Group> group_;
    GroupCommand cmd_;
    GroupCommand saved_cmd_;
    GroupCommand saved_stance_cmd_;
    std::pair<double, double> curFootPos; // Ldx, Rdx

    // leg info
    std::vector<std::unique_ptr<QuadLeg> > legs_;
    Eigen::VectorXd joint_angles; //get joint angles from fbk and put them into leg

    std::chrono::time_point<std::chrono::steady_clock> latest_fbk_time;
    QuadrupedParameters params_;

    // feedback physical quantities
    Eigen::Vector3d gravity_direction_;
    Eigen::Matrix3d body_R;

    bool updateBodyR = false;
    bool startGait = false; // signal for first time enter Gait loop

    // two locks to get feedback
    std::mutex fbk_lock_;
    std::mutex grav_lock_;

    // planner trajectories
    std::vector<std::shared_ptr<trajectory::Trajectory>> startup_trajectories;
    std::vector<std::shared_ptr<trajectory::Trajectory>> stance_trajectories;  // used in runTest
    std::vector<std::shared_ptr<trajectory::Trajectory>> swing_trajectories;   // used in runTest and followFootTraj
    std::vector<std::shared_ptr<trajectory::Trajectory>> body_move_trajectories;  // used in static walk
    std::vector<std::shared_ptr<trajectory::Trajectory>> wave_gait_trajectories;  // used in wave gait
    std::vector<std::shared_ptr<trajectory::Trajectory>> trot_gait_trajectories;  // used in trot gait

    int trajFootIndex; // used to indicate which foot does the trajectory corresponding to
    bool is_exec_traj; // flag to show that it is still running trajectories

    // control constants
    const float fbk_frq_hz_ = 200.0f;
    const float fbk_frq_ms_ = 5.0f;
    const float ctrl_frq_hz_ = 200.0f;
    // structural constants
    static const int num_legs_ = 6;
    const int num_locomote_legs_ = 4;
    const int num_manipulate_legs_ = 2;
    const int num_joints_per_leg_ = 3;
    const int num_joints_ = num_legs_ * num_joints_per_leg_;
    const int num_locomote_joints_ = num_locomote_legs_ * num_joints_per_leg_;
    const int num_manipulate_joints_ = num_manipulate_legs_ * num_joints_per_leg_;
    static constexpr double weight_ = 9.8f * 21.0f; // mass = 21 kg

    // structural and stance constants to make system stand
    // these bars need to be tuned
    const double bar_y = 0.1187; // distance fromt com of the robot to motor 0, y direction
    const double bar_x = 0.2057; // distance fromt com of the robot to motor 0, x direction
    double foot_bar_y = 0.1187 + 0.24; // distance fromt com of the robot to foot 0, y direction
    double foot_bar_x = 0.2057 + 0.34; // distance fromt com of the robot to foot 0, x direction
    double foot_bar_y_list[4];          // (see function saveFootPose)
    double foot_bar_x_list[4];          // (see function saveFootPose)
    double foot_force_ratio[4];
    double nominal_height_z = 0.31;

    // leg Definition
    const std::vector<int> walkingLegs = {0, 1, 4, 5};
    const std::vector<int> manipulateLegs = {2, 3};

    // wave gait params
    const double totalTime = 10;
    const double stepSize = 16;

    // Dynamic Walk params
    const double stepTime = 1*1;
    const double dynamicStepSize = 15;

    Eigen::Vector4d base_stance_ee_xyz; // expressed in base motor's frame
    Eigen::Vector4d base_stance_ee_xyz_offset; // expressed in base motor's frame
    Eigen::Vector3d stance_ee_xyz_fk_offset;  // HEBI's bug: they does not consider the offset of the last link.
    Eigen::Vector3d com_stance_ee_xyz;  // expressed in com of the robot's frame

    Eigen::Matrix<double, 4, 6> mountPoints;

    const double L1 = 0.325;
    const double L2 = 0.325;

};

} // namespace hebi


//
// Copyright (c) 2016-2018 CNRS, INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/math/quaternion.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model, const JointModelBase<D> & jmodel, const Model::JointIndex parent_id, const SE3 & joint_placement, const std::string & name, const Inertia & Y)
{
  Model::JointIndex idx;
  typedef typename D::TangentVector_t TV;
  typedef typename D::ConfigVector_t CV;

  idx = model.addJoint(parent_id,jmodel,joint_placement,
                       name + "_joint",
                       TV::Zero(),
                       1e3 * (TV::Random() + TV::Constant(1)),
                       1e3 * (CV::Random() - CV::Constant(1)),
                       1e3 * (CV::Random() + CV::Constant(1))
                       );

  model.appendBodyToJoint(idx,Y,SE3::Identity());
}

void buildModel(Model & model)
{
  addJointAndBody(model,JointModelFreeFlyer(),model.getJointId("universe"),SE3::Identity(),"freeflyer",Inertia::Random());
  addJointAndBody(model,JointModelSpherical(),model.getJointId("freeflyer_joint"),SE3::Identity(),"spherical",Inertia::Random());
  addJointAndBody(model,JointModelPlanar(),model.getJointId("spherical_joint"),SE3::Identity(),"planar",Inertia::Random());
  addJointAndBody(model,JointModelRX(),model.getJointId("planar_joint"),SE3::Identity(),"rx",Inertia::Random());
  addJointAndBody(model,JointModelPX(),model.getJointId("rx_joint"),SE3::Identity(),"px",Inertia::Random());
  addJointAndBody(model,JointModelPrismaticUnaligned(SE3::Vector3(1,0,0)),model.getJointId("px_joint"),SE3::Identity(),"pu",Inertia::Random());
  addJointAndBody(model,JointModelRevoluteUnaligned(SE3::Vector3(0,0,1)),model.getJointId("pu_joint"),SE3::Identity(),"ru",Inertia::Random());
  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("ru_joint"),SE3::Identity(),"sphericalZYX",Inertia::Random());
  addJointAndBody(model,JointModelTranslation(),model.getJointId("sphericalZYX_joint"),SE3::Identity(),"translation",Inertia::Random());
}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( integration_test )
{
  Model model; buildModel(model);

  std::vector<Eigen::VectorXd> qs(2);
  std::vector<Eigen::VectorXd> qdots(2);
  std::vector<Eigen::VectorXd> results(2);

  //
  // Test Case 0 : Integration of a config with zero velocity
  //
  qs[0] = Eigen::VectorXd::Ones(model.nq);
  qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
  qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
  qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint

  qdots[0] = Eigen::VectorXd::Zero(model.nv);
  results[0] = integrate(model,qs[0],qdots[0]);

  BOOST_CHECK_MESSAGE(results[0].isApprox(qs[0], 1e-12), "integration of full body with zero velocity - wrong results");
}

BOOST_AUTO_TEST_CASE ( interpolate_test )
{
  Model model; buildModel(model);

  Eigen::VectorXd q0(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
  Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));

  Eigen::VectorXd q01_0 = interpolate(model,q0,q1,0.0);
  BOOST_CHECK_MESSAGE(isSameConfiguration(model,q01_0,q0), "interpolation: q01_0 != q0");

  Eigen::VectorXd q01_1 = interpolate(model,q0,q1,1.0);
  BOOST_CHECK_MESSAGE(isSameConfiguration(model,q01_1,q1), "interpolation: q01_1 != q1");
}

BOOST_AUTO_TEST_CASE ( diff_integration_test )
{
  Model model; buildModel(model);

  std::vector<Eigen::VectorXd> qs(2);
  std::vector<Eigen::VectorXd> vs(2);
  std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
  std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));

  qs[0] = Eigen::VectorXd::Ones(model.nq);
  qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
  qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
  qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint

  vs[0] = Eigen::VectorXd::Zero(model.nv);
  vs[1] = Eigen::VectorXd::Ones(model.nv);
  dIntegrate(model,qs[0],vs[0],results[0],ARG0);

  Eigen::VectorXd q_fd(model.nq), v_fd(model.nv); v_fd.setZero();
  const double eps = 1e-8;
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_fd[k] = eps;
    q_fd = integrate(model,qs[0],v_fd);
    results_fd[0].col(k) = difference(model,qs[0],q_fd)/eps;
    v_fd[k] = 0.;
  }
  BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));

  dIntegrate(model,qs[0],vs[0],results[1],ARG1);
  BOOST_CHECK(results[1].isApprox(results[0]));

  dIntegrate(model,qs[0],vs[1],results[0],ARG0);
  Eigen::VectorXd q_fd_intermediate(model.nq);
  Eigen::VectorXd q0_plus_v = integrate(model,qs[0],vs[1]);
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_fd[k] = eps;
    q_fd_intermediate = integrate(model,qs[0],v_fd);
    q_fd = integrate(model,q_fd_intermediate,vs[1]);
    results_fd[0].col(k) = difference(model,q0_plus_v,q_fd)/eps;
    v_fd[k] = 0.;
  }

  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));

  dIntegrate(model,qs[0],vs[1],results[1],ARG1);
  v_fd = vs[1];
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_fd[k] += eps;
    q_fd = integrate(model,qs[0],v_fd);
    results_fd[1].col(k) = difference(model,q0_plus_v,q_fd)/eps;
    v_fd[k] -= eps;
  }

  BOOST_CHECK(results[1].isApprox(results_fd[1],sqrt(eps)));
}

BOOST_AUTO_TEST_CASE ( integrate_difference_test )
{
 Model model; buildModel(model);

 Eigen::VectorXd q0(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
 Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
 Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv));

 BOOST_CHECK_MESSAGE(isSameConfiguration(model, integrate(model, q0, difference(model, q0,q1)), q1), "Integrate (difference) - wrong results");

 BOOST_CHECK_MESSAGE(difference(model, q0, integrate(model,q0, qdot)).isApprox(qdot),"difference (integrate) - wrong results");
}

BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
{
  Model model; buildModel(model);

  Eigen::VectorXd expected(model.nq);
  expected << 0,0,0,0,0,0,1,
              0,0,0,1,
              0,0,1,0,
              0,
              0,
              0,
              0,
              0,0,0,
              0,0,0;

  Eigen::VectorXd neutral_config = neutral(model);
  BOOST_CHECK_MESSAGE(neutral_config.isApprox(expected, 1e-12), "neutral configuration - wrong results");
}

BOOST_AUTO_TEST_CASE ( distance_configuration_test )
{
  Model model; buildModel(model);

  Model::ConfigVectorType q0 = neutral(model);
  Model::ConfigVectorType q1(integrate (model, q0, Model::TangentVectorType::Ones(model.nv)));

  double dist = distance(model,q0,q1);

  BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
  BOOST_CHECK_SMALL(dist-difference(model,q0,q1).norm(), 1e-12);
}

BOOST_AUTO_TEST_CASE ( squared_distance_test )
{
  Model model; buildModel(model);

  Eigen::VectorXd q0(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
  Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));

  double dist = distance(model,q0,q1);
  Eigen::VectorXd squaredDistance_ = squaredDistance(model,q0,q1);

  BOOST_CHECK_SMALL(dist-math::sqrt(squaredDistance_.sum()), 1e-12);
}

BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
{
  Model model; buildModel(model);

  model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
  model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
  Eigen::VectorXd q1(randomConfiguration(model));

  for (int i = 0; i < model.nq; ++i)
  {
    BOOST_CHECK_MESSAGE(q1[i] >= model.lowerPositionLimit[i] && q1[i] <= model.upperPositionLimit[i], " UniformlySample : Generated config not in bounds");
  }
}

BOOST_AUTO_TEST_CASE ( normalize_test )
{
  Model model; buildModel(model);

  Eigen::VectorXd q (Eigen::VectorXd::Ones(model.nq));
  pinocchio::normalize(model, q);

  BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
  BOOST_CHECK(fabs(q.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
  BOOST_CHECK(fabs(q.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
  const int n = model.nq - 7 - 4 - 4; // free flyer + spherical + planar
  BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
}

BOOST_AUTO_TEST_CASE ( integrateCoeffWiseJacobian_test )
{
  Model model; buildModels::humanoidRandom(model);

  Eigen::VectorXd q(Eigen::VectorXd::Ones(model.nq));
  pinocchio::normalize(model, q);

  Eigen::MatrixXd jac(model.nq,model.nv); jac.setZero();

  integrateCoeffWiseJacobian(model,q,jac);

  Eigen::MatrixXd jac_fd(model.nq,model.nv);
  Eigen::VectorXd q_plus;
  const double eps = 1e-8;

  Eigen::VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = integrate(model,q,v_eps);
    jac_fd.col(k) = (q_plus - q)/eps;

    v_eps[k] = 0.;
  }
  BOOST_CHECK(jac.isApprox(jac_fd,sqrt(eps)));
}

BOOST_AUTO_TEST_SUITE_END ()


/*!
@file
Defines `boost::hana::mult`.

@copyright Louis Dionne 2015
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_HANA_MULT_HPP
#define BOOST_HANA_MULT_HPP

#include <boost/hana/fwd/mult.hpp>

#include <boost/hana/concept/constant.hpp>
#include <boost/hana/concept/ring.hpp>
#include <boost/hana/core/convert.hpp>
#include <boost/hana/core/dispatch.hpp>
#include <boost/hana/detail/canonical_constant.hpp>
#include <boost/hana/detail/has_common_embedding.hpp>
#include <boost/hana/value.hpp>

#include <type_traits>

namespace boost { namespace hana {
    //! @cond
    template <typename X, typename Y>
    constexpr decltype(auto) mult_t::operator()(X&& x, Y&& y) const {
        using T = typename hana::tag_of<X>::type;
        using U = typename hana::tag_of<Y>::type;
        using Mult = BOOST_HANA_DISPATCH_IF(decltype(mult_impl<T, U>{}),
            Ring<T>::value &&
            Ring<U>::value &&
            !is_default<mult_impl<T, U>>::value
        );

    #ifndef BOOST_HANA_CONFIG_DISABLE_CONCEPT_CHECKS
        static_assert(Ring<T>::value,
        "hana::mult(x, y) requires 'x' to be in a Ring");

        static_assert(Ring<U>::value,
        "hana::mult(x, y) requires 'y' to be in a Ring");

        static_assert(!is_default<mult_impl<T, U>>::value,
        "hana::mult(x, y) requires 'x' and 'y' to be embeddable "
        "in a common Ring");
    #endif

        return Mult::apply(static_cast<X&&>(x), static_cast<Y&&>(y));
    }
    //! @endcond

    template <typename T, typename U, bool condition>
    struct mult_impl<T, U, when<condition>> : default_ {
        template <typename ...Args>
        static constexpr auto apply(Args&& ...) = delete;
    };

    // Cross-type overload
    template <typename T, typename U>
    struct mult_impl<T, U, when<
        detail::has_nontrivial_common_embedding<Ring, T, U>::value
    >> {
        using C = typename common<T, U>::type;
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y) {
            return hana::mult(hana::to<C>(static_cast<X&&>(x)),
                              hana::to<C>(static_cast<Y&&>(y)));
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model for non-boolean arithmetic data types
    //////////////////////////////////////////////////////////////////////////
    template <typename T>
    struct mult_impl<T, T, when<std::is_arithmetic<T>::value &&
                                !std::is_same<bool, T>::value>> {
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y)
        { return static_cast<X&&>(x) * static_cast<Y&&>(y); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model for Constants over a Ring
    //////////////////////////////////////////////////////////////////////////
    namespace detail {
        template <typename C, typename X, typename Y>
        struct constant_from_mult {
            static constexpr auto value = hana::mult(hana::value<X>(), hana::value<Y>());
            using hana_tag = detail::CanonicalConstant<typename C::value_type>;
        };
    }

    template <typename C>
    struct mult_impl<C, C, when<
        Constant<C>::value &&
        Ring<typename C::value_type>::value
    >> {
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X const&, Y const&)
        { return hana::to<C>(detail::constant_from_mult<C, X, Y>{}); }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_MULT_HPP


/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef _DeviceTorqueCtrl_H_
#define _DeviceTorqueCtrl_H_

#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/device.hh>
#include <sot/core/abstract-sot-external-interface.hh>

#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/common.hh>
#include <sot/torque_control/utils/logger.hh>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/tasks/task-se3-equality.hpp>

#include <Eigen/Cholesky>

namespace dgsot=dynamicgraph::sot;

namespace dynamicgraph
{
  namespace sot
  {
    namespace torque_control
    {

      /** Version of device for testing the code without the
      * real robot. This version of the device assumes that the robot
      * is torque controlled (i.e. the control input are the desired
      * joint torques). These desired joint torques are used to compute
      * the joint accelerations and the contact forces.
      * The joint accelerations are then integrated to get the
      * measured joint angles.
      *
      * The feet are supposed to be fixed to the ground. The accelerometer
      * and the gyrometer output a constant value.
      *
      * A white Gaussian noise is added to the force/torque sensor
      * measurements.
      *
      * TODO: The original Device class should be a clean abstraction for
      * the concept of device, but currently it is not. It defines a lot
      * of input/output signals that are specific of HRP-2 (e.g. zmpSIN,
      * attitudeSIN, forcesSOUT) and some design choices (e.g. secondOrderIntegration).
      * It would be nice to clean the original Device from all this stuff
      * and move it in the specific subclasses.
      */
      class DeviceTorqueCtrl: public
          dgsot::Device
      {
      public:

        static const std::string CLASS_NAME;
        static const double TIMESTEP_DEFAULT;
        static const double FORCE_SENSOR_NOISE_STD_DEV;

        virtual const std::string& getClassName () const
        {
          return CLASS_NAME;
        }

        DeviceTorqueCtrl(std::string RobotName);
        virtual ~DeviceTorqueCtrl();

        virtual void setStateSize(const unsigned int& size);
        virtual void setState(const dynamicgraph::Vector& st);
        virtual void setVelocity(const dynamicgraph::Vector & vel);
        virtual void setControlInputType(const std::string& cit);

        virtual void init(const double& dt, const std::string& urdfFile);

      protected:
        virtual void integrate(const double & dt);
        void computeForwardDynamics();

        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
        {
          getLogger().sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
        }

        /// \brief Current integration step.
        double timestep_;
        bool m_initSucceeded;

        /// input force sensor values
        dynamicgraph::SignalPtr <dynamicgraph::Vector, int>* forcesSIN_[4];

        /// Accelerations measured by accelerometers
        dynamicgraph::Signal <dynamicgraph::Vector, int> accelerometerSOUT_;
        /// Rotation velocity measured by gyrometers
        dynamicgraph::Signal <dynamicgraph::Vector, int> gyrometerSOUT_;
        /// base 6d pose + joints' angles measured by encoders
        dynamicgraph::Signal <dynamicgraph::Vector, int> robotStateSOUT_;
        /// joints' velocities computed by the integrator
        dynamicgraph::Signal <dynamicgraph::Vector, int> jointsVelocitiesSOUT_;
        /// joints' accelerations computed by the integrator
        dynamicgraph::Signal <dynamicgraph::Vector, int> jointsAccelerationsSOUT_;
        /// motor currents
        dynamicgraph::Signal <dynamicgraph::Vector, int> currentSOUT_;
        /// proportional and derivative position-control gains
        dynamicgraph::Signal <dynamicgraph::Vector, int> p_gainsSOUT_;
        dynamicgraph::Signal <dynamicgraph::Vector, int> d_gainsSOUT_;

        DECLARE_SIGNAL_IN(kp_constraints,             dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(kd_constraints,             dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(rotor_inertias,             dynamicgraph::Vector);
        DECLARE_SIGNAL_IN(gear_ratios,                dynamicgraph::Vector);

        /// Intermediate variables to avoid allocation during control
        dynamicgraph::Vector accelerometer_;
        dynamicgraph::Vector gyrometer_;

        dynamicgraph::Vector base6d_encoders_;      /// base 6d pose + joints' angles
        dynamicgraph::Vector jointsVelocities_;     /// joints' velocities
        dynamicgraph::Vector jointsAccelerations_;  /// joints' accelerations

        dynamicgraph::Vector wrenches_[4];
        dynamicgraph::Vector temp6_;

        bool m_isTorqueControlled;

        /// robot geometric/inertial data
        tsid::robots::RobotWrapper *                       m_robot;
        se3::Data *                                     m_data;
        tsid::tasks::TaskSE3Equality *             m_contactRF;
        tsid::tasks::TaskSE3Equality *             m_contactLF;
        unsigned int                                    m_nk; // number of contact forces

        tsid::math::Vector m_q, m_v, m_dv, m_f;
        tsid::math::Vector m_q_sot, m_v_sot, m_dv_sot;

        typedef Eigen::LDLT<Eigen::MatrixXd> Cholesky;
        Cholesky        m_K_chol; /// cholesky decomposition of the K matrix
        Eigen::MatrixXd m_K;
        Eigen::VectorXd m_k;
        Eigen::MatrixXd m_Jc; /// constraint Jacobian

        double                    m_numericalDamping; /// numerical damping to regularize constraint resolution
        Eigen::JacobiSVD<Matrix>  m_Jc_svd;     /// svd of the constraint matrix
        Vector                    m_dJcv;
        Matrix                    m_Z;          /// base of constraint null space
        Matrix                    m_ZMZ;        /// projected mass matrix: Z_c^T*M*Z_c
        Eigen::LDLT<Matrix>       m_ZMZ_chol;   /// Cholesky decomposition of _ZMZ
        Vector                    m_dv_c;       /// constrained accelerations
        Vector                    m_dvBar;      /// solution of Jc*dv=-dJc*v
        Vector                    m_tau_np6;
        int                       m_nj;         /// number of joints

        typedef boost::mt11213b                    ENG;    // uniform random number generator
        typedef boost::normal_distribution<double> DIST;   // Normal Distribution
        typedef boost::variate_generator<ENG,DIST> GEN;    // Variate generator
        ENG  randomNumberGenerator_;
        DIST normalDistribution_;
        GEN  normalRandomNumberGenerator_;

        RobotUtil * m_robot_util;
      };

    }   // end namespace torque_control
  }   // end namespace sot
}   // end namespace dynamicgraph
#endif /* DevicePosCtrl*/


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/def.hpp>

#include <scitbx/array_family/boost_python/c_grid_flex_conversions.h>

#include "cctbx/maptbx/asymmetric_map.h"

namespace cctbx { namespace maptbx { namespace {

void init_module()
{
  typedef asymmetric_map w_t;
  using namespace boost::python;
  typedef return_value_policy<return_by_value> rbv;
  typedef scitbx::af::const_ref<double, scitbx::af::flex_grid<> > cm_t;
  typedef scitbx::af::versa<double, scitbx::af::flex_grid<> > m_t;
  class_<w_t,boost::noncopyable>("asymmetric_map", no_init)
    .def(init< const cctbx::sgtbx::space_group_type &, cm_t >
        ( (arg("space_group_type"), arg("density_map")) ))
    .def(init< const cctbx::sgtbx::space_group_type &, m_t,
        const scitbx::af::int3 & >
        ( (arg("space_group_type"), arg("density_map"), arg("grid_size")) ))
    .def("structure_factors", &w_t::structure_factors, (arg("indices")))
    .def("map_for_fft", &w_t::map_for_fft)
    .def("symmetry_expanded_map", &w_t::symmetry_expanded_map)
    .def("data", &w_t::data, rbv())
  ;

  scitbx::af::boost_python::c_grid_flex_conversions<double,
    scitbx::af::c_interval_grid<3ul> >();
}

}}} // namespace cctbx::maptbx::anonymous

BOOST_PYTHON_MODULE(cctbx_asymmetric_map_ext)
{
  cctbx::maptbx::init_module();
}


//
//  main.cpp
//
//  Created by Dylan Menzies on 10/11/2014.
//  Copyright (c) 2014 ISVR, University of Southampton. All rights reserved.
//

#include <libpanning/VBAP.h>
#include <libpanning/AllRAD.h>

#include <libpml/matrix_parameter.hpp>

#include <boost/filesystem.hpp>
#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <vector>

BOOST_AUTO_TEST_CASE( AllRadInstantiation )
{
  using namespace visr;
  using namespace visr::panning;

  LoudspeakerArray array, regularArray;
  // AllRAD allrad;

  // Initialisation:
  boost::filesystem::path const configDir( CMAKE_SOURCE_DIR "/config" );
  boost::filesystem::path const sourceDir( CMAKE_CURRENT_SOURCE_DIR );
  boost::filesystem::path const realArrayPath = configDir / boost::filesystem::path( "generic/octahedron.xml" );
  BOOST_CHECK( exists(realArrayPath) );
  BOOST_CHECK_NO_THROW( array.loadXmlFile(realArrayPath.string().c_str() ) );
  VBAP vbap( array );

  // file = fopen("arrays/t-design_t8_P40.txt","r");
  boost::filesystem::path const regularArrayPath = sourceDir / boost::filesystem::path( "matlab/arrays/t-design_t8_P40.xml" );
  BOOST_CHECK( exists( regularArrayPath ) );
  BOOST_CHECK_NO_THROW( regularArray.loadXmlFile( regularArrayPath.string().c_str() ) );
  //allrad.setRegArray( &regularArray );

  boost::filesystem::path const coeffFilePath = sourceDir / boost::filesystem::path( "matlab/arrays/decode_N8_P40_t-design_t8_P40.txt" );
  BOOST_CHECK( exists( coeffFilePath ) );

  pml::MatrixParameter<Afloat> coeffMtx = pml::MatrixParameter<Afloat>::fromTextFile( coeffFilePath.string() );

  //explicit AllRAD( LoudspeakerArray const & regularArray,
  //  LoudspeakerArray const & realArray,
  //  efl::BasicMatrix<Afloat> const & decodeCoeffs,
  //  unsigned int maxHoaOrder );

  std::size_t const hoaOrder = 8;

  AllRAD allRAD( regularArray, array, coeffMtx, hoaOrder );

  // Initially and every time listener moves:
  vbap.setListenerPosition( 0.0, 0.0, 0.0 );

  // Load vbap with other sources and find loudspeaker gains
  // ....
  // ....

  // Load vbap with reg-array-speaker-sources
  // Calc vbap gains then calc AllRAD b-format decode gains

  //allrad.calcDecodeGains( &vbap );
  //efl::BasicMatrix<Afloat> const & decodeGains = allrad.getDecodeGains();

  //// Write b-format2vbap gains for matlab testing:

  //// file = fopen("testoutput/decodeB2VBAP.txt","w");
  //boost::filesystem::path const sourceDir( CMAKE_CURRENT_SOURCE_DIR );
  //boost::filesystem::path const referenceCoeffPath = sourceDir / boost::filesystem::path( "referencedata/decodeB2VBAP.txt" );
  //BOOST_CHECK( exists( referenceCoeffPath ) );
  //pml::MatrixParameter<Afloat> referenceCoeffs = pml::MatrixParameter<Afloat>::fromTextFile( referenceCoeffPath.string(), 0/*alignment*/ );
  //BOOST_CHECK( referenceCoeffs.numberOfRows() == 9 ); // Reference data goes only up to 9 (second-order Ambisonics)
  //BOOST_CHECK( referenceCoeffs.numberOfColumns() == array.getNumRegularSpeakers() );

  //for( int k = 0; k < 9; k++ )
  //{ // 9 harms - 2nd order only
  //  for( std::size_t j = 0; j < array.getNumRegularSpeakers(); j++ )
  //  {
  //    BOOST_CHECK_CLOSE( decodeGains(k,j), referenceCoeffs(k,j), 0.1f ); // Allowed tolerance in percent.
  //  }
  //}
}


// (C) Steve Macenski - University of Illinois at Urbana Champaign
// BSD License

// convert PointCloud2 data from the incorrect frame to the correct one by means of rotation matrices
// add in noise calculated from another script stddev=.0305 to mimic output of Google Tango
// take out some points to have density of that similar to the google tango output.

#include <ros/ros.h>
#include <iterator>
#include <stdio.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <std_msgs/String.h>
#include <sstream>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/conversions.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/projection_matrix.h>
#include <gazebo_msgs/ModelStates.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>

// globals
ros::Publisher publish_transformation;
gazebo_msgs::ModelStates state_pose;

void callback(const sensor_msgs::PointCloud2& input_cloud)
{

	// create pointer for source cloud to convert the input cloud to
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

	// convert input_cloud (PointCloud2) to PCL data type(PointXYZ), source_cloud
	pcl::fromROSMsg(input_cloud, *source_cloud);

	//rotation matrix
	Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
	transformation(0,0) = 0;//x.x		Rotation		row,column
	transformation(0,1) = 0;//x.y
	transformation(0,2) = 1;//x.z
	transformation(1,0) = -1;//y.x
	transformation(1,1) = 0;//y.y
	transformation(1,2) = 0;//y.z
	transformation(2,0) = 0;//z.x
	transformation(2,1) = -1;//z.y
	transformation(2,2) = 0;//z.z

	transformation(0,3) = 0; //translation x
	transformation(1,3) = 0; //translation y
	transformation(2,3) = 0; //translation z

	//transformed_cloud pointer
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud_XYZ (new pcl::PointCloud<pcl::PointXYZ> ());
	// transformation
	pcl::transformPointCloud (*source_cloud, *transformed_cloud_XYZ, transformation);

	// add noise into the point cloud, cloud currently in pcl::PointCloud<pcl::PointXYZ>::Ptr format
    boost::mt19937 rng; rng.seed (static_cast<unsigned int> (time (0)));
    boost::normal_distribution<> nd (0, 0.0305); // mean, stddev
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);

    for (size_t point_i = 0; point_i < transformed_cloud_XYZ->points.size (); ++point_i)
    {
      transformed_cloud_XYZ->points[point_i].x += static_cast<float> (var_nor ());
      transformed_cloud_XYZ->points[point_i].y += static_cast<float> (var_nor ());
      transformed_cloud_XYZ->points[point_i].z += static_cast<float> (var_nor ());
    }

	// remove some points in the point cloud to reduce fidelity
	pcl::PointCloud<pcl::PointXYZ>::Ptr thinned_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud (transformed_cloud_XYZ);
	sor.setLeafSize (0.045f, 0.045f, 0.045f);
	sor.filter (*thinned_cloud);

	// transforming thinned_cloud to a global frame by taking pose and offsetting in orientation/translation

	Eigen::Vector3d translation;  //translation offset
	translation[0] = state_pose.pose[2].position.x; //.x.y.z
	translation[1] = state_pose.pose[2].position.y;
	translation[2] = state_pose.pose[2].position.z;

	Eigen::Quaterniond quat;     // orientation offset
	quat.w() = state_pose.pose[2].orientation.w; //.w.x.y.z
	quat.x() = state_pose.pose[2].orientation.x;
	quat.y() = state_pose.pose[2].orientation.y;
	quat.z() = state_pose.pose[2].orientation.z;

							    // transform using offsets and PCL
	pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
	pcl::transformPointCloud(*thinned_cloud, *rotated_cloud, translation, quat);

	// storing a global point cloud
	static pcl::PointCloud<pcl::PointXYZ> global_cloud;
	global_cloud += *rotated_cloud;

	// convert transformed cloud to PointCloud2 for ROS
	sensor_msgs::PointCloud2 transformed_cloud_2;
	pcl::toROSMsg(global_cloud, transformed_cloud_2); //*thinned_cloud

	// write the frame to be transformed with respect to and time to header
    transformed_cloud_2.header.stamp = ros::Time::now();
    transformed_cloud_2.header.frame_id = "front_laser";

	// publish results
	publish_transformation.publish(transformed_cloud_2);

}

void callback_2(const gazebo_msgs::ModelStates& Model_State)
{
	::state_pose = Model_State;
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "PCDataConversion");
	ros::NodeHandle PCDataConversion;

	publish_transformation = PCDataConversion.advertise<sensor_msgs::PointCloud2>("/camera/depth/transformedPoints", 1000);
	ros::Subscriber sub = PCDataConversion.subscribe("/camera/depth/points",1000,callback);
	ros::Subscriber sub_pose = PCDataConversion.subscribe("/gazebo/model_states",1000,callback_2);

	while(ros::ok()){
		ros::spinOnce();
	}
}