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

#include "libmv/simple_pipeline/keyframe_selection.h"

#include "libmv/numeric/numeric.h"
#include "ceres/ceres.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/homography.h"
#include "libmv/multiview/fundamental.h"
#include "libmv/simple_pipeline/intersect.h"
#include "libmv/simple_pipeline/bundle.h"

#include <Eigen/Eigenvalues>

namespace libmv {
namespace {

Vec2 NorrmalizedToPixelSpace(const Vec2 &vec,
                             const CameraIntrinsics &intrinsics) {
  Vec2 result;

  double focal_length_x = intrinsics.focal_length_x();
  double focal_length_y = intrinsics.focal_length_y();

  double principal_point_x = intrinsics.principal_point_x();
  double principal_point_y = intrinsics.principal_point_y();

  result(0) = vec(0) * focal_length_x + principal_point_x;
  result(1) = vec(1) * focal_length_y + principal_point_y;

  return result;
}

Mat3 IntrinsicsNormalizationMatrix(const CameraIntrinsics &intrinsics) {
  Mat3 T = Mat3::Identity(), S = Mat3::Identity();

  T(0, 2) = -intrinsics.principal_point_x();
  T(1, 2) = -intrinsics.principal_point_y();

  S(0, 0) /= intrinsics.focal_length_x();
  S(1, 1) /= intrinsics.focal_length_y();

  return S * T;
}

class HomographySymmetricGeometricCostFunctor {
 public:
  HomographySymmetricGeometricCostFunctor(const Vec2 &x,
                                          const Vec2 &y)
      : x_(x), y_(y) { }

  template<typename T>
  bool operator()(const T *homography_parameters, T *residuals) const {
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, 3, 1> Vec3;

    Mat3 H(homography_parameters);

    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));

    Vec3 H_x = H * x;
    Vec3 Hinv_y = H.inverse() * y;

    H_x /= H_x(2);
    Hinv_y /= Hinv_y(2);

    residuals[0] = H_x(0) - T(y_(0));
    residuals[1] = H_x(1) - T(y_(1));

    residuals[2] = Hinv_y(0) - T(x_(0));
    residuals[3] = Hinv_y(1) - T(x_(1));

    return true;
  }

  const Vec2 x_;
  const Vec2 y_;
};

void ComputeHomographyFromCorrespondences(const Mat &x1, const Mat &x2,
                                          CameraIntrinsics &intrinsics,
                                          Mat3 *H) {
  // Algebraic homography estimation, happens with normalized coordinates
  Homography2DFromCorrespondencesLinear(x1, x2, H, 1e-12);

  // Refine matrix using Ceres minimizer

  // TODO(sergey): look into refinement in pixel space.
  ceres::Problem problem;

  for (int i = 0; i < x1.cols(); i++) {
    HomographySymmetricGeometricCostFunctor
        *homography_symmetric_geometric_cost_function =
            new HomographySymmetricGeometricCostFunctor(x1.col(i),
                                                        x2.col(i));

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<
            HomographySymmetricGeometricCostFunctor,
            4, /* num_residuals */
            9>(homography_symmetric_geometric_cost_function),
        NULL,
        H->data());
  }

  // Configure the solve.
  ceres::Solver::Options solver_options;
  solver_options.linear_solver_type = ceres::DENSE_QR;
  solver_options.max_num_iterations = 50;
  solver_options.update_state_every_iteration = true;
  solver_options.parameter_tolerance = 1e-16;
  solver_options.function_tolerance = 1e-16;

  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);

  VLOG(1) << "Summary:\n" << summary.FullReport();

  // Convert homography to original pixel space
  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  *H = N.inverse() * (*H) * N;
}

class FundamentalSymmetricEpipolarCostFunctor {
 public:
  FundamentalSymmetricEpipolarCostFunctor(const Vec2 &x,
                                          const Vec2 &y)
    : x_(x), y_(y) {}

  template<typename T>
  bool operator()(const T *fundamental_parameters, T *residuals) const {
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, 3, 1> Vec3;

    Mat3 F(fundamental_parameters);

    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));

    Vec3 F_x = F * x;
    Vec3 Ft_y = F.transpose() * y;
    T y_F_x = y.dot(F_x);

    residuals[0] = y_F_x * T(1) / F_x.head(2).norm();
    residuals[1] = y_F_x * T(1) / Ft_y.head(2).norm();

    return true;
  }

  const Mat x_;
  const Mat y_;
};

void ComputeFundamentalFromCorrespondences(const Mat &x1, const Mat &x2,
                                           CameraIntrinsics &intrinsics,
                                           Mat3 *F) {
  // Algebraic fundamental estimation, happens with normalized coordinates
  NormalizedEightPointSolver(x1, x2, F);

  // Refine matrix using Ceres minimizer

  // TODO(sergey): look into refinement in pixel space.
  ceres::Problem problem;

  for (int i = 0; i < x1.cols(); i++) {
    FundamentalSymmetricEpipolarCostFunctor
        *fundamental_symmetric_epipolar_cost_function =
            new FundamentalSymmetricEpipolarCostFunctor(x1.col(i),
                                                        x2.col(i));

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<
            FundamentalSymmetricEpipolarCostFunctor,
            2, /* num_residuals */
            9>(fundamental_symmetric_epipolar_cost_function),
        NULL,
        F->data());
  }

  // Configure the solve.
  ceres::Solver::Options solver_options;
  solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  solver_options.max_num_iterations = 50;
  solver_options.update_state_every_iteration = true;
  solver_options.parameter_tolerance = 1e-16;
  solver_options.function_tolerance = 1e-16;

  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);

  VLOG(1) << "Summary:\n" << summary.FullReport();

  // Convert fundamental to original pixel space
  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  *F = N.inverse() * (*F) * N;
}

// P.H.S. Torr
// Geometric Motion Segmentation and Model Selection
//
// http://reference.kfupm.edu.sa/content/g/e/geometric_motion_segmentation_and_model__126445.pdf
//
// d is the number of dimensions modeled
//     (d = 3 for a fundamental matrix or 2 for a homography)
// k is the number of degrees of freedom in the model
//     (k = 7 for a fundamental matrix or 8 for a homography)
// r is the dimension of the data
//     (r = 4 for 2D correspondences between two frames)
double GRIC(const Vec &e, int d, int k, int r) {
  int n = e.rows();
  double lambda1 = log(static_cast<double>(r));
  double lambda2 = log(static_cast<double>(r * n));

  // lambda3 limits the residual error, and this paper
  // http://elvera.nue.tu-berlin.de/files/0990Knorr2006.pdf
  // suggests using lambda3 of 2
  // same value is used in Torr's Problem of degeneracy in structure
  // and motion recovery from uncalibrated image sequences
  // http://www.robots.ox.ac.uk/~vgg/publications/papers/torr99.ps.gz
  double lambda3 = 2.0;

  // measurement error of tracker
  double sigma2 = 0.01;

  // Actual GRIC computation
  double gric_result = 0.0;

  for (int i = 0; i < n; i++) {
    double rho = std::min(e(i) * e(i) / sigma2, lambda3 * (r - d));
    gric_result += rho;
  }

  gric_result += lambda1 * d * n;
  gric_result += lambda2 * k;

  return gric_result;
}

// Compute a generalized inverse using eigen value decomposition.
// It'll actually also zero 7 last eigen values to deal with
// gauges, since this function is used to compute variance of
// reconstructed 3D points.
//
// TODO(sergey): Could be generalized by making it so number
//               of values to be zeroed is passed by an argument
//               and moved to numeric module.
Mat pseudoInverse(const Mat &matrix) {
  Eigen::EigenSolver<Mat> eigenSolver(matrix);
  Mat D = eigenSolver.pseudoEigenvalueMatrix();
  Mat V = eigenSolver.pseudoEigenvectors();

  double epsilon = std::numeric_limits<double>::epsilon();

  for (int i = 0; i < D.cols(); ++i) {
    if (D(i, i) > epsilon)
      D(i, i) = 1.0 / D(i, i);
    else
      D(i, i) = 0.0;
  }

  // Zero last 7 (which corresponds to smallest eigen values).
  // 7 equals to the number of gauge freedoms.
  for (int i = D.cols() - 7; i < D.cols(); ++i)
    D(i, i) = 0.0;

  return V * D * V.inverse();
}
}  // namespace

void SelectkeyframesBasedOnGRICAndVariance(const Tracks &tracks,
                                           CameraIntrinsics &intrinsics,
                                           vector<int> &keyframes) {
  // Mirza Tahir Ahmed, Matthew N. Dailey
  // Robust key frame extraction for 3D reconstruction from video streams
  //
  // http://www.cs.ait.ac.th/~mdailey/papers/Tahir-KeyFrame.pdf

  int max_image = tracks.MaxImage();
  int next_keyframe = 1;
  int number_keyframes = 0;

  // Limit correspondence ratio from both sides.
  // On the one hand if number of correspondent features is too low,
  // triangulation will suffer.
  // On the other hand high correspondence likely means short baseline.
  // which also will affect om accuracy
  const double Tmin = 0.8;
  const double Tmax = 1.0;

  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  Mat3 N_inverse = N.inverse();

  double Sc_best = std::numeric_limits<double>::max();
  double success_intersects_factor_best = 0.0f;

  while (next_keyframe != -1) {
    int current_keyframe = next_keyframe;
    double Sc_best_candidate = std::numeric_limits<double>::max();

    LG << "Found keyframe " << next_keyframe;

    number_keyframes++;
    next_keyframe = -1;

    for (int candidate_image = current_keyframe + 1;
         candidate_image <= max_image;
         candidate_image++) {
      // Conjunction of all markers from both keyframes
      vector<Marker> all_markers =
        tracks.MarkersInBothImages(current_keyframe, candidate_image);

      // Match keypoints between frames current_keyframe and candidate_image
      vector<Marker> tracked_markers =
        tracks.MarkersForTracksInBothImages(current_keyframe, candidate_image);

      // Correspondences in normalized space
      Mat x1, x2;
      CoordinatesForMarkersInImage(tracked_markers, current_keyframe, &x1);
      CoordinatesForMarkersInImage(tracked_markers, candidate_image, &x2);

      LG << "Found " << x1.cols()
         << " correspondences between " << current_keyframe
         << " and " << candidate_image;

      // Not enough points to construct fundamental matrix
      if (x1.cols() < 8 || x2.cols() < 8)
        continue;

      // STEP 1: Correspondence ratio constraint
      int Tc = tracked_markers.size();
      int Tf = all_markers.size();
      double Rc = static_cast<double>(Tc) / Tf;

      LG << "Correspondence between " << current_keyframe
         << " and " << candidate_image
         << ": " << Rc;

      if (Rc < Tmin || Rc > Tmax)
        continue;

      Mat3 H, F;
      ComputeHomographyFromCorrespondences(x1, x2, intrinsics, &H);
      ComputeFundamentalFromCorrespondences(x1, x2, intrinsics, &F);

      // TODO(sergey): STEP 2: Discard outlier matches

      // STEP 3: Geometric Robust Information Criteria

      // Compute error values for homography and fundamental matrices
      Vec H_e, F_e;
      H_e.resize(x1.cols());
      F_e.resize(x1.cols());
      for (int i = 0; i < x1.cols(); i++) {
        Vec2 current_x1 =
          NorrmalizedToPixelSpace(Vec2(x1(0, i), x1(1, i)), intrinsics);
        Vec2 current_x2 =
          NorrmalizedToPixelSpace(Vec2(x2(0, i), x2(1, i)), intrinsics);

        H_e(i) = SymmetricGeometricDistance(H, current_x1, current_x2);
        F_e(i) = SymmetricEpipolarDistance(F, current_x1, current_x2);
      }

      LG << "H_e: " << H_e.transpose();
      LG << "F_e: " << F_e.transpose();

      // Degeneracy constraint
      double GRIC_H = GRIC(H_e, 2, 8, 4);
      double GRIC_F = GRIC(F_e, 3, 7, 4);

      LG << "GRIC values for frames " << current_keyframe
         << " and " << candidate_image
         << ", H-GRIC: " << GRIC_H
         << ", F-GRIC: " << GRIC_F;

      if (GRIC_H <= GRIC_F)
        continue;

      // TODO(sergey): STEP 4: PELC criterion

      // STEP 5: Estimation of reconstruction error
      //
      // Uses paper Keyframe Selection for Camera Motion and Structure
      // Estimation from Multiple Views
      // Uses ftp://ftp.tnt.uni-hannover.de/pub/papers/2004/ECCV2004-TTHBAW.pdf
      // Basically, equation (15)
      //
      // TODO(sergey): separate all the constraints into functions,
      //               this one is getting to much cluttered already

      // Definitions in equation (15):
      // - I is the number of 3D feature points
      // - A is the number of essential parameters of one camera

      EuclideanReconstruction reconstruction;

      // The F matrix should be an E matrix, but squash it just to be sure

      // Reconstruction should happen using normalized fundamental matrix
      Mat3 F_normal = N * F * N_inverse;

      Mat3 E;
      FundamentalToEssential(F_normal, &E);

      // Recover motion between the two images. Since this function assumes a
      // calibrated camera, use the identity for K
      Mat3 R;
      Vec3 t;
      Mat3 K = Mat3::Identity();

      if (!MotionFromEssentialAndCorrespondence(E,
                                                K, x1.col(0),
                                                K, x2.col(0),
                                                &R, &t)) {
        LG << "Failed to compute R and t from E and K";
        continue;
      }

      LG << "Camera transform between frames " << current_keyframe
         << " and " << candidate_image
         << ":\nR:\n" << R
         << "\nt:" << t.transpose();

      // First camera is identity, second one is relative to it
      reconstruction.InsertCamera(current_keyframe,
                                  Mat3::Identity(),
                                  Vec3::Zero());
      reconstruction.InsertCamera(candidate_image, R, t);

      // Reconstruct 3D points
      int intersects_total = 0, intersects_success = 0;
      for (int i = 0; i < tracked_markers.size(); i++) {
        if (!reconstruction.PointForTrack(tracked_markers[i].track)) {
          vector<Marker> reconstructed_markers;

          int track = tracked_markers[i].track;

          reconstructed_markers.push_back(tracked_markers[i]);

          // We know there're always only two markers for a track
          // Also, we're using brute-force search because we don't
          // actually know about markers layout in a list, but
          // at this moment this cycle will run just once, which
          // is not so big deal

          for (int j = i + 1; j < tracked_markers.size(); j++) {
            if (tracked_markers[j].track == track) {
              reconstructed_markers.push_back(tracked_markers[j]);
              break;
            }
          }

          intersects_total++;

          if (EuclideanIntersect(reconstructed_markers, &reconstruction)) {
            LG << "Ran Intersect() for track " << track;
            intersects_success++;
          } else {
            LG << "Filed to intersect track " << track;
          }
        }
      }

      double success_intersects_factor =
          (double) intersects_success / intersects_total;

      if (success_intersects_factor < success_intersects_factor_best) {
        LG << "Skip keyframe candidate because of "
              "lower successful intersections ratio";

        continue;
      }

      success_intersects_factor_best = success_intersects_factor;

      Tracks two_frames_tracks(tracked_markers);
      CameraIntrinsics empty_intrinsics;
      BundleEvaluation evaluation;
      evaluation.evaluate_jacobian = true;

      EuclideanBundleCommonIntrinsics(two_frames_tracks,
                                      BUNDLE_NO_INTRINSICS,
                                      BUNDLE_NO_CONSTRAINTS,
                                      &reconstruction,
                                      &empty_intrinsics,
                                      &evaluation);

      Mat &jacobian = evaluation.jacobian;

      Mat JT_J = jacobian.transpose() * jacobian;
      Mat JT_J_inv = pseudoInverse(JT_J);

      Mat temp_derived = JT_J * JT_J_inv * JT_J;
      bool is_inversed = (temp_derived - JT_J).cwiseAbs2().sum() <
          1e-4 * std::min(temp_derived.cwiseAbs2().sum(),
                          JT_J.cwiseAbs2().sum());

      LG << "Check on inversed: " << (is_inversed ? "true" : "false" )
         << ", det(JT_J): " << JT_J.determinant();

      if (!is_inversed) {
        LG << "Ignoring candidature due to poor jacobian stability";
        continue;
      }

      Mat Sigma_P;
      Sigma_P = JT_J_inv.bottomRightCorner(evaluation.num_points * 3,
                                           evaluation.num_points * 3);

      int I = evaluation.num_points;
      int A = 12;

      double Sc = static_cast<double>(I + A) / Square(3 * I) * Sigma_P.trace();

      LG << "Expected estimation error between "
         << current_keyframe << " and "
         << candidate_image << ": " << Sc;

      // Pairing with a lower Sc indicates a better choice
      if (Sc > Sc_best_candidate)
        continue;

      Sc_best_candidate = Sc;

      next_keyframe = candidate_image;
    }

    // This is a bit arbitrary and main reason of having this is to deal
    // better with situations when there's no keyframes were found for
    // current keyframe this could happen when there's no so much parallax
    // in the beginning of image sequence and then most of features are
    // getting occluded. In this case there could be good keyframe pair in
    // the middle of the sequence
    //
    // However, it's just quick hack and smarter way to do this would be nice
    if (next_keyframe == -1) {
      next_keyframe = current_keyframe + 10;
      number_keyframes = 0;

      if (next_keyframe >= max_image)
        break;

      LG << "Starting searching for keyframes starting from " << next_keyframe;
    } else {
      // New pair's expected reconstruction error is lower
      // than existing pair's one.
      //
      // For now let's store just one candidate, easy to
      // store more candidates but needs some thoughts
      // how to choose best one automatically from them
      // (or allow user to choose pair manually).
      if (Sc_best > Sc_best_candidate) {
        keyframes.clear();
        keyframes.push_back(current_keyframe);
        keyframes.push_back(next_keyframe);
        Sc_best = Sc_best_candidate;
      }
    }
  }
}

}  // namespace libmv


/*
==============================================================================
KratosStructuralApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
janosch.stascheit@rub.de
nagel@sd.rub.de
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain
- Ruhr-University Bochum, Institute for Structural Mechanics, Germany

Permission is hereby granted, free  of charge, to any person obtaining
a  copy  of this  software  and  associated  documentation files  (the
"Software"), to  deal in  the Software without  restriction, including
without limitation  the rights to  use, copy, modify,  merge, publish,
distribute,  sublicense and/or  sell copies  of the  Software,  and to
permit persons to whom the Software  is furnished to do so, subject to
the following condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

The  above  copyright  notice  and  this permission  notice  shall  be
included in all copies or substantial portions of the Software.

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/
/* **************************************************************************************
*
*   Last Modified by:    $Author: G.D.Huynh $
*   Date:                $Date: 25.042018 $
*   Revision:            $Revision: 1.0 $
*
* ***************************************************************************************/

// System includes
// External includes
#include <boost/timer.hpp>
// Project includes
#include "includes/define.h"
#include "custom_elements/nonlinear_bending_strip.h"
#include "utilities/math_utils.h"
#include "utilities/openmp_utils.h"
#include "structural_application/custom_utilities/sd_math_utils.h"
#include "isogeometric_application/custom_utilities/isogeometric_math_utils.h"
#include "isogeometric_application/isogeometric_application.h"

#define ENABLE_PROFILING

#include <iostream>
using namespace std;

namespace Kratos
{
//***********************************************************************************
//***********************************************************************************
// -------- //
//  PUBLIC  //
// -------- //

// Constructor
NonLinearBendingStrip::NonLinearBendingStrip()
{
}

// Constructor
NonLinearBendingStrip::NonLinearBendingStrip( IndexType NewId, GeometryType::Pointer pGeometry )
    : Element( NewId, pGeometry )
{
    mpIsogeometricGeometry =
        boost::dynamic_pointer_cast<IsogeometricGeometryType>(pGetGeometry());
}

// Constructor
NonLinearBendingStrip::NonLinearBendingStrip( IndexType NewId, GeometryType::Pointer pGeometry,
                          PropertiesType::Pointer pProperties )
    : Element( NewId, pGeometry, pProperties )
{
    mpIsogeometricGeometry =
        boost::dynamic_pointer_cast<IsogeometricGeometryType>(pGetGeometry());
}

//***********************************************************************************
//***********************************************************************************
Element::Pointer NonLinearBendingStrip::Create( IndexType NewId,
                                        NodesArrayType const& ThisNodes,
                                        PropertiesType::Pointer pProperties ) const
{
    return Element::Pointer( new NonLinearBendingStrip( NewId, GetGeometry().Create( ThisNodes ), pProperties ) );
}

//***********************************************************************************
//***********************************************************************************
Element::Pointer NonLinearBendingStrip::Create( IndexType NewId,
    GeometryType::Pointer pGeom,
    PropertiesType::Pointer pProperties ) const
{
return Element::Pointer( new NonLinearBendingStrip( NewId, pGeom, pProperties ) );
}

//***********************************************************************************
//***********************************************************************************
// Destructor
NonLinearBendingStrip::~NonLinearBendingStrip()
{
}

//***********************************************************************************
//***********************************************************************************
void NonLinearBendingStrip::EquationIdVector( EquationIdVectorType& rResult,
                                    const ProcessInfo& rCurrentProcessInfo ) const
{
    KRATOS_TRY
    unsigned int number_of_nodes = GetGeometry().size();
    unsigned int dim = number_of_nodes * 3;

    if ( rResult.size() != dim )
        rResult.resize( dim );

    for ( unsigned int i = 0; i < number_of_nodes; i++ )
    {
        int index = i * 3;
        rResult[index]   = GetGeometry()[i].GetDof( DISPLACEMENT_X ).EquationId();
        rResult[index+1] = GetGeometry()[i].GetDof( DISPLACEMENT_Y ).EquationId();
        rResult[index+2] = GetGeometry()[i].GetDof( DISPLACEMENT_Z ).EquationId();
    }

    //KRATOS_WATCH(rResult)

    KRATOS_CATCH( "" )
}

//***********************************************************************************
//***********************************************************************************
void NonLinearBendingStrip::GetDofList( DofsVectorType& ElementalDofList,
                              const ProcessInfo& rCurrentProcessInfo ) const
{
    ElementalDofList.resize( 0 );

    for ( unsigned int i = 0; i < GetGeometry().size(); i++ )
    {
        ElementalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_X ) );
        ElementalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_Y ) );
        ElementalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_Z ) );
    }
}

//***********************************************************************************
//***********************************************************************************
void NonLinearBendingStrip::Initialize(const ProcessInfo& rCurrentProcessInfo)
{
    KRATOS_TRY

        mThisIntegrationMethod = GeometryData::GI_GAUSS_1;

        mDim = 3;
        mNumberOfNodes = mpIsogeometricGeometry->size();
        mStrainSize = 3;
        mNumberOfDof = mNumberOfNodes*mDim;
        mThickness = GetProperties()[THICKNESS];

        // material parameters
        mE = GetProperties()[YOUNG_MODULUS];
        mNU = GetProperties()[POISSON_RATIO];
        mLambda = mE*mNU /(1.0 +mNU)/(1.0 - 2.0*mNU);
        mMu = 0.5*mE/(1.0 + mNU);

        ////////////////////////////////////////////////////////////////
        // get nodal coordinates vector R in undeformed configuration //
        if(mNodalCoordinates.size() != mNumberOfNodes)
            mNodalCoordinates.resize(mNumberOfNodes);

        for(unsigned int I=0; I< mNumberOfNodes; ++I)
            mNodalCoordinates[I].resize(mDim);

        for (unsigned int I=0; I< mNumberOfNodes; ++I)
        {
            mNodalCoordinates[I](0) = (*mpIsogeometricGeometry)[I].X0();
            mNodalCoordinates[I](1) = (*mpIsogeometricGeometry)[I].Y0();
            mNodalCoordinates[I](2) = (*mpIsogeometricGeometry)[I].Z0();
        }

        #ifdef ENABLE_BEZIER_GEOMETRY
        //initialize the geometry
        mpIsogeometricGeometry->Initialize(mThisIntegrationMethod);
        #endif

        //Initialization of the constitutive law vector and
        // declaration, definition and initialization of the material
        // law was at each integration point
        const GeometryType::IntegrationPointsArrayType& integration_points =
        mpIsogeometricGeometry->IntegrationPoints(mThisIntegrationMethod);

        mNumberOfIntegrationPoint = integration_points.size();

        //////////////////////////////////////////////////////////////
        ///// compute  Jacobian ,Inverse Jacobian, Det Jacobian /////
        mInvJ0.resize(integration_points.size());
        mN.resize(integration_points.size());
        mDN_De.resize(integration_points.size());

        for (unsigned int i = 0; i < integration_points.size(); ++i)
        {
            mInvJ0[i].resize(mDim, mDim, false);
            noalias(mInvJ0[i]) = ZeroMatrix(mDim, mDim);

            mN[i].resize(mNumberOfNodes);
            noalias(mN[i]) = ZeroVector(mNumberOfNodes);

            mDN_De[i].resize(mNumberOfNodes, 2);
            noalias(mDN_De[i]) = ZeroMatrix(mNumberOfNodes,2);

        }

        mDetJ0.resize(integration_points.size(), false);
        // TODO remove the storage for Jacobian to save memory
        noalias(mDetJ0) = ZeroVector(integration_points.size());

        mIntegrationWeight.resize(integration_points.size());

        // calculate the Jacobian
        mJ0.resize(integration_points.size());
        mJ0 = mpIsogeometricGeometry->Jacobian0(mJ0, mThisIntegrationMethod);
        double DetJ_temp;
        mTotalDomainInitialSize = 0.0;

        for(unsigned int PointNumber = 0; PointNumber < integration_points.size(); ++ PointNumber)
        {
            mN[PointNumber] = mpIsogeometricGeometry->ShapeFunctionsValues( mN[PointNumber] , integration_points[PointNumber]);
            mDN_De[PointNumber] = mpIsogeometricGeometry->ShapeFunctionsLocalGradients( mDN_De[PointNumber], integration_points[PointNumber]);

            MathUtils<double>::InvertMatrix(mJ0[PointNumber], mInvJ0[PointNumber],  DetJ_temp);

            Matrix JtJ = prod(trans(mJ0[PointNumber]), mJ0[PointNumber]);
            mDetJ0[PointNumber] = sqrt(MathUtils<double>::Det(JtJ));

            //getting informations for integration
            mIntegrationWeight[PointNumber] = integration_points[PointNumber].Weight();

            mTotalDomainInitialSize += mDetJ0[PointNumber]* mIntegrationWeight[PointNumber];

        }

        mIsInitialized = true;

        #ifdef ENABLE_BEZIER_GEOMETRY
        // clean the geometry internal data
        mpIsogeometricGeometry->Clean();
        #endif

    KRATOS_CATCH("")
}

//***********************************************************************************
//***********************************************************************************
void NonLinearBendingStrip::CalculateLocalSystem( MatrixType& rLeftHandSideMatrix,
                                                VectorType& rRightHandSideVector,
                                                const ProcessInfo& rCurrentProcessInfo )
{
    //calculation flags
    bool CalculateStiffnessMatrixFlag = true;
    bool CalculateResidualVectorFlag = true;

    CalculateAll( rLeftHandSideMatrix, rRightHandSideVector, rCurrentProcessInfo,
                  CalculateStiffnessMatrixFlag, CalculateResidualVectorFlag );
}

//***********************************************************************************
//***********************************************************************************
void NonLinearBendingStrip::CalculateAll( MatrixType& rLeftHandSideMatrix,
                                VectorType& rRightHandSideVector,
                                const ProcessInfo& rCurrentProcessInfo,
                                bool CalculateStiffnessMatrixFlag,
                                bool CalculateResidualVectorFlag )
{
    KRATOS_TRY

    if (CalculateStiffnessMatrixFlag==true)
    {
        if (rLeftHandSideMatrix.size1() != mNumberOfDof)
            rLeftHandSideMatrix.resize(mNumberOfDof, mNumberOfDof);
        noalias(rLeftHandSideMatrix) = ZeroMatrix(mNumberOfDof, mNumberOfDof);
    }

    if (CalculateResidualVectorFlag==true)
    {
        if (rRightHandSideVector.size() != mNumberOfDof)
            rRightHandSideVector.resize(mNumberOfDof);
        noalias(rRightHandSideVector) = ZeroVector(mNumberOfDof);
    }

    ////////////////////////////////////////////////////////////////////////////////
    ////// compute residual vector and stiffness matrix over integration points/////

    #ifdef ENABLE_BEZIER_GEOMETRY
    //initialize the geometry
    mpIsogeometricGeometry->Initialize(mThisIntegrationMethod);
    #endif

    // get integration points
    const GeometryType::IntegrationPointsArrayType& integration_points =
        mpIsogeometricGeometry->IntegrationPoints(mThisIntegrationMethod);

    // Current displacements
    Matrix CurrentDisplacement(mNumberOfNodes, mDim);
    for(unsigned int node =0; node < mpIsogeometricGeometry->size() ;++node)
        noalias(row(CurrentDisplacement, node)) = (*mpIsogeometricGeometry)[node].GetSolutionStepValue(DISPLACEMENT);

    ///////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////// loop over integration points
    for(unsigned int PointNumber=0; PointNumber < integration_points.size(); ++PointNumber)
    {

            //////////// get shape function values and their derivatives
            ShapeFunctionsSecondDerivativesType D2N_De2;
            D2N_De2 = mpIsogeometricGeometry->ShapeFunctionsSecondDerivatives(D2N_De2, integration_points[PointNumber]);

            /////// i. covariant base vectors
            std::vector<Vector> u_a;
            std::vector<Vector> a;
            std::vector<Vector> A;  // covarianti base vector of undeformed configuration
            ReferenceCovariantBaseVector(A, mDN_De[PointNumber], mNodalCoordinates);
            FirstDerivativeDisplacement_a( u_a, mDN_De[PointNumber] , CurrentDisplacement);
            DeformedCovariantBaseVector( a, A, u_a );

            // normal directors
            Vector a3Vector, A3Vector, aa3Vector, AA3Vector; double a3, A3;
            NormalDirector(a3Vector,  aa3Vector, a3, a);
            NormalDirector(A3Vector, AA3Vector, A3, A);

            ////ii. derivative of covariant base vectors
            std::vector<std::vector<Vector> > u_ab;
            std::vector<std::vector<Vector> > a_ab;
            std::vector<std::vector<Vector> > A_ab;
            DerivativeReferenceCovariantBaseVector(A_ab, D2N_De2, mNodalCoordinates);
            SecondDerivativeDisplacement_ab( u_ab,  D2N_De2 , CurrentDisplacement);
            DerivativeDeformedCovariantBaseVector( a_ab , A_ab,  u_ab);

            std::vector<Vector> UnitBasisVector;
            this->UnitBaseVectors(UnitBasisVector);

            // a_ar
            std::vector< std::vector<std::vector<Vector>> > a_ar;
            DerivativeCovariantBaseVector_r( a_ar, mDN_De[PointNumber], UnitBasisVector);

            // aa3_rVector
            std::vector<std::vector<Vector>> aa3_rVector;
            DerivativeNonNormalizedDirector_r( aa3_rVector , a_ar, a);

            // a3_r
            std::vector<std::vector<double>> a3_r;
            DerivativeDirectorNorm_r(a3_r , aa3_rVector, a3Vector);

            // a3_rVector
            std::vector<std::vector<Vector>> a3_rVector;
            DerivativeDirector_r(a3_rVector ,  aa3_rVector, a3_r, a3Vector, a3);

            // a_abr
            std::vector< std::vector<std::vector<std::vector<Vector>>> > a_abr;
            DerivativeCovariantBaseVector_abr( a_abr, D2N_De2, UnitBasisVector);

            /*// aa3_rsVector
            std::vector<std::vector<std::vector<std::vector<Vector>>> > aa3_rsVector;
            SecondDerivativeNonNormalizedDirector_rs( aa3_rsVector , a_ar);

            // a3_rs
            std::vector<std::vector<std::vector<std::vector<double>>> > a3_rs;
            SecondDerivativeDirectorNorm_rs(a3_rs , aa3_rsVector,  aa3Vector, a3Vector,  a3, aa3_rVector);

            // a3_rsVector
            std::vector<std::vector<std::vector<std::vector<Vector>>> > a3_rsVector;
            SecondDerivativeDirector_rs( a3_rsVector,  aa3_rsVector, a3_rs, a3Vector, a3,  aa3_rVector, a3_r);
            */

            // metric and curvature coefficents
            Matrix Aab(2,2);
            Matrix aab(2,2);
            Matrix Bab(2,2);
            Matrix bab(2,2);
            CovariantMetricCoefficient(Aab, A);
            CovariantMetricCoefficient(aab, a);
            CovariantCurvatureCoefficient(Bab, A_ab, A3Vector);
            CovariantCurvatureCoefficient(bab, a_ab, a3Vector);

            double detJA = sqrt(MathUtils<double>::Det(Aab));
            // contravariant base vectors
            std::vector<Vector> AA;
            ContravariantBaseVector( AA, A, Aab);

            // local Cartesian basis
            std::vector<Vector> EE;
            LocalCartesianBasisVector(EE,  A, A3Vector);

            // transformation coeff
            std::vector< std::vector<std::vector<std::vector<double>>> > TransformationCoeff;
            LocalTransformationCoefficient(TransformationCoeff, EE, AA);

            // membrane and bending strains
            Matrix kTensor;
            computeCurvatureChange(kTensor,  Bab, bab);

            // transform strains to local form
            Matrix kkTensor = ZeroMatrix(2,2);
            Vector kkVector(3);
            LocalTransformationOfTensor(kkTensor , kTensor, TransformationCoeff);
            kkVector = SD_MathUtils<int>::TensorToStrainVector( kkTensor);

            // material matrix
            Matrix D0 = ZeroMatrix(mStrainSize,mStrainSize);
            CalculateConstitutiveMatrix(D0);

            Vector moVector= ZeroVector(mStrainSize);
            noalias(moVector) = pow(mThickness,3.0)/12.0*prod(D0,kkVector) ;

            // first derivative of bending strains w.r.t displacement
            std::vector<std::vector<Vector> > kLocalVector_r;
            FirstDerivativeLocalCurvatureChange_r( kLocalVector_r,  a_abr, a_ab, a3Vector , a3_rVector, TransformationCoeff);

            // bending bending B matrix
            Matrix BBb(mStrainSize, mNumberOfDof);
            CreatingBmatrix( BBb, kLocalVector_r);

            ///////////////////////////////////////////////
            ///// nonlinear part of stiffness matrix //////
            ///////////////////////////////////////////////

            //std::vector<std::vector<Matrix>> kLocalVector_rs;
            //SecondDerivativeLocalCurvatureChange_rs(kLocalVector_rs , a_abr, a3_rVector,  a_ab, a3_rsVector, TransformationCoeff);

            if(CalculateStiffnessMatrixFlag == true)
            {
                AddLinearStiffnessMatrix(rLeftHandSideMatrix, D0, BBb, BBb, pow(mThickness,3.0)/12.0*detJA , mIntegrationWeight[PointNumber]);
                //AddNonlinearStiffnessMatrix(rLeftHandSideMatrix,moVector, kLocalVector_rs,  detJA , mIntegrationWeight[PointNumber]);
            }

            if(CalculateResidualVectorFlag == true)
            {
                AddInternalForces(rRightHandSideVector , moVector,kLocalVector_r, detJA , mIntegrationWeight[PointNumber]);

            }

    }// loop over integration points

    //KRATOS_WATCH(rRightHandSideVector)

    #ifdef ENABLE_BEZIER_GEOMETRY
    // clean the geometry internal data
    mpIsogeometricGeometry->Clean();
    #endif

    KRATOS_CATCH( "" )
}

    ///////////////////////////////////////// all components of residual vectors and stiffness matrices ///////////
    ///////////////////// add left hand side contribution
    void NonLinearBendingStrip::AddInternalForces(VectorType& RightHandSideVector, const Vector& StressResultants,
        std::vector<std::vector<Vector> >& StrainVector_r, const double& DetJ, const double& Weight)
    {
       noalias(RightHandSideVector) -= StressResultants(0)*StrainVector_r[0][0]*DetJ*Weight;
       noalias(RightHandSideVector) -= StressResultants(1)*StrainVector_r[1][1]*DetJ*Weight;
       noalias(RightHandSideVector) -= StressResultants(2)*StrainVector_r[0][1]*DetJ*Weight;
    }

    void NonLinearBendingStrip::AddLinearStiffnessMatrix(MatrixType& LeftHandSideMatrix, const Matrix& Di, const Matrix& BlhsMatrix
        , const Matrix& BrhsMatrix, const double& DetJ, const double& Weight)
    {
        noalias(LeftHandSideMatrix) += prod( trans(BlhsMatrix), Matrix(prod(Di, BrhsMatrix)) )*DetJ*Weight;
    }

    void NonLinearBendingStrip::AddNonlinearStiffnessMatrix( MatrixType& LeftHandSideMatrix,const Vector& StressResultants
        , const std::vector<std::vector<Matrix>>& StrainVector_rs, const double& DetJ, const double& Weight)
    {
        noalias(LeftHandSideMatrix) += StressResultants(0)*StrainVector_rs[0][0]*DetJ*Weight;
        noalias(LeftHandSideMatrix) += StressResultants(1)*StrainVector_rs[1][1]*DetJ*Weight;
        noalias(LeftHandSideMatrix) += StressResultants(2)*StrainVector_rs[0][1]*DetJ*Weight;
    }
    ////////////////////////////////////////////////////////
    ///////////////////// strain tensors ///////////////////
    ////////////////////////////////////////////////////////

    void NonLinearBendingStrip::computeCurvatureChange(Matrix& kTensor, Matrix& Bab, Matrix& bab)
    {
        kTensor.resize(2,2);
        kTensor = ZeroMatrix(2,2);

        noalias(kTensor) = Bab - bab;

    }

    /////////////////////////////////////////////////////////////////////////
    /////////////////////////// base vectors and their derivatives///////////
    /////////////////////////////////////////////////////////////////////////
    void NonLinearBendingStrip::DeformedCovariantBaseVector(std::vector<Vector>& a, std::vector<Vector>& A
                    , std::vector<Vector>& u_a )
    {

        a.resize(2);
        for(unsigned int alpha=0; alpha< 2; alpha++)
        {
            a[alpha].resize(mDim);
            a[alpha] = ZeroVector(mDim);
        }

        for(unsigned int alpha=0; alpha < 2; alpha++)
        {
            noalias(a[alpha]) = A[alpha] + u_a[alpha];
        }
    }

    void NonLinearBendingStrip::ReferenceCovariantBaseVector(std::vector<Vector>& A, const Matrix& DN_De
                                                                            , const std::vector<Vector>& X)
    {

        // resize A
        A.resize(2);
        for(unsigned int alpha=0; alpha< 2; alpha++)
        {
            A[alpha].resize(mDim);
            A[alpha] = ZeroVector(mDim);
        }

        for(unsigned alpha=0; alpha < 2; alpha++)
        {
            for(unsigned int i=0; i<mDim; i++)
            {
                for(unsigned int I=0; I<mNumberOfNodes; I++)
                {
                    // compute a1, a2
                    A[alpha](i) += DN_De(I,alpha)*X[I](i) ;
                }
            }
        }
    }

    void NonLinearBendingStrip::FirstDerivativeDisplacement_a(std::vector<Vector>& u_a, const Matrix& DN_De , const Matrix& u)
    {
        u_a.resize(2);

        for(unsigned int i=0; i< 2; i++)
        {
            u_a[i].resize(mDim);
            u_a[i] = ZeroVector(mDim);
        }

        for(unsigned alpha=0; alpha < 2; alpha++)
        {
            for(unsigned int i=0; i<mDim; i++)
            {
                for(unsigned int I=0; I<mNumberOfNodes; I++)
                {
                    // compute a1, a2
                    u_a[alpha](i) += DN_De(I,alpha)*u(I,i) ;
                }
            }
        }
    }

    void NonLinearBendingStrip::SecondDerivativeDisplacement_ab(std::vector<std::vector<Vector> >& u_ab
        , const ShapeFunctionsSecondDerivativesType& D2N_De2 , const Matrix& u)
    {
        u_ab.resize(2);

        for(unsigned int alpha=0; alpha< 2; alpha++)
        {
            u_ab[alpha].resize(2);

            for(unsigned int beta=0; beta<2; beta++)
            {

                    u_ab[alpha][beta].resize(mDim);
                    u_ab[alpha][beta] = ZeroVector(mDim);

            }
        }

        for(unsigned alpha=0; alpha<2; alpha++)
        {
            for(unsigned beta=0; beta<2; beta++)
            {
                for(unsigned int i=0; i< mDim; ++i)
                {
                    for(unsigned int I=0; I< mNumberOfNodes; ++I)
                    {
                        // compute a_11
                        u_ab[alpha][beta](i) += D2N_De2[I](alpha,beta)*u(I,i);
                    }
                }
            }
        }
    }

    void NonLinearBendingStrip::NormalDirector(Vector& a3Vector, Vector& aa3Vector, double& a3,  std::vector<Vector>& a)
    {
        aa3Vector = MathUtils<double>::CrossProduct(a[0], a[1]);
        a3 = MathUtils<double>::Norm3(aa3Vector);
        a3Vector = aa3Vector/a3;
    }

    void NonLinearBendingStrip::ContravariantBaseVector(std::vector<Vector>& AA, std::vector<Vector>& A, Matrix& Aab)
    {
        AA.resize(2);
        for(unsigned alpha=0; alpha<2;alpha++)
        {
            AA[alpha].resize(3);
            AA[alpha] = ZeroVector(3);
        }

        double temp_ab;
        Matrix AAab(2,2);
        MathUtils<double>::InvertMatrix(Aab, AAab, temp_ab);

        noalias(AA[0]) = AAab(0,0)*A[0] +  AAab(0,1)*A[1] ;

        noalias(AA[1]) = AAab(1,0)*A[0] + AAab(1,1)*A[1] ;
    }

    void NonLinearBendingStrip::DerivativeReferenceCovariantBaseVector(std::vector<std::vector<Vector> >& A_ab,
        const ShapeFunctionsSecondDerivativesType& D2N_De2, const std::vector<Vector>& X)
    {
        A_ab.resize(2);

        for(unsigned int alpha=0; alpha< 2; alpha++)
        {
            A_ab[alpha].resize(2);

            for(unsigned int beta=0; beta<2; beta++)
            {

                    A_ab[alpha][beta].resize(mDim);
                    A_ab[alpha][beta] = ZeroVector(mDim);

            }
        }

        for(unsigned alpha=0; alpha<2; alpha++)
        {
            for(unsigned beta=0; beta<2; beta++)
            {
                for(unsigned int i=0; i< mDim; ++i)
                {
                    for(unsigned int I=0; I< mNumberOfNodes; ++I)
                    {
                        // compute a_11
                        A_ab[alpha][beta](i) += D2N_De2[I](alpha,beta)*X[I](i) ;
                    }
                }
            }
        }

    }

    void NonLinearBendingStrip::DerivativeDeformedCovariantBaseVector(std::vector<std::vector<Vector> >& a_ab
        , std::vector<std::vector<Vector> >& A_ab
        , std::vector<std::vector<Vector> >& u_ab)
    {
        a_ab.resize(2);

        for(unsigned int alpha=0; alpha< 2; alpha++)
        {
            a_ab[alpha].resize(2);

            for(unsigned int beta=0; beta<2; beta++)
            {
                a_ab[alpha][beta].resize(mDim);
                a_ab[alpha][beta] = ZeroVector(mDim);
            }
        }

        for(unsigned alpha=0; alpha<2; alpha++)
        {
            for(unsigned beta=0; beta<2; beta++)
            {
                noalias(a_ab[alpha][beta]) = A_ab[alpha][beta] + u_ab[alpha][beta];
            }
        }

    }

    ///////////////////////////////////////////////////////////////////
    ////////// shell fundamental properties ///////////////////////////
    ///////////////////////////////////////////////////////////////////

    void NonLinearBendingStrip::CovariantCurvatureCoefficient(Matrix& Bab
        , std::vector<std::vector<Vector> >& A_ab, Vector& A3Vector)
    {
        Bab.resize(2,2);
        Bab = ZeroMatrix(2,2);

        for(unsigned int alpha=0; alpha<2; alpha++)
        {
            for(unsigned int beta=0; beta<2; beta++)
            {
                Bab(alpha,beta)= MathUtils<double>::Dot3(A_ab[alpha][beta], A3Vector);
            }
        }
    }

    void NonLinearBendingStrip::CovariantMetricCoefficient(Matrix& Aab, std::vector<Vector>& A)
    {
        Aab.resize(2,2);
        Aab = ZeroMatrix(2,2);

        for(unsigned int alpha=0; alpha<2; alpha++)
        {
            for(unsigned int beta=0; beta<2; beta++)
            {
                Aab(alpha,beta)= MathUtils<double>::Dot3(A[alpha], A[beta]);
            }
        }
    }

    // second derivative of base vector w.r.t u_r
    void NonLinearBendingStrip::DerivativeCovariantBaseVector_abr( std::vector< std::vector<std::vector<std::vector<Vector>>> >& a_abr
        ,const ShapeFunctionsSecondDerivativesType& D2N_De2, std::vector<Vector>& UnitBasisVector)
    {
        a_abr.resize(2);
        for(unsigned int alpha = 0; alpha < 2; alpha++)
        {
            a_abr[alpha].resize(2);
            for(unsigned int beta = 0; beta < 2; beta++)
            {
                a_abr[alpha][beta].resize(mNumberOfNodes);
                for(unsigned int I = 0; I< mNumberOfNodes; I++)
                {
                    a_abr[alpha][beta][I].resize(mDim);
                    for(unsigned int i=0; i< mDim; i++)
                    {
                        a_abr[alpha][beta][I][i].resize(mDim);
                        a_abr[alpha][beta][I][i] = ZeroVector(mDim);
                    }
                }

            }
        }

        for(unsigned int alpha = 0; alpha < 2; alpha++)
        {
            for(unsigned int beta = 0; beta < 2; beta++)
            {
                for(unsigned int I=0; I < mNumberOfNodes; I++)
                {
                    for(unsigned int i=0; i<mDim; i++)
                    {
                        a_abr[alpha][beta][I][i] = D2N_De2[I](alpha,beta)*UnitBasisVector[i];
                    }
                }
            }
        }

    }

    void NonLinearBendingStrip::DerivativeCovariantBaseVector_r(std::vector< std::vector<std::vector<Vector>> >& a_ar, const Matrix& DN_De, std::vector<Vector>& UnitBasisVector)
    {
        a_ar.resize(2);
        for(unsigned int alpha = 0; alpha<2; alpha++)
        {
            a_ar[alpha].resize(mNumberOfNodes);
            for(unsigned int I=0; I< mNumberOfNodes; I++)
            {
                a_ar[alpha][I].resize(mDim);

                for(unsigned int i=0; i<mDim; i++)
                {
                    a_ar[alpha][I][i].resize(mDim);
                    a_ar[alpha][I][i] = ZeroVector(mDim);
                }
            }

        }

        for(unsigned int alpha = 0; alpha<2; alpha++)
        {
            for(unsigned int I=0; I < mNumberOfNodes; I++)
            {
                for(unsigned int i=0; i<mDim; i++)
                {
                    a_ar[alpha][I][i] = DN_De(I,alpha)*UnitBasisVector[i];
                }
            }
        }

    }
    ///////////////////////////////////////////////////////////////////////////
    ///// first derivative of curvature changes and necessary components //////
    ///////////////////////////////////////////////////////////////////////////
    void NonLinearBendingStrip::DerivativeNonNormalizedDirector_r( std::vector<std::vector<Vector>>& aa3_rVector
        , std::vector< std::vector<std::vector<Vector>> >& a_ar, std::vector<Vector>& a)
    {
        aa3_rVector.resize(mNumberOfNodes);
        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            aa3_rVector[I].resize(mDim);
            for(unsigned int i=0; i< mDim; i++)
            {
                aa3_rVector[I][i].resize(mDim);
                aa3_rVector[I][i] = ZeroVector(mDim);
            }
        }

        for(unsigned int I=0; I < mNumberOfNodes; I++)
        {
            for(unsigned int i=0; i<mDim; i++)
            {
                aa3_rVector[I][i] = MathUtils<double>::CrossProduct(a_ar[0][I][i] ,a[1]) + MathUtils<double>::CrossProduct(a[0], a_ar[1][I][i] );
            }
        }

    }

    void NonLinearBendingStrip::SecondDerivativeNonNormalizedDirector_rs(
          std::vector<std::vector<std::vector<std::vector<Vector>>> >& aa3_rsVector
        , std::vector< std::vector<std::vector<Vector>> >& a_ar)
    {
        aa3_rsVector.resize(mNumberOfNodes);
        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            aa3_rsVector[I].resize(mNumberOfNodes);
            for(unsigned int J = 0; J< mNumberOfNodes; J++)
            {
                aa3_rsVector[I][J].resize(mDim);
                for(unsigned int i=0; i< mDim; i++)
                {
                    aa3_rsVector[I][J][i].resize(mDim);
                    for(unsigned int j=0; j< mDim; j++)
                    {
                        aa3_rsVector[I][J][i][j].resize(mDim);
                        aa3_rsVector[I][J][i][j] = ZeroVector(mDim);
                    }
                }
            }
        }

        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            for(unsigned int J = 0; J< mNumberOfNodes; J++)
            {
                for(unsigned int i=0; i< mDim; i++)
                {
                    for(unsigned int j=0; j< mDim; j++)
                    {
                        aa3_rsVector[I][J][i][j] = MathUtils<double>::CrossProduct(a_ar[0][I][i], a_ar[1][J][j]) + MathUtils<double>::CrossProduct(a_ar[0][J][j], a_ar[1][I][i]);
                    }
                }
            }
        }

    }

    void NonLinearBendingStrip::DerivativeDirectorNorm_r(std::vector<std::vector<double>>& a3_r
        , std::vector<std::vector<Vector>>& aa3_rVector, Vector& a3Vector)
    {
        a3_r.resize(mNumberOfNodes);
        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            a3_r[I].resize(mDim);
            for(unsigned int i=0; i< mDim; i++)
            {
                a3_r[I][i] = 0.0;
            }
        }

        for(unsigned int I=0; I < mNumberOfNodes; I++)
        {
            for(unsigned int i=0; i<mDim; i++)
            {
                a3_r[I][i] = MathUtils<double>::Dot3(a3Vector ,aa3_rVector[I][i]) ;
            }
        }

    }

    void NonLinearBendingStrip::SecondDerivativeDirectorNorm_rs(
          std::vector<std::vector<std::vector<std::vector<double>>> >& a3_rs
        , std::vector<std::vector<std::vector<std::vector<Vector>>> >& aa3_rsVector
        , Vector& aa3Vector, Vector& a3Vector, double& a3,  std::vector<std::vector<Vector>>& aa3_rVector)
    {
        a3_rs.resize(mNumberOfNodes);
        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            a3_rs[I].resize(mNumberOfNodes);
            for(unsigned int J = 0; J< mNumberOfNodes; J++)
            {
                a3_rs[I][J].resize(mDim);
                for(unsigned int i=0; i< mDim; i++)
                {
                    a3_rs[I][J][i].resize(mDim);
                    for(unsigned int j=0; j< mDim; j++)
                    {
                        a3_rs[I][J][i][j]= 0.0;
                    }
                }
            }
        }

        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            for(unsigned int J = 0; J< mNumberOfNodes; J++)
            {
                for(unsigned int i=0; i< mDim; i++)
                {
                    for(unsigned int j=0; j< mDim; j++)
                    {
                        a3_rs[I][J][i][j] = ( MathUtils<double>::Dot3(aa3_rsVector[I][J][i][j], aa3Vector)
                         + MathUtils<double>::Dot3(aa3_rVector[I][i], aa3_rVector[J][j])
                         - MathUtils<double>::Dot3(aa3_rVector[I][i], a3Vector)*MathUtils<double>::Dot3(aa3_rVector[J][j], a3Vector) )/a3;
                    }
                }
            }
        }

    }

    void NonLinearBendingStrip::DerivativeDirector_r( std::vector<std::vector<Vector>>& a3_rVector
        ,  std::vector<std::vector<Vector>>& aa3_rVector, std::vector<std::vector<double>>& a3_r, Vector& a3Vector, double& a3)
    {
        a3_rVector.resize(mNumberOfNodes);
        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            a3_rVector[I].resize(mDim);
            for(unsigned int i=0; i< mDim; i++)
            {
                a3_rVector[I][i].resize(mDim);
                a3_rVector[I][i] = ZeroVector(mDim);
            }
        }

        for(unsigned int I=0; I < mNumberOfNodes; I++)
        {
            for(unsigned int i=0; i<mDim; i++)
            {
                a3_rVector[I][i] = (aa3_rVector[I][i] - a3_r[I][i]*a3Vector)/a3;
            }
        }

    }

    void NonLinearBendingStrip::SecondDerivativeDirector_rs(
          std::vector<std::vector<std::vector<std::vector<Vector>>> >& a3_rsVector
        , std::vector<std::vector<std::vector<std::vector<Vector>>> >& aa3_rsVector
        , std::vector<std::vector<std::vector<std::vector<double>>> >& a3_rs, Vector& a3Vector, double& a3
        ,  std::vector<std::vector<Vector>>& aa3_rVector, std::vector<std::vector<double>>& a3_r)
    {
        a3_rsVector.resize(mNumberOfNodes);
        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            a3_rsVector[I].resize(mNumberOfNodes);
            for(unsigned int J = 0; J< mNumberOfNodes; J++)
            {
                a3_rsVector[I][J].resize(mDim);
                for(unsigned int i=0; i< mDim; i++)
                {
                    a3_rsVector[I][J][i].resize(mDim);
                    for(unsigned int j=0; j< mDim; j++)
                    {
                        a3_rsVector[I][J][i][j].resize(mDim);
                        a3_rsVector[I][J][i][j]= ZeroVector(mDim);;
                    }
                }
            }
        }

        for(unsigned int I = 0; I< mNumberOfNodes; I++)
        {
            for(unsigned int J = 0; J< mNumberOfNodes; J++)
            {
                for(unsigned int i=0; i< mDim; i++)
                {
                    for(unsigned int j=0; j< mDim; j++)
                    {
                        a3_rsVector[I][J][i][j] = (aa3_rsVector[I][J][i][j] - a3_rs[I][J][i][j]*a3Vector)/a3
                            + (2.0*a3_r[I][i]*a3_r[J][j]*a3Vector - a3_r[I][i]*aa3_rVector[J][j] - a3_r[J][j]*aa3_rVector[I][i])*pow(a3,-2.0);
                    }
                }
            }
        }

    }

    void NonLinearBendingStrip::FirstDerivativeLocalCurvatureChange_r(std::vector<std::vector<Vector> >& kLocalVector_r
        , std::vector< std::vector<std::vector<std::vector<Vector>>> >& a_abr
        , std::vector<std::vector<Vector> >& a_ab
        , Vector& a3Vector , std::vector<std::vector<Vector>>& a3_rVector
        , std::vector< std::vector<std::vector<std::vector<double>>> >& TransformationCoeff)
    {
        kLocalVector_r.resize(2);
        for(unsigned int alpha=0; alpha< 2; alpha++)
        {
            kLocalVector_r[alpha].resize(2);

            for(unsigned int beta=0; beta<2; beta++)
            {
                kLocalVector_r[alpha][beta].resize(mNumberOfDof);
                kLocalVector_r[alpha][beta] = ZeroVector(mNumberOfDof);
            }
        }

        Vector temp(mNumberOfDof);
        for(unsigned int gamma=0; gamma<2;gamma++)
        {
            for(unsigned int delta=0; delta<2; delta++)
            {
                temp = ZeroVector(mNumberOfDof);

                for(unsigned int alpha=0; alpha<2;alpha++)
                {
                    for(unsigned int beta=0; beta<2; beta++)
                    {
                        for(unsigned int I=0; I< mNumberOfNodes; I++)
                        {
                            for(unsigned int i=0; i< mDim; i++)
                            {
                                temp(I*mDim + i) += ( MathUtils<double>::Dot3(a_abr[alpha][beta][I][i] ,a3Vector) + MathUtils<double>::Dot3(a_ab[alpha][beta], a3_rVector[I][i]) )
                                *TransformationCoeff[gamma][delta][alpha][beta] ;
                            }
                        }
                    }
                }

                noalias(kLocalVector_r[gamma][delta])= temp*(-1.0);
            }
        }

        kLocalVector_r[0][1] *= 2.0;
        kLocalVector_r[1][0] *= 2.0;
    }

    void NonLinearBendingStrip::SecondDerivativeLocalCurvatureChange_rs(std::vector<std::vector<Matrix> >& kLocalVector_rs
        ,std::vector< std::vector<std::vector<std::vector<Vector>>> >& a_abr, std::vector<std::vector<Vector>>& a3_rVector
        , std::vector<std::vector<Vector> >& a_ab, std::vector<std::vector<std::vector<std::vector<Vector>>> >& a3_rsVector
        , std::vector< std::vector<std::vector<std::vector<double>>> >& TransformationCoeff)
    {
        if (kLocalVector_rs.size() != 2)
            kLocalVector_rs.resize(2);

        for(unsigned int i=0; i< 2; ++i)
        {
            if(kLocalVector_rs[i].size() !=2)
                kLocalVector_rs[i].resize(2);

            for(unsigned int j=0; j<2; ++j)
            {
                if (kLocalVector_rs[i][j].size1() != mNumberOfDof)
                {
                    kLocalVector_rs[i][j].resize(mNumberOfDof, mNumberOfDof);
                    noalias(kLocalVector_rs[i][j]) = ZeroMatrix(mNumberOfDof, mNumberOfDof);
                }
            }
        }

        Matrix temp(mNumberOfDof, mNumberOfDof);

        for(unsigned int gamma=0; gamma<2;++gamma)
        {
            for(unsigned int delta=0; delta<2; ++delta)
            {
                temp = ZeroMatrix(mNumberOfDof, mNumberOfDof);

                for(unsigned int alpha=0; alpha<2;++alpha)
                {
                    for(unsigned int beta=0; beta<2; ++beta)
                    {
                        for(unsigned int I=0; I< mNumberOfNodes; ++I)
                        {
                            for(unsigned int J=0; J < mNumberOfNodes; ++J)
                            {
                                for(unsigned int i=0; i< mDim; ++i)
                                {
                                    for(unsigned int j=0; j< mDim; ++j)
                                    {
                                        temp(I*mDim+i, J*mDim+j) += ( MathUtils<double>::Dot3(a_abr[alpha][beta][I][i], a3_rVector[J][j])
                                           + MathUtils<double>::Dot3(a_abr[alpha][beta][J][j], a3_rVector[I][i])
                                           + MathUtils<double>::Dot3(a_ab[alpha][beta], a3_rsVector[I][J][i][j]) )
                                           *TransformationCoeff[gamma][delta][alpha][beta] ;

                                    }
                                }
                            }
                        }
                    }
                }

                noalias(kLocalVector_rs[gamma][delta]) = temp*(-1.0);
            }
        }

        kLocalVector_rs[0][1] *= 2.0;
        kLocalVector_rs[1][0] *= 2.0;
    }

    //////////////////////////////////////////////////////////////
    //////////// addtional utilities /////////////////////////////
    //////////////////////////////////////////////////////////////
    void NonLinearBendingStrip::CreatingBmatrix(Matrix& BMatrix, const std::vector<std::vector<Vector> >& LocalStrainVector_r)
    {
        if(BMatrix.size1() != mStrainSize)
            BMatrix.resize(mStrainSize, mNumberOfDof);
        noalias(BMatrix) = ZeroMatrix(mStrainSize, mNumberOfDof);

        for (int i=0; i< mNumberOfDof; i++)
        {
            BMatrix(0,i) = LocalStrainVector_r[0][0](i);
            BMatrix(1,i) = LocalStrainVector_r[1][1](i);
            BMatrix(2,i) = LocalStrainVector_r[0][1](i);
        }

    }

    void NonLinearBendingStrip::LocalTransformationOfTensor(Matrix& T, Matrix& M, std::vector< std::vector<std::vector<std::vector<double>>> >& TransformationCoeff)
    {
        if(T.size1()!=2)
            T.resize(2, 2);

        for(unsigned int gamma=0; gamma<2;++gamma)
        {
            for(unsigned int delta=0; delta<2; ++delta)
            {
                double temp=0.0;
                for(unsigned int alpha=0; alpha<2;++alpha)
                {
                    for(unsigned int beta=0; beta<2; ++beta)
                    {
                        temp += M(alpha,beta)*TransformationCoeff[gamma][delta][alpha][beta] ;
                    }
                }
                T(gamma,delta)= temp;
            }
        }
    }

    void NonLinearBendingStrip::UnitBaseVectors(std::vector<Vector>& e)
    {
            if (e.size() != 3)
                e.resize(3);

            e[0]=e[1]=e[2]=ZeroVector(mDim);
            e[0](0)=e[1](1)=e[2](2)= 1.0;
    }

    void NonLinearBendingStrip::LocalCartesianBasisVector(std::vector<Vector>& EE, std::vector<Vector>& A, Vector& A3Vector)
    {
        EE.resize(3);
        for(unsigned int i=0; i<3; ++i)
        {
            EE[i].resize(3);
        }

        EE[0] = A[0]/MathUtils<double>::Norm3(A[0]);

        Vector EE2_temp = ZeroVector(3);
        noalias(EE2_temp) +=  A[1];
        noalias(EE2_temp) -=  MathUtils<double>::Dot3(A[1], EE[0])*EE[0];
        noalias(EE[1]) = ( EE2_temp )/MathUtils<double>::Norm3(EE2_temp);

        EE[2] = A3Vector;

    }

    void NonLinearBendingStrip::LocalTransformationCoefficient(std::vector< std::vector<std::vector<std::vector<double>>> >& TransformationCoeff, std::vector<Vector>& EE, std::vector<Vector>& AA)
    {
        TransformationCoeff.resize(2);
        for(unsigned int gamma=0; gamma <2; gamma++)
        {
            TransformationCoeff[gamma].resize(2);
            for(unsigned int delta = 0; delta<2; delta++)
            {
                TransformationCoeff[gamma][delta].resize(2);
                for(unsigned int alpha=0; alpha< 2; alpha++)
                {
                    TransformationCoeff[gamma][delta][alpha].resize(2);

                    for(unsigned int beta=0; beta<2; beta++)
                    {
                        TransformationCoeff[gamma][delta][alpha][beta] = 0.0;
                    }
                }

            }
        }

        for(unsigned int gamma=0; gamma <2; gamma++)
        {
            for(unsigned int delta = 0; delta<2; delta++)
            {
                for(unsigned int alpha=0; alpha< 2; alpha++)
                {
                    for(unsigned int beta=0; beta<2; beta++)
                    {
                        TransformationCoeff[gamma][delta][alpha][beta] = MathUtils<double>::Dot3(EE[gamma],AA[alpha])*MathUtils<double>::Dot3(AA[beta],EE[delta]);
                    }
                }

            }
        }

    }

    ///////////// material matrix
    void NonLinearBendingStrip::CalculateConstitutiveMatrix(Matrix& D)
    {
        if (D.size1() != 3)
            D.resize(3,3);
        D = ZeroMatrix(3,3);

        D(1,1) = pow(10.0 ,4.0)*mE;
    }

} // Namespace Kratos.


//
//  synthetic_data_helper.cpp
//  synthetic_data_helper
//
//  Created by Cristián Garay on 10/15/16.
//  Revised and edited by Anirudhan Badrinath on 27/02/20.
//

#define NPY_NO_DEPRECATED_API NPY_1_11_API_VERSION

#include <iostream>
#include <stdint.h>
#include <alloca.h>
#include <Eigen/Core>
#include <Python.h>
#include <numpy/ndarrayobject.h>

using namespace Eigen;
using namespace std;

static double extract_double(PyArrayObject *arr, int i) {
    return ((double*) PyArray_DATA(arr))[i];
}

static double extract_double_2d(PyArrayObject *arr, int i, int j) {
    return ((double*) PyArray_DATA(arr))[i * PyArray_DIM(arr, 1) + j];
}

static double extract_int64_t(PyArrayObject *arr, int i) {
    return ((int64_t*) PyArray_DATA(arr))[i];
}

void capsule_cleanup(PyObject *capsule) {
    void *memory = PyCapsule_GetPointer(capsule, NULL);
    delete memory;
}

static PyObject* run(PyObject * module, PyObject * args) {
    //TODO: check if parameters are null.
    //TODO: check that dicts have the required members.
    //TODO: check that all parameters have the right sizes.
    //TODO: i'm not sending any error messages.
    import_array();

    PyObject *model_ptr = NULL, *starts_obj = NULL, *lengths_obj = NULL, *resources_obj = NULL;
    PyArrayObject *resources = NULL, *starts = NULL, *lengths = NULL, *learns = NULL, *forgets = NULL, *guesses = NULL, *slips = NULL;
    double prior;

    if (!PyArg_ParseTuple(args, "OOOO", &model_ptr, &starts_obj, &lengths_obj, &resources_obj)) {
        PyErr_SetString(PyExc_ValueError, "Error parsing arguments.");
        return NULL;
    }

    int DTYPE = PyArray_ObjectType(starts_obj, NPY_INT64);
    starts = (PyArrayObject *)PyArray_FROM_OTF(starts_obj, DTYPE, NPY_ARRAY_IN_ARRAY);
    DTYPE = PyArray_ObjectType(lengths_obj, NPY_INT64);
    lengths = (PyArrayObject *)PyArray_FROM_OTF(lengths_obj, DTYPE, NPY_ARRAY_IN_ARRAY);
    DTYPE = PyArray_ObjectType(resources_obj, NPY_INT64);
    resources = (PyArrayObject *)PyArray_FROM_OTF(resources_obj, DTYPE, NPY_ARRAY_IN_ARRAY);

    char* DM_NAMES[] = {"learns", "forgets", "guesses", "slips"};
    PyArrayObject** DM_PTRS[] = {&learns, &forgets, &guesses, &slips};
    for (int i = 0; i < 4; i++) {
        PyObject *dp = PyDict_GetItemString(model_ptr, DM_NAMES[i]);
        DTYPE = PyArray_ObjectType(dp, NPY_FLOAT); // hack to force correct type
        *DM_PTRS[i] = (PyArrayObject *)PyArray_FROM_OTF(dp, DTYPE, NPY_ARRAY_IN_ARRAY);
    }
    prior = PyFloat_AsDouble(PyDict_GetItemString(model_ptr, "prior"));

    int num_subparts = (int) PyArray_DIM(slips, 0);
    int num_sequences = (int) PyArray_DIM(starts, 0);
    int num_resources = (int) PyArray_DIM(learns, 0);

    Vector2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2, 2*num_resources);
    for (int n=0; n<num_resources; n++) {
        double learn = extract_double(learns,n);
        double forget = extract_double(forgets,n);
        As.col(2*n) << 1-learn, learn;
        As.col(2*n+1) << forget, 1-forget;
    }

    int64_t bigT = 0;
    for (int k=0; k<num_sequences; k++) {
        bigT += extract_int64_t(lengths,k); //extract this as int??
    }

    //// outputs
    int* all_stateseqs = new int[bigT];
    int* all_data = new int[num_subparts * bigT]; //used to be int8_t
    *all_data = 0;

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        int64_t sequence_start = extract_int64_t(starts,sequence_index) - 1; //should i extract these as ints?
        int64_t T = extract_int64_t(lengths, sequence_index);

        Vector2d nextstate_distr = initial_distn;

        for (int t=0; t<T; t++) {
            *(all_stateseqs + sequence_start + t) = nextstate_distr(0) < ((double) rand()) / ((double) RAND_MAX); //always all_stateseqs[0]?
            for (int n=0; n<num_subparts; n++) {
                *(all_data + n * (bigT) + sequence_start + t) = ((*(all_stateseqs + sequence_start + t)) ? extract_double(slips, n) : (1-extract_double(guesses, n))) < (((double) rand()) / ((double) RAND_MAX));
            }

            nextstate_distr = As.col(2*(extract_int64_t(resources, sequence_start + t)-1)+*(all_stateseqs + sequence_start + t)); //extract int is right??
        }
    }

    PyObject *result = PyDict_New();

    npy_intp dims1[] = {1, bigT};
    PyObject *all_stateseqs_arr = (PyObject *) PyArray_SimpleNewFromData(2, dims1, NPY_INT, all_stateseqs);
    PyObject *capsule1 = PyCapsule_New(all_stateseqs, NULL, capsule_cleanup);
    PyArray_SetBaseObject((PyArrayObject *) all_stateseqs_arr, capsule1);

    npy_intp dims2[] = {num_subparts, bigT};
    PyObject *all_data_arr = (PyObject *) PyArray_SimpleNewFromData(2, dims2, NPY_INT, all_data);
    PyObject *capsule2 = PyCapsule_New(all_data, NULL, capsule_cleanup);
    PyArray_SetBaseObject((PyArrayObject *) all_data_arr, capsule2);

    PyDict_SetItemString(result, "stateseqs", all_stateseqs_arr);
    PyDict_SetItemString(result, "data", all_data_arr);

    Py_XDECREF(resources);
    Py_XDECREF(starts);
    Py_XDECREF(lengths);
    Py_XDECREF(all_stateseqs_arr);
    Py_XDECREF(all_data_arr);

    for (int i = 0; i < 4; i++)
        Py_XDECREF(*DM_PTRS[i]);

    return(result);
}

static PyMethodDef synthetic_data_helper_Methods[] = {
    {"create_synthetic_data",  run, METH_VARARGS,
     "Helper for creating synthetic data from true model"},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

static struct PyModuleDef synthetic_data_helper_module = {
   PyModuleDef_HEAD_INIT,
   "synthetic_data_helper",   /* name of module */
   NULL, /* module documentation, may be NULL */
   -1,       /* size of per-interpreter state of the module,
                or -1 if the module keeps state in global variables. */
   synthetic_data_helper_Methods
};

PyMODINIT_FUNC PyInit_synthetic_data_helper() {
    return PyModule_Create(&synthetic_data_helper_module);
}


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

#ifndef BOOST_HYPERGEOMETRIC_PFQ_SERIES_HPP_
#define BOOST_HYPERGEOMETRIC_PFQ_SERIES_HPP_

#ifndef BOOST_MATH_PFQ_MAX_B_TERMS
#  define BOOST_MATH_PFQ_MAX_B_TERMS 5
#endif

#include <boost/array.hpp>
#include <boost/math/special_functions/detail/hypergeometric_series.hpp>

  namespace boost { namespace math { namespace detail {

     template <class Seq, class Real>
     unsigned set_crossover_locations(const Seq& aj, const Seq& bj, const Real& z, unsigned int* crossover_locations)
     {
        BOOST_MATH_STD_USING
        unsigned N_terms = 0;

        if(aj.size() == 1 && bj.size() == 1)
        {
           //
           // For 1F1 we can work out where the peaks in the series occur,
           //  which is to say when:
           //
           // (a + k)z / (k(b + k)) == +-1
           //
           // Then we are at either a maxima or a minima in the series, and the
           // last such point must be a maxima since the series is globally convergent.
           // Potentially then we are solving 2 quadratic equations and have up to 4
           // solutions, any solutions which are complex or negative are discarded,
           // leaving us with 4 options:
           //
           // 0 solutions: The series is directly convergent.
           // 1 solution : The series diverges to a maxima before converging.
           // 2 solutions: The series is initially convergent, followed by divergence to a maxima before final convergence.
           // 3 solutions: The series diverges to a maxima, converges to a minima before diverging again to a second maxima before final convergence.
           // 4 solutions: The series converges to a minima before diverging to a maxima, converging to a minima, diverging to a second maxima and then converging.
           //
           // The first 2 situations are adequately handled by direct series evaluation, while the 2,3 and 4 solutions are not.
           //
           Real a = *aj.begin();
           Real b = *bj.begin();
           Real sq = 4 * a * z + b * b - 2 * b * z + z * z;
           if (sq >= 0)
           {
              Real t = (-sqrt(sq) - b + z) / 2;
              if (t >= 0)
              {
                 crossover_locations[N_terms] = itrunc(t);
                 ++N_terms;
              }
              t = (sqrt(sq) - b + z) / 2;
              if (t >= 0)
              {
                 crossover_locations[N_terms] = itrunc(t);
                 ++N_terms;
              }
           }
           sq = -4 * a * z + b * b + 2 * b * z + z * z;
           if (sq >= 0)
           {
              Real t = (-sqrt(sq) - b - z) / 2;
              if (t >= 0)
              {
                 crossover_locations[N_terms] = itrunc(t);
                 ++N_terms;
              }
              t = (sqrt(sq) - b - z) / 2;
              if (t >= 0)
              {
                 crossover_locations[N_terms] = itrunc(t);
                 ++N_terms;
              }
           }
           std::sort(crossover_locations, crossover_locations + N_terms, std::less<Real>());
           //
           // Now we need to discard every other terms, as these are the minima:
           //
           switch (N_terms)
           {
           case 0:
           case 1:
              break;
           case 2:
              crossover_locations[0] = crossover_locations[1];
              --N_terms;
              break;
           case 3:
              crossover_locations[1] = crossover_locations[2];
              --N_terms;
              break;
           case 4:
              crossover_locations[0] = crossover_locations[1];
              crossover_locations[1] = crossover_locations[3];
              N_terms -= 2;
              break;
           }
        }
        else
        {
           unsigned n = 0;
           for (auto bi = bj.begin(); bi != bj.end(); ++bi, ++n)
           {
              crossover_locations[n] = *bi >= 0 ? 0 : itrunc(-*bi) + 1;
           }
           std::sort(crossover_locations, crossover_locations + bj.size(), std::less<Real>());
           N_terms = (unsigned)bj.size();
        }
        return N_terms;
     }

     template <class Seq, class Real, class Policy, class Terminal>
     std::pair<Real, Real> hypergeometric_pFq_checked_series_impl(const Seq& aj, const Seq& bj, const Real& z, const Policy& pol, const Terminal& termination, int& log_scale)
     {
        BOOST_MATH_STD_USING
        Real result = 1;
        Real abs_result = 1;
        Real term = 1;
        Real term0 = 0;
        Real tol = boost::math::policies::get_epsilon<Real, Policy>();
        boost::uintmax_t k = 0;
        Real upper_limit(sqrt(boost::math::tools::max_value<Real>())), diff;
        Real lower_limit(1 / upper_limit);
        int log_scaling_factor = itrunc(boost::math::tools::log_max_value<Real>()) - 2;
        Real scaling_factor = exp(Real(log_scaling_factor));
        Real term_m1;
        int local_scaling = 0;

        if ((aj.size() == 1) && (bj.size() == 0))
        {
           if (fabs(z) > 1)
           {
              if ((z > 0) && (floor(*aj.begin()) != *aj.begin()))
              {
                 Real r = policies::raise_domain_error("boost::math::hypergeometric_pFq", "Got p == 1 and q == 0 and |z| > 1, result is imaginary", z, pol);
                 return std::make_pair(r, r);
              }
              std::pair<Real, Real> r = hypergeometric_pFq_checked_series_impl(aj, bj, Real(1 / z), pol, termination, log_scale);
              Real mul = pow(-z, -*aj.begin());
              r.first *= mul;
              r.second *= mul;
              return r;
           }
        }

        if (aj.size() > bj.size())
        {
           if (aj.size() == bj.size() + 1)
           {
              if (fabs(z) > 1)
              {
                 Real r = policies::raise_domain_error("boost::math::hypergeometric_pFq", "Got p == q+1 and |z| > 1, series does not converge", z, pol);
                 return std::make_pair(r, r);
              }
              if (fabs(z) == 1)
              {
                 Real s = 0;
                 for (auto i = bj.begin(); i != bj.end(); ++i)
                    s += *i;
                 for (auto i = aj.begin(); i != aj.end(); ++i)
                    s -= *i;
                 if ((z == 1) && (s <= 0))
                 {
                    Real r = policies::raise_domain_error("boost::math::hypergeometric_pFq", "Got p == q+1 and |z| == 1, in a situation where the series does not converge", z, pol);
                    return std::make_pair(r, r);
                 }
                 if ((z == -1) && (s <= -1))
                 {
                    Real r = policies::raise_domain_error("boost::math::hypergeometric_pFq", "Got p == q+1 and |z| == 1, in a situation where the series does not converge", z, pol);
                    return std::make_pair(r, r);
                 }
              }
           }
           else
           {
              Real r = policies::raise_domain_error("boost::math::hypergeometric_pFq", "Got p > q+1, series does not converge", z, pol);
              return std::make_pair(r, r);
           }
        }

        while (!termination(k))
        {
           for (auto ai = aj.begin(); ai != aj.end(); ++ai)
           {
              term *= *ai + k;
           }
           if (term == 0)
           {
              // There is a negative integer in the aj's:
              return std::make_pair(result, abs_result);
           }
           for (auto bi = bj.begin(); bi != bj.end(); ++bi)
           {
              if (*bi + k == 0)
              {
                 // The series is undefined:
                 result = boost::math::policies::raise_domain_error("boost::math::hypergeometric_pFq<%1%>", "One of the b values was the negative integer %1%", *bi, pol);
                 return std::make_pair(result, result);
              }
              term /= *bi + k;
           }
           term *= z;
           ++k;
           term /= k;
           //std::cout << k << " " << *bj.begin() + k << " " << result << " " << term << /*" " << term_at_k(*aj.begin(), *bj.begin(), z, k, pol) <<*/ std::endl;
           result += term;
           abs_result += abs(term);
           //std::cout << "k = " << k << " term = " << term * exp(log_scale) << " result = " << result * exp(log_scale) << " abs_result = " << abs_result * exp(log_scale) << std::endl;

           //
           // Rescaling:
           //
           if (fabs(abs_result) >= upper_limit)
           {
              abs_result /= scaling_factor;
              result /= scaling_factor;
              term /= scaling_factor;
              log_scale += log_scaling_factor;
              local_scaling += log_scaling_factor;
           }
           if (fabs(abs_result) < lower_limit)
           {
              abs_result *= scaling_factor;
              result *= scaling_factor;
              term *= scaling_factor;
              log_scale -= log_scaling_factor;
              local_scaling -= log_scaling_factor;
           }

           if ((abs(result * tol) > abs(term)) && (abs(term0) > abs(term)))
              break;
           if (abs_result * tol > abs(result))
           {
              // We have no correct bits in the result... just give up!
              result = boost::math::policies::raise_evaluation_error("boost::math::hypergeometric_pFq<%1%>", "Cancellation is so severe that no bits in the reuslt are correct, last result was %1%", Real(result * exp(Real(log_scale))), pol);
              return std::make_pair(result, result);
           }
           term0 = term;
        }
        //std::cout << "result = " << result << std::endl;
        //std::cout << "local_scaling = " << local_scaling << std::endl;
        //std::cout << "Norm result = " << std::setprecision(35) << boost::multiprecision::mpfr_float_50(result) * exp(boost::multiprecision::mpfr_float_50(local_scaling)) << std::endl;
        //
        // We have to be careful when one of the b's crosses the origin:
        //
        if(bj.size() > BOOST_MATH_PFQ_MAX_B_TERMS)
           policies::raise_domain_error<Real>("boost::math::hypergeometric_pFq<%1%>(Seq, Seq, %1%)",
              "The number of b terms must be less than the value of BOOST_MATH_PFQ_MAX_B_TERMS (" BOOST_STRINGIZE(BOOST_MATH_PFQ_MAX_B_TERMS)  "), but got %1%.",
              Real(bj.size()), pol);

        unsigned crossover_locations[BOOST_MATH_PFQ_MAX_B_TERMS];

        unsigned N_crossovers = set_crossover_locations(aj, bj, z, crossover_locations);

        bool terminate = false;   // Set to true if one of the a's passes through the origin and terminates the series.

        for (unsigned n = 0; n < N_crossovers; ++n)
        {
           if (k < crossover_locations[n])
           {
              for (auto ai = aj.begin(); ai != aj.end(); ++ai)
              {
                 if ((*ai < 0) && (floor(*ai) == *ai) && (*ai > crossover_locations[n]))
                    return std::make_pair(result, abs_result);  // b's will never cross the origin!
              }
              //
              // local results:
              //
              Real loop_result = 0;
              Real loop_abs_result = 0;
              int loop_scale = 0;
              //
              // loop_error_scale will be used to increase the size of the error
              // estimate (absolute sum), based on the errors inherent in calculating
              // the pochhammer symbols.
              //
              Real loop_error_scale = 0;
              //boost::multiprecision::mpfi_float err_est = 0;
              //
              // b hasn't crossed the origin yet and the series may spring back into life at that point
              // so we need to jump forward to that term and then evaluate forwards and backwards from there:
              //
              unsigned s = crossover_locations[n];
              boost::uintmax_t backstop = k;
              int s1(1), s2(1);
              term = 0;
              for (auto ai = aj.begin(); ai != aj.end(); ++ai)
              {
                 if ((floor(*ai) == *ai) && (*ai < 0) && (-*ai <= s))
                 {
                    // One of the a terms has passed through zero and terminated the series:
                    terminate = true;
                    break;
                 }
                 else
                 {
                    int ls = 1;
                    Real p = log_pochhammer(*ai, s, pol, &ls);
                    s1 *= ls;
                    term += p;
                    loop_error_scale = (std::max)(p, loop_error_scale);
                    //err_est += boost::multiprecision::mpfi_float(p);
                 }
              }
              //std::cout << "term = " << term << std::endl;
              if (terminate)
                 break;
              for (auto bi = bj.begin(); bi != bj.end(); ++bi)
              {
                 int ls = 1;
                 Real p = log_pochhammer(*bi, s, pol, &ls);
                 s2 *= ls;
                 term -= p;
                 loop_error_scale = (std::max)(p, loop_error_scale);
                 //err_est -= boost::multiprecision::mpfi_float(p);
              }
              //std::cout << "term = " << term << std::endl;
              Real p = lgamma(Real(s + 1), pol);
              term -= p;
              loop_error_scale = (std::max)(p, loop_error_scale);
              //err_est -= boost::multiprecision::mpfi_float(p);
              p = s * log(fabs(z));
              term += p;
              loop_error_scale = (std::max)(p, loop_error_scale);
              //err_est += boost::multiprecision::mpfi_float(p);
              //err_est = exp(err_est);
              //std::cout << err_est << std::endl;
              //
              // Convert loop_error scale to the absolute error
              // in term after exp is applied:
              //
              loop_error_scale *= tools::epsilon<Real>();
              //
              // Convert to relative error after exp:
              //
              loop_error_scale = fabs(expm1(loop_error_scale, pol));
              //
              // Convert to multiplier for the error term:
              //
              loop_error_scale /= tools::epsilon<Real>();

              if (z < 0)
                 s1 *= (s & 1 ? -1 : 1);

              if (term <= tools::log_min_value<Real>())
              {
                 // rescale if we can:
                 int scale = itrunc(floor(term - tools::log_min_value<Real>()) - 2);
                 term -= scale;
                 loop_scale += scale;
              }
               if (term > 10)
               {
                  int scale = itrunc(floor(term));
                  term -= scale;
                  loop_scale += scale;
               }
               //std::cout << "term = " << term << std::endl;
               term = s1 * s2 * exp(term);
               //std::cout << "term = " << term << std::endl;
               //std::cout << "loop_scale = " << loop_scale << std::endl;
               k = s;
               term0 = term;
               int saved_loop_scale = loop_scale;
               bool terms_are_growing = true;
               bool trivial_small_series_check = false;
               do
               {
                  loop_result += term;
                  loop_abs_result += fabs(term);
                  //std::cout << "k = " << k << " term = " << term * exp(loop_scale) << " result = " << loop_result * exp(loop_scale) << " abs_result = " << loop_abs_result * exp(loop_scale) << std::endl;
                  if (fabs(loop_result) >= upper_limit)
                  {
                     loop_result /= scaling_factor;
                     loop_abs_result /= scaling_factor;
                     term /= scaling_factor;
                     loop_scale += log_scaling_factor;
                  }
                  if (fabs(loop_result) < lower_limit)
                  {
                     loop_result *= scaling_factor;
                     loop_abs_result *= scaling_factor;
                     term *= scaling_factor;
                     loop_scale -= log_scaling_factor;
                  }
                  term_m1 = term;
                  for (auto ai = aj.begin(); ai != aj.end(); ++ai)
                  {
                     term *= *ai + k;
                  }
                  if (term == 0)
                  {
                     // There is a negative integer in the aj's:
                     return std::make_pair(result, abs_result);
                  }
                  for (auto bi = bj.begin(); bi != bj.end(); ++bi)
                  {
                     if (*bi + k == 0)
                     {
                        // The series is undefined:
                        result = boost::math::policies::raise_domain_error("boost::math::hypergeometric_pFq<%1%>", "One of the b values was the negative integer %1%", *bi, pol);
                        return std::make_pair(result, result);
                     }
                     term /= *bi + k;
                  }
                  term *= z / (k + 1);

                  ++k;
                  diff = fabs(term / loop_result);
                  terms_are_growing = fabs(term) > fabs(term_m1);
                  if (!trivial_small_series_check && !terms_are_growing)
                  {
                     //
                     // Now that we have started to converge, check to see if the value of
                     // this local sum is trivially small compared to the result.  If so
                     // abort this part of the series.
                     //
                     trivial_small_series_check = true;
                     Real d;
                     if (loop_scale > local_scaling)
                     {
                        int rescale = local_scaling - loop_scale;
                        if (rescale < tools::log_min_value<Real>())
                           d = 1;  // arbitrary value, we want to keep going
                        else
                           d = fabs(term / (result * exp(Real(rescale))));
                     }
                     else
                     {
                        int rescale = loop_scale - local_scaling;
                        if (rescale < tools::log_min_value<Real>())
                           d = 0;  // terminate this loop
                        else
                           d = fabs(term * exp(Real(rescale)) / result);
                     }
                     if (d < boost::math::policies::get_epsilon<Real, Policy>())
                        break;
                  }
               } while (!termination(k - s) && ((diff > boost::math::policies::get_epsilon<Real, Policy>()) || terms_are_growing));

               //std::cout << "Norm loop result = " << std::setprecision(35) << boost::multiprecision::mpfr_float_50(loop_result)* exp(boost::multiprecision::mpfr_float_50(loop_scale)) << std::endl;
               //
               // We now need to combine the results of the first series summation with whatever
               // local results we have now.  First though, rescale abs_result by loop_error_scale
               // to factor in the error in the pochhammer terms at the start of this block:
               //
               boost::uintmax_t next_backstop = k;
               loop_abs_result += loop_error_scale * fabs(loop_result);
               if (loop_scale > local_scaling)
               {
                  //
                  // Need to shrink previous result:
                  //
                  int rescale = local_scaling - loop_scale;
                  local_scaling = loop_scale;
                  log_scale -= rescale;
                  Real ex = exp(Real(rescale));
                  result *= ex;
                  abs_result *= ex;
                  result += loop_result;
                  abs_result += loop_abs_result;
               }
               else if (local_scaling > loop_scale)
               {
                  //
                  // Need to shrink local result:
                  //
                  int rescale = loop_scale - local_scaling;
                  Real ex = exp(Real(rescale));
                  loop_result *= ex;
                  loop_abs_result *= ex;
                  result += loop_result;
                  abs_result += loop_abs_result;
               }
               else
               {
                  result += loop_result;
                  abs_result += loop_abs_result;
               }
               //
               // Now go backwards as well:
               //
               k = s;
               term = term0;
               loop_result = 0;
               loop_abs_result = 0;
               loop_scale = saved_loop_scale;
               trivial_small_series_check = false;
               do
               {
                  --k;
                  if (k == backstop)
                     break;
                  term_m1 = term;
                  for (auto ai = aj.begin(); ai != aj.end(); ++ai)
                  {
                     term /= *ai + k;
                  }
                  for (auto bi = bj.begin(); bi != bj.end(); ++bi)
                  {
                     if (*bi + k == 0)
                     {
                        // The series is undefined:
                        result = boost::math::policies::raise_domain_error("boost::math::hypergeometric_pFq<%1%>", "One of the b values was the negative integer %1%", *bi, pol);
                        return std::make_pair(result, result);
                     }
                     term *= *bi + k;
                  }
                  term *= (k + 1) / z;
                  loop_result += term;
                  loop_abs_result += fabs(term);

                  if (!trivial_small_series_check && (fabs(term) < fabs(term_m1)))
                  {
                     //
                     // Now that we have started to converge, check to see if the value of
                     // this local sum is trivially small compared to the result.  If so
                     // abort this part of the series.
                     //
                     trivial_small_series_check = true;
                     Real d;
                     if (loop_scale > local_scaling)
                     {
                        int rescale = local_scaling - loop_scale;
                        if (rescale < tools::log_min_value<Real>())
                           d = 1;  // keep going
                        else
                           d = fabs(term / (result * exp(Real(rescale))));
                     }
                     else
                     {
                        int rescale = loop_scale - local_scaling;
                        if (rescale < tools::log_min_value<Real>())
                           d = 0;  // stop, underflow
                        else
                           d = fabs(term * exp(Real(rescale)) / result);
                     }
                     if (d < boost::math::policies::get_epsilon<Real, Policy>())
                        break;
                  }

                  //std::cout << "k = " << k << " result = " << result << " abs_result = " << abs_result << std::endl;
                  if (fabs(loop_result) >= upper_limit)
                  {
                     loop_result /= scaling_factor;
                     loop_abs_result /= scaling_factor;
                     term /= scaling_factor;
                     loop_scale += log_scaling_factor;
                  }
                  if (fabs(loop_result) < lower_limit)
                  {
                     loop_result *= scaling_factor;
                     loop_abs_result *= scaling_factor;
                     term *= scaling_factor;
                     loop_scale -= log_scaling_factor;
                  }
                  diff = fabs(term / loop_result);
               } while (!termination(s - k) && ((diff > boost::math::policies::get_epsilon<Real, Policy>()) || (fabs(term) > fabs(term_m1))));

               //std::cout << "Norm loop result = " << std::setprecision(35) << boost::multiprecision::mpfr_float_50(loop_result)* exp(boost::multiprecision::mpfr_float_50(loop_scale)) << std::endl;
               //
               // We now need to combine the results of the first series summation with whatever
               // local results we have now.  First though, rescale abs_result by loop_error_scale
               // to factor in the error in the pochhammer terms at the start of this block:
               //
               loop_abs_result += loop_error_scale * fabs(loop_result);
               //
               if (loop_scale > local_scaling)
               {
                  //
                  // Need to shrink previous result:
                  //
                  int rescale = local_scaling - loop_scale;
                  local_scaling = loop_scale;
                  log_scale -= rescale;
                  Real ex = exp(Real(rescale));
                  result *= ex;
                  abs_result *= ex;
                  result += loop_result;
                  abs_result += loop_abs_result;
               }
               else if (local_scaling > loop_scale)
               {
                  //
                  // Need to shrink local result:
                  //
                  int rescale = loop_scale - local_scaling;
                  Real ex = exp(Real(rescale));
                  loop_result *= ex;
                  loop_abs_result *= ex;
                  result += loop_result;
                  abs_result += loop_abs_result;
               }
               else
               {
                  result += loop_result;
                  abs_result += loop_abs_result;
               }
               //
               // Reset k to the largest k we reached
               //
               k = next_backstop;
           }
        }

        return std::make_pair(result, abs_result);
     }

     struct iteration_terminator
     {
        iteration_terminator(boost::uintmax_t i) : m(i) {}

        bool operator()(boost::uintmax_t v) const { return v >= m; }

        boost::uintmax_t m;
     };

     template <class Seq, class Real, class Policy>
     Real hypergeometric_pFq_checked_series_impl(const Seq& aj, const Seq& bj, const Real& z, const Policy& pol, int& log_scale)
     {
        BOOST_MATH_STD_USING
        iteration_terminator term(boost::math::policies::get_max_series_iterations<Policy>());
        std::pair<Real, Real> result = hypergeometric_pFq_checked_series_impl(aj, bj, z, pol, term, log_scale);
        //
        // Check to see how many digits we've lost, if it's more than half, raise an evaluation error -
        // this is an entirely arbitrary cut off, but not unreasonable.
        //
        if (result.second * sqrt(boost::math::policies::get_epsilon<Real, Policy>()) > abs(result.first))
        {
           return boost::math::policies::raise_evaluation_error("boost::math::hypergeometric_pFq<%1%>", "Cancellation is so severe that fewer than half the bits in the result are correct, last result was %1%", Real(result.first * exp(Real(log_scale))), pol);
        }
        return result.first;
     }

     template <class Real, class Policy>
     inline Real hypergeometric_1F1_checked_series_impl(const Real& a, const Real& b, const Real& z, const Policy& pol, int& log_scale)
     {
        boost::array<Real, 1> aj = { a };
        boost::array<Real, 1> bj = { b };
        return hypergeometric_pFq_checked_series_impl(aj, bj, z, pol, log_scale);
     }

  } } } // namespaces

#endif // BOOST_HYPERGEOMETRIC_PFQ_SERIES_HPP_