#define NOMINMAX
#include "LinearHexahedralPusoFormulation.hpp"
#include <assert.h>
#include <boost/detail/limits.hpp>
#include "LinearHexahedralPusoFormulationHelper.hpp"
#include "domain/elements/FiniteElement.hpp"
#include "domain/elements/ElementGeometry.hpp"
#include "domain/elements/MatrixOption.hpp"
#include "domain/elements/Node.hpp"
#include "domain/materials/MaterialModel.hpp"
#include "domain/physics/InfinitesimalState.hpp"
#include "domain/physics/UpdatedPhysicalState.hpp"
#include "foundation/NotImplementedException.hpp"
#include "foundation/NotSupportedException.hpp"
#include "Foundation/BLAS/ColumnVector.hpp"
#include "domain/analyses/AnalysisTimeline.hpp"
#include "foundation/memory/pointer.hpp"

namespace adf = axis::domain::formulations;
namespace ada = axis::domain::analyses;
namespace ade = axis::domain::elements;
namespace adm = axis::domain::materials;
namespace adp = axis::domain::physics;
namespace af = axis::foundation;
namespace afb = axis::foundation::blas;
namespace afm = axis::foundation::memory;
namespace afu = axis::foundation::uuids;

namespace {
  typedef adf::LinearHexahedralPusoFormulation hexa;
}

hexa::LinearHexahedralPusoFormulation(void)
{
  lumpedMass_ = NULLPTR;
  Bmatrix_ = NULLPTR;
  nodeCoordinates_ = NULLPTR;
  jacobian_ = NULLPTR;
  for (int i = 0; i < 4; i++)
  {
    hourglassForces_[i] = NULLPTR;
  }
  volume_ = 0;
  hourglassEnergy_ = 0;
}

hexa::~LinearHexahedralPusoFormulation(void)
{
  ClearMemory();
  if (nodeCoordinates_ != NULLPTR)
  {
    absref<afb::DenseMatrix>(nodeCoordinates_).Destroy();
    System::ModelMemory().Deallocate(nodeCoordinates_);
  }
  if (Bmatrix_ != NULLPTR)
  {
    absref<afb::DenseMatrix>(Bmatrix_).Destroy();
    System::ModelMemory().Deallocate(Bmatrix_);
  }
  if (jacobian_ != NULLPTR)
  {
    absref<afb::DenseMatrix>(jacobian_).Destroy();
    System::ModelMemory().Deallocate(jacobian_);
  }
  for (int i = 0; i < 4; i++)
  {
    if (hourglassForces_[i] != NULLPTR)
    {
      absref<afb::ColumnVector>(hourglassForces_[i]).Destroy();
      System::ModelMemory().Deallocate(hourglassForces_[i]);
      hourglassForces_[i] = NULLPTR;
    }
  }
  jacobian_ = NULLPTR;
  Bmatrix_ = NULLPTR;
  nodeCoordinates_ = NULLPTR;
}

void hexa::Destroy( void ) const
{
  delete this;
}

const afb::SymmetricMatrix& hexa::GetStiffness( void ) const
{
  // TODO : implement stiffness matrix according to Puso (2000).
  throw af::NotImplementedException(
    _T("This element formulation does not implement stiffness matrix."));
}

const afb::SymmetricMatrix& hexa::GetConsistentMass( void ) const
{
  throw af::NotImplementedException(
    _T("This element formulation does not implement consistent mass."));
}

const afb::ColumnVector& hexa::GetLumpedMass( void ) const
{
  return absref<afb::ColumnVector>(lumpedMass_);
}

void hexa::AllocateMemory( void )
{
  // initialize matrices
  nodeCoordinates_ = afb::DenseMatrix::Create(8, 3);
  Bmatrix_ = afb::DenseMatrix::Create(3, 8);
  jacobian_ = afb::DenseMatrix::Create(3, 3);
  for (int i = 0; i < 4; i++)
  {
    hourglassForces_[i] = afb::ColumnVector::Create(3);
  }
}

void hexa::CalculateInitialState( void )
{
  // get initial nodal coordinates
  ade::ElementGeometry& geometry = Element().Geometry();
  auto& Bmatrix = absref<afb::DenseMatrix>(Bmatrix_);
  auto& nodeCoordinates = absref<afb::DenseMatrix>(nodeCoordinates_);
  auto& jacobian = absref<afb::DenseMatrix>(jacobian_);
  CalculateNodeCoordinate(nodeCoordinates, geometry);
  CalculateBMatrix(Bmatrix, volume_, nodeCoordinates);
  CalculateJacobian(jacobian, nodeCoordinates);

  // clear hourglass force vectors
  for (int i = 0; i < 4; i++)
  {
    absref<afb::ColumnVector>(hourglassForces_[i]).ClearAll();
  }
}

void hexa::UpdateStrain( const afb::ColumnVector& elementDisplacementIncrement )
{
  const afb::ColumnVector& du = elementDisplacementIncrement; // shorthand
  const afb::DenseMatrix& B = absref<afb::DenseMatrix>(Bmatrix_);
  afb::ColumnVector& strain = Element().PhysicalState().Strain();
  afb::ColumnVector& strainIncrement = Element().PhysicalState().LastStrainIncrement();

  // our B-matrix is somewhat "different", so we rearrange the matrix-vector
  // computation normally done
  strainIncrement.ClearAll();
  for (int i = 0; i < 8; i++)
  {
    real e1 = B(0,i)*du(3*i);                                 // du1/dx
    real e2 = B(1,i)*du(3*i + 1);                             // du2/dy
    real e3 = B(2,i)*du(3*i + 2);                             // du3/dz
    real e4 = B(2,i)*du(3*i + 1) + B(1,i)*du(3*i + 2);        // du2/dz + du3/dy
    real e5 = B(2,i)*du(3*i) + B(0,i)*du(3*i + 2);            // du1/dz + du3/dx
    real e6 = B(1,i)*du(3*i) + B(0,i)*du(3*i + 1);            // du1/dy + du2/dx
    strainIncrement(0) += e1;
    strainIncrement(1) += e2;
    strainIncrement(2) += e3;
    strainIncrement(3) += e4;
    strainIncrement(4) += e5;
    strainIncrement(5) += e6;
  }
  afb::VectorSum(strain, 1.0, strain, 1.0, strainIncrement);
}

void hexa::UpdateInternalForce( afb::ColumnVector& internalForce,
  const afb::ColumnVector& elementDisplacementIncrement,
  const afb::ColumnVector& elementVelocity,
  const ada::AnalysisTimeline& timeInfo )
{
  internalForce.ClearAll();
  CalculateCentroidalInternalForce(internalForce);
  StabilizeInternalForce(internalForce, elementDisplacementIncrement);
  internalForce.Scale(-1.0);
}

void hexa::UpdateMatrices( const ade::MatrixOption& whichMatrices,
  const afb::ColumnVector& elementDisplacement,
  const afb::ColumnVector& elementVelocity )
{
  if (whichMatrices.DoesRequestConsistentMassMatrix())
  {
    throw af::NotSupportedException(
      _T("This element formulation does not support consistent mass."));
  }
  else if (whichMatrices.DoesRequestLumpedMassMatrix())
  {
    UpdateLumpedMassMatrix();
  }
  else if (whichMatrices.DoesRequestStiffnessMatrix())
  {
    throw af::NotSupportedException(
      _T("This element formulation does not support stiffness matrix."));
  }
  else
  {
    assert(!_T("An unpredictable matrix option was requested!"));
  }
}

void hexa::ClearMemory( void )
{
  if (lumpedMass_ != NULLPTR)
  {
    absref<afb::ColumnVector>(lumpedMass_).Destroy();
    System::ModelMemory().Deallocate(lumpedMass_);
  }
  lumpedMass_ = NULLPTR;
}

real hexa::GetCriticalTimestep(const afb::ColumnVector& modelDisplacement) const
{
  /* According to Puso (2000), the calculation below is empirically faster and
     works for most of the problems. Also, other more conservative equations,
     is generally coupled to material model and does not account hourglass
     stiffness, which generally presents two significant drawbacks.
  **/
  const ade::ElementGeometry& geometry = Element().Geometry();
  const adm::MaterialModel& material = Element().Material();
  const real speed = material.GetWavePropagationSpeed();

  // get the shortest distance between nodes
  real length = std::numeric_limits<real>::max();
  for (int i = 0; i < 7; i++)
  {
    const ade::Node& nodeX = geometry[i];
    for (int j = i + 1; j < 8; j++)
    {
      const ade::Node& nodeY = geometry[j];
      real dx = nodeX.X() - nodeY.X();
      real dy = nodeX.Y() - nodeY.Y();
      real dz = nodeX.Z() - nodeY.Z();
      real dist = sqrt(dx*dx + dy*dy + dz*dz);
      if (dist < length)
      {
        length = dist;
      }
    }
  }

  // this is a pessimistic approach: the fastest speed and the shortest distance
  real solidLongitudinalWaveSpeed = sqrt(speed / length);
  return solidLongitudinalWaveSpeed;
}

void hexa::UpdateLumpedMassMatrix( void )
{
  if (lumpedMass_ == NULLPTR)
  {
    lumpedMass_ = axis::foundation::blas::ColumnVector::Create(24);
  }
  adm::MaterialModel& material = Element().Material();
  real massPerNode = volume_ * material.Density() / 8;
  absref<afb::ColumnVector>(lumpedMass_).SetAll(massPerNode);
}

void hexa::CalculateCentroidalInternalForce( afb::ColumnVector& internalForce )
{
  const afb::DenseMatrix& B = absref<afb::DenseMatrix>(Bmatrix_);
  const afb::ColumnVector& stress = Element().PhysicalState().Stress();
  afb::ColumnVector& fint = internalForce;
  for (int i = 0; i < 8; i++)
  {
    real f1 = B(0, i)*stress(0) + B(2, i)*stress(4) + B(1, i)*stress(5);
    real f2 = B(1, i)*stress(1) + B(2, i)*stress(3) + B(0, i)*stress(5);
    real f3 = B(2, i)*stress(2) + B(1, i)*stress(3) + B(0, i)*stress(4);

    fint(3*i  ) = f1;
    fint(3*i+1) = f2;
    fint(3*i+2) = f3;
  }
  fint.Scale(volume_);
}

real adf::LinearHexahedralPusoFormulation::GetTotalArtificialEnergy( void ) const
{
  return hourglassEnergy_;
}

void hexa::StabilizeInternalForce( axis::foundation::blas::ColumnVector& internalForce,
                                   const axis::foundation::blas::ColumnVector& displacementIncrement )
{
  id_type id = Element().GetInternalId();

  // These are some shorthands that we are going to use
  const afb::DenseMatrix& J = absref<afb::DenseMatrix>(jacobian_);
  const afb::DenseMatrix& B = absref<afb::DenseMatrix>(Bmatrix_);
  const afb::ColumnVector& du = displacementIncrement;
  afb::DenseMatrix& X = absref<afb::DenseMatrix>(nodeCoordinates_);
  const afb::DenseMatrix& C = Element().Material().GetMaterialTensor();

  afb::ColumnVector sv1(8), sv2(8), sv3(8), sv4(8);
  afb::ColumnVector *stabilizationVector[4] = {&sv1, &sv2, &sv3, &sv4};
  for (int i = 0; i < 4; i++) // iterate through vectors
  {
    // gamma_i = h_i
    for (int idx = 0; idx < 8; idx++)
    {
      (*stabilizationVector[i])(idx) = hourglassVector_[i][idx];
    }

    // sum [j = 1 : 3]
    for (int j = 0; j < 3; j++)
    {
      // dot product h_i * x_j
      real dotProduct = 0;
      for (int m = 0; m < 8; m++)
      {
        dotProduct += hourglassVector_[i][m] * X(m, j);
      }

      for (int k = 0; k < 8; k++) // iterate through vector indices
      {
        (*stabilizationVector[i])(k) -= dotProduct * B(j, k);
      }
    }

    stabilizationVector[i]->Scale(0.125);
  }

  // Step 2: Calculate hourglass displacements (already in isoparametric
  // domain, Eq. 8)
  afb::ColumnVector v1(8), v2(8), v3(8), v4(8);
  afb::ColumnVector *hourglassDisplacement[4] = {&v1, &v2, &v3, &v4};
  for (int i = 0; i < 4; i++)
  {
    hourglassDisplacement[i]->ClearAll();
    real v_1 = 0, v_2 = 0, v_3 = 0;
    for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
    {
      // in generalized coordinates...
      v_1 += (*stabilizationVector[i])(nodeIdx) * du(3*nodeIdx    );
      v_2 += (*stabilizationVector[i])(nodeIdx) * du(3*nodeIdx + 1);
      v_3 += (*stabilizationVector[i])(nodeIdx) * du(3*nodeIdx + 2);
    }

    // ...now in isoparametric coordinates
    for (int k = 0; k < 3; k++)
    {
      (*hourglassDisplacement[i])(k) = J(0, k)*v_1 + J(1, k)*v_2 + J(2, k)*v_3;
    }
  }

  // Step 3: Calculate hat(J)_0^(-1), which maps strains to isoparametric
  // coordinates (Eq. 3.61 pg. 3.14, LS-DYNA Theory Manual)
  afb::ColumnVector strainJacobian(6);
  real jSquaredNorm[3];
  for (int i = 0; i < 3; i++)
  {
    real squaredNorm = J(0,i)*J(0,i) + J(1,i)*J(1,i) + J(2,i)*J(2,i);
    jSquaredNorm[i] = squaredNorm;
  }
  strainJacobian(0) = 1.0 / jSquaredNorm[0];
  strainJacobian(1) = 1.0 / jSquaredNorm[1];
  strainJacobian(2) = 1.0 / jSquaredNorm[2];
  strainJacobian(3) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[1]);
  strainJacobian(4) = 1.0 / sqrt(jSquaredNorm[2] * jSquaredNorm[1]);
  strainJacobian(5) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[2]);

  // Step 4: Map material stiffness matrix to isoparametric space.
  afb::DenseMatrix Ciso(6, 6);
  for (int i = 0; i < 6; i++)
  {
    for (int j = 0; j < 6; j++)
    {
      real c_ij = strainJacobian(i) * C(i, j) * strainJacobian(j);
      Ciso(i, j) = c_ij;
    }
  }

  // Step 5: Calculate internal degrees of freedom for enhanced strain.
  real H = Ciso(0,0) + Ciso(1,1) + Ciso(2,2) + 2.0*(Ciso(0,1) +
    Ciso(1,2) + Ciso(0,2));
  real da1 = 1.0/Ciso(0,0) * (Ciso(0,1)*v1(1) + Ciso(0,4)*v2(1) +
    Ciso(0,2)*v2(2) + Ciso(0,4)*v1(2));
  real da2 = 1.0/Ciso(1,1) * (Ciso(1,0)*v1(0) + Ciso(1,5)*v3(0) +
    Ciso(1,2)*v3(2));
  real da3 = 1.0/Ciso(2,2) * (Ciso(2,0)*v2(0) + Ciso(2,3)*v3(0) +
    Ciso(2,1)*v3(1) + Ciso(2,3)*v2(1));
  real da4 = v4(2)/H * (Ciso(0,2) + Ciso(1,2) + Ciso(2,2));
  real da5 = v4(0)/H * (Ciso(0,0) + Ciso(1,0) + Ciso(2,0));
  real da6 = v4(1)/H * (Ciso(0,1) + Ciso(1,1) + Ciso(2,1));

  // Step 6: Calculate hourglass force increment
  afb::ColumnVector& df1 = absref<afb::ColumnVector>(hourglassForces_[0]);
  afb::ColumnVector& df2 = absref<afb::ColumnVector>(hourglassForces_[1]);
  afb::ColumnVector& df3 = absref<afb::ColumnVector>(hourglassForces_[2]);
  afb::ColumnVector& df4 = absref<afb::ColumnVector>(hourglassForces_[3]);
  real beta1 = 8.0/3.0 * volume_;
  real beta2 = 8.0/9.0 * volume_;
  real df11 = beta1 * (Ciso(0,0)*v1(0) + Ciso(0,2)*v3(2) + Ciso(0,5)*(v3(0) +
    v1(2)) - Ciso(1,0)*da2);
  real df12 = beta1 * (Ciso(1,1)*v1(1) + Ciso(1,2)*v2(2) + Ciso(1,4)*(v2(1) +
    v1(2)) - Ciso(0,1)*da1);
  real df13 = beta1 * (Ciso(4,4)*(v2(1) + v1(2)) + Ciso(5,5)*(v3(0) + v1(2)) +
    Ciso(5,0)*v1(0) + Ciso(4,1)*v1(1)
    + Ciso(4,2)*v2(2) + Ciso(5,2)*v3(2) - Ciso(0,4)*da1);
  real df21 = beta1 * (Ciso(0,0)*v2(0) + Ciso(0,1)*v3(1) + Ciso(0,3)*(v3(0) +
    v2(1)) - Ciso(2,0)*da3);
  real df22 = beta1 * (Ciso(3,3)*(v3(0) + v2(1)) + Ciso(4,4)*(v2(1) + v1(2)) +
    Ciso(3,0)*v2(0) + Ciso(3,1)*v3(1)
    + Ciso(4,1)*v1(1) + Ciso(4,2)*v2(2) - Ciso(0,4)*da1 - Ciso(2,3)*da3);
  real df23 = beta1 * (Ciso(2,1)*v1(1) + Ciso(2,2)*v2(2) + Ciso(2,4)*(v2(1) +
    v1(2)) - Ciso(0,2)*da1);
  real df31 = beta1 * (Ciso(3,3)*(v3(0) + v2(1)) + Ciso(5,5)*(v3(0) + v1(2)) +
    Ciso(3,0)*v2(0) + Ciso(3,1)*v3(1)
    + Ciso(5,0)*v1(0) + Ciso(5,2)*v3(2) - Ciso(1,5)*da2 - Ciso(2,3)*da3);
  real df32 = beta1 * (Ciso(1,0)*v2(0) + Ciso(1,1)*v3(1) + Ciso(1,3)*(v3(0) +
    v2(1)) - Ciso(2,1)*da3);
  real df33 = beta1 * (Ciso(2,0)*v1(0) + Ciso(2,2)*v3(2) + Ciso(2,5)*(v3(0) +
    v1(2)) - Ciso(1,2)*da2);
  real df41 = beta2 * (Ciso(0,0)*v4(0) - (Ciso(0,0) + Ciso(1,0) + Ciso(2,0))*da5);
  real df42 = beta2 * (Ciso(1,1)*v4(1) - (Ciso(0,1) + Ciso(1,1) + Ciso(2,1))*da6);
  real df43 = beta2 * (Ciso(2,2)*v4(2) - (Ciso(0,2) + Ciso(1,2) + Ciso(2,2))*da4);
  df1(0) += df11; df1(1) += df12; df1(2) += df13;
  df2(0) += df21; df2(1) += df22; df2(2) += df23;
  df3(0) += df31; df3(1) += df32; df3(2) += df33;
  df4(0) += df41; df4(1) += df42; df4(2) += df43;

  // Step 7: Calculate stabilization forces (fStab).
  afb::ColumnVector fStab(24);
  fStab.ClearAll();
  for (int i = 0; i < 4; i++)
  {
    const afb::ColumnVector& fi = absref<afb::ColumnVector>(hourglassForces_[i]);
    for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
    {
      real gamma = (*stabilizationVector[i])(nodeIdx);
      real fs1 = gamma * (J(0,0)*fi(0) + J(0,1)*fi(1) + J(0,2)*fi(2));
      real fs2 = gamma * (J(1,0)*fi(0) + J(1,1)*fi(1) + J(1,2)*fi(2));
      real fs3 = gamma * (J(2,0)*fi(0) + J(2,1)*fi(1) + J(2,2)*fi(2));

      fStab(3*nodeIdx    ) += fs1;
      fStab(3*nodeIdx + 1) += fs2;
      fStab(3*nodeIdx + 2) += fs3;
    }
  }

  // Calculate hourglass energy increment
  real hourglassEnergyIncrement = 0;
  for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
  {
    real gamma1 = (*stabilizationVector[0])(nodeIdx);
    real gamma2 = (*stabilizationVector[1])(nodeIdx);
    real gamma3 = (*stabilizationVector[2])(nodeIdx);
    real gamma4 = (*stabilizationVector[3])(nodeIdx);
    real dfs1 = gamma1 * (J(0,0)*df11 + J(0,1)*df12 + J(0,2)*df13)
              + gamma2 * (J(0,0)*df21 + J(0,1)*df22 + J(0,2)*df23)
              + gamma3 * (J(0,0)*df31 + J(0,1)*df32 + J(0,2)*df33)
              + gamma4 * (J(0,0)*df41 + J(0,1)*df42 + J(0,2)*df43);
    real dfs2 = gamma1 * (J(1,0)*df11 + J(1,1)*df12 + J(1,2)*df13)
              + gamma2 * (J(1,0)*df21 + J(1,1)*df22 + J(1,2)*df23)
              + gamma3 * (J(1,0)*df31 + J(1,1)*df32 + J(1,2)*df33)
              + gamma4 * (J(1,0)*df41 + J(1,1)*df42 + J(1,2)*df43);
    real dfs3 = gamma1 * (J(2,0)*df11 + J(2,1)*df12 + J(2,2)*df13)
              + gamma2 * (J(2,0)*df21 + J(2,1)*df22 + J(2,2)*df23)
              + gamma3 * (J(2,0)*df31 + J(2,1)*df32 + J(2,2)*df33)
              + gamma4 * (J(2,0)*df41 + J(2,1)*df42 + J(2,2)*df43);
    hourglassEnergyIncrement += dfs1*du(3*nodeIdx) + dfs2*du(3*nodeIdx + 1) +
      dfs3*du(3*nodeIdx + 2);
  }
  hourglassEnergy_ += hourglassEnergyIncrement;

  // Step 8: Stabilize internal force
  afb::VectorSum(internalForce, 1.0, internalForce, 1.0, fStab);
}

afu::Uuid adf::LinearHexahedralPusoFormulation::GetTypeId( void ) const
{
  // 993F06D4-A4B7-4B49-B5D3-42DA6FE86AE3
  int bytes[16] = {0x99, 0x3F, 0x06, 0xD4, 0xA4, 0xB7, 0x4B, 0x49,
                   0xB5, 0xD3, 0x42, 0xDA, 0x6F, 0xE8, 0x6A, 0xE3};
  return afu::Uuid(bytes);
}

// void hexa::StabilizeInternalForce_org( afb::Vector& internalForce,
//                                    const afb::Vector& displacementIncrement )
// {
//   id_type id = Element().GetInternalId();
//
//   // These are some shorthands that we are going to use
//   const afb::Matrix& J = absref<afb::Matrix>(jacobian_);
//   const afb::Matrix& B = absref<afb::Matrix>(Bmatrix_);
//   const afb::Vector& du = displacementIncrement;
//   afb::Matrix& X = absref<afb::Matrix>(nodeCoordinates_);
//   const afb::Matrix& C = Element().Material().GetMaterialTensor();
//
//   // Step 1: Calculate stabilization vectors gamma_i (Eq. 11)
//   real stabilizationVector[3][4][8];
//   for (int dofIdx = 0; dofIdx < 3; dofIdx++)
//   {
//     for (int modeIdx = 0; modeIdx < 4; modeIdx++) // iterate through vectors
//     {
//       real *gamma = stabilizationVector[dofIdx][modeIdx];
//       const real *h = hourglassVector_[modeIdx];
//       real dotProduct = 0;
//       for (int m = 0; m < 8; m++) // dot product h_i * x_j
//       {
//         dotProduct += h[m] * X(m, dofIdx);
//       }
//       for (int k = 0; k < 8; k++) // iterate through vector indices
//       {
//         gamma[k] = 0.125 * (h[k] - dotProduct*B(dofIdx, k));
//       }
//     }
//   }
//
//   // Step 2: Calculate hourglass displacements (already in isoparametric
//   // domain, Eq. 8)
//   real hourglassDisplacement[4][3];
//   for (int i = 0; i < 4; i++)
//   {
//     real v_1 = 0, v_2 = 0, v_3 = 0;
//     for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
//     {
//       // in generalized coordinates...
//       v_1 += stabilizationVector[0][i][nodeIdx] * du(3*nodeIdx    );
//       v_2 += stabilizationVector[1][i][nodeIdx] * du(3*nodeIdx + 1);
//       v_3 += stabilizationVector[2][i][nodeIdx] * du(3*nodeIdx + 2);
//     }
//
//     // ...now in isoparametric coordinates
//     for (int k = 0; k < 3; k++)
//     {
//       hourglassDisplacement[k][i] = J(0, k)*v_1 + J(1, k)*v_2 + J(2, k)*v_3;
//     }
//   }
//
//   // Step 3: Calculate hat(J)_0^(-1), which maps strains to isoparametric
//   // coordinates (Eq. 3.61 pg. 3.14, LS-DYNA Theory Manual)
//   afb::ColumnVector strainJacobian(6);
//   real jSquaredNorm[3];
//   for (int i = 0; i < 3; i++)
//   {
//     real squaredNorm = J(0,i)*J(0,i) + J(1,i)*J(1,i) + J(2,i)*J(2,i);
//     jSquaredNorm[i] = squaredNorm;
//   }
//   strainJacobian(0) = 1.0 / jSquaredNorm[0];
//   strainJacobian(1) = 1.0 / jSquaredNorm[1];
//   strainJacobian(2) = 1.0 / jSquaredNorm[2];
//   strainJacobian(3) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[1]);
//   strainJacobian(4) = 1.0 / sqrt(jSquaredNorm[2] * jSquaredNorm[1]);
//   strainJacobian(5) = 1.0 / sqrt(jSquaredNorm[0] * jSquaredNorm[2]);
//
//   // Step 4: Map material stiffness matrix to isoparametric space.
//   afb::DenseMatrix Ciso(6, 6);
//   for (int i = 0; i < 6; i++)
//   {
//     for (int j = 0; j < 6; j++)
//     {
//       real c_ij = strainJacobian(i) * C(i, j) * strainJacobian(j);
//       Ciso(i, j) = c_ij;
//     }
//   }
//
//   // Step 5: Calculate internal degrees of freedom for enhanced strain.
//   real *v1 = hourglassDisplacement[0];
//   real *v2 = hourglassDisplacement[1];
//   real *v3 = hourglassDisplacement[2];
//   real *v4 = hourglassDisplacement[3];
//   real H = Ciso(0,0) + Ciso(1,1) + Ciso(2,2) + 2.0*(Ciso(0,1) + Ciso(1,2) + Ciso(0,2));
//   real da1 = 1.0/Ciso(0,0) * (Ciso(0,1)*v1[1] + Ciso(0,4)*v2[1] + Ciso(0,2)*v2[2] + Ciso(0,4)*v1[2]);
//   real da2 = 1.0/Ciso(1,1) * (Ciso(1,0)*v1[0] + Ciso(1,5)*v3[0] + Ciso(1,2)*v3[2]);
//   real da3 = 1.0/Ciso(2,2) * (Ciso(2,0)*v2[0] + Ciso(2,3)*v3[0] + Ciso(2,1)*v3[1] + Ciso(2,3)*v2[1]);
//   real da4 = v4[2]/H * (Ciso(0,2) + Ciso(1,2) + Ciso(2,2));
//   real da5 = v4[0]/H * (Ciso(0,0) + Ciso(1,0) + Ciso(2,0));
//   real da6 = v4[1]/H * (Ciso(0,1) + Ciso(1,1) + Ciso(2,1));
//
//   // Step 6: Calculate hourglass force increment
//   afb::Vector& df1 = absref<afb::Vector>(hourglassForces_[0]);
//   afb::Vector& df2 = absref<afb::Vector>(hourglassForces_[1]);
//   afb::Vector& df3 = absref<afb::Vector>(hourglassForces_[2]);
//   afb::Vector& df4 = absref<afb::Vector>(hourglassForces_[3]);
//   real beta1 = 8.0/3.0 * volume_;
//   real beta2 = 8.0/9.0 * volume_;
//   real df11 = beta1 * (Ciso(0,0)*v1[0] + Ciso(0,2)*v3[2] + Ciso(0,5)*(v3[0] + v1[2]) - Ciso(1,0)*da2);
//   real df12 = beta1 * (Ciso(1,1)*v1[1] + Ciso(1,2)*v2[2] + Ciso(1,4)*(v2[1] + v1[2]) - Ciso(0,1)*da1);
//   real df13 = beta1 * (Ciso(4,4)*(v2[1] + v1[2]) + Ciso(5,5)*(v3[0] + v1[2]) + Ciso(5,0)*v1[0] + Ciso(4,1)*v1[1]
//               + Ciso(4,2)*v2[2] + Ciso(5,2)*v3[2] - Ciso(0,4)*da1);
//   real df21 = beta1 * (Ciso(0,0)*v2[0] + Ciso(0,1)*v3[1] + Ciso(0,3)*(v3[0] + v2[1]) - Ciso(2,0)*da3);
//   real df22 = beta1 * (Ciso(3,3)*(v3[0] + v2[1]) + Ciso(4,4)*(v2[1] + v1[2]) + Ciso(3,0)*v2[0] + Ciso(3,1)*v3[1]
//               + Ciso(4,1)*v1[1] + Ciso(4,2)*v2[2] - Ciso(0,4)*da1 - Ciso(2,3)*da3);
//   real df23 = beta1 * (Ciso(2,1)*v1[1] + Ciso(2,2)*v2[2] + Ciso(2,4)*(v2[1] + v1[2]) - Ciso(0,2)*da1);
//   real df31 = beta1 * (Ciso(3,3)*(v3[0] + v2[1]) + Ciso(5,5)*(v3[0] + v1[2]) + Ciso(3,0)*v2[0] + Ciso(3,1)*v3[1]
//               + Ciso(5,0)*v1[0] + Ciso(5,2)*v3[2] - Ciso(1,5)*da2 - Ciso(2,3)*da3);
//   real df32 = beta1 * (Ciso(1,0)*v2[0] + Ciso(1,1)*v3[1] + Ciso(1,3)*(v3[0] + v2[1]) - Ciso(2,1)*da3);
//   real df33 = beta1 * (Ciso(2,0)*v1[0] + Ciso(2,2)*v3[2] + Ciso(2,5)*(v3[0] + v1[2]) - Ciso(1,2)*da2);
//   real df41 = beta2 * (Ciso(0,0)*v4[0] - (Ciso(0,0) + Ciso(1,0) + Ciso(2,0))*da5);
//   real df42 = beta2 * (Ciso(1,1)*v4[1] - (Ciso(0,1) + Ciso(1,1) + Ciso(2,1))*da6);
//   real df43 = beta2 * (Ciso(2,2)*v4[2] - (Ciso(0,2) + Ciso(1,2) + Ciso(2,2))*da4);
//   df1(0) += df11; df1(1) += df12; df1(2) += df13;
//   df2(0) += df21; df2(1) += df22; df2(2) += df23;
//   df3(0) += df31; df3(1) += df32; df3(2) += df33;
//   df4(0) += df41; df4(1) += df42; df4(2) += df43;
//
//   // Step 7: Calculate stabilization forces (fStab).
//   afb::ColumnVector fStab(24);
//   afb::ColumnVector dFstab(24);
//   fStab.ClearAll();
//   for (int i = 0; i < 4; i++)
//   {
//     const afb::Vector& fi = absref<afb::Vector>(hourglassForces_[i]);
//     for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
//     {
//       real gammaX = stabilizationVector[0][i][nodeIdx];
//       real gammaY = stabilizationVector[1][i][nodeIdx];
//       real gammaZ = stabilizationVector[2][i][nodeIdx];
//       real fs1 = gammaX * (J(0,0)*fi(0) + J(0,1)*fi(1) + J(0,2)*fi(2));
//       real fs2 = gammaY * (J(1,0)*fi(0) + J(1,1)*fi(1) + J(1,2)*fi(2));
//       real fs3 = gammaZ * (J(2,0)*fi(0) + J(2,1)*fi(1) + J(2,2)*fi(2));
//
//       fStab(3*nodeIdx    ) += fs1;
//       fStab(3*nodeIdx + 1) += fs2;
//       fStab(3*nodeIdx + 2) += fs3;
//     }
//   }
//
//   // Calculate hourglass energy increment
//   real hourglassEnergyIncrement = 0;
//   for (int nodeIdx = 0; nodeIdx < 8; nodeIdx++)
//   {
//     real gammaX1 = stabilizationVector[0][0][nodeIdx];
//     real gammaY1 = stabilizationVector[1][0][nodeIdx];
//     real gammaZ1 = stabilizationVector[2][0][nodeIdx];
//     real gammaX2 = stabilizationVector[0][1][nodeIdx];
//     real gammaY2 = stabilizationVector[1][1][nodeIdx];
//     real gammaZ2 = stabilizationVector[2][1][nodeIdx];
//     real gammaX3 = stabilizationVector[0][2][nodeIdx];
//     real gammaY3 = stabilizationVector[1][2][nodeIdx];
//     real gammaZ3 = stabilizationVector[2][2][nodeIdx];
//     real gammaX4 = stabilizationVector[0][3][nodeIdx];
//     real gammaY4 = stabilizationVector[1][3][nodeIdx];
//     real gammaZ4 = stabilizationVector[2][3][nodeIdx];
//
//     real dfs1 = gammaX1 * (J(0,0)*df11 + J(0,1)*df12 + J(0,2)*df13)
//               + gammaX2 * (J(0,0)*df21 + J(0,1)*df22 + J(0,2)*df23)
//               + gammaX3 * (J(0,0)*df31 + J(0,1)*df32 + J(0,2)*df33)
//               + gammaX4 * (J(0,0)*df41 + J(0,1)*df42 + J(0,2)*df43);
//     real dfs2 = gammaY1 * (J(1,0)*df11 + J(1,1)*df12 + J(1,2)*df13)
//               + gammaY2 * (J(1,0)*df21 + J(1,1)*df22 + J(1,2)*df23)
//               + gammaY3 * (J(1,0)*df31 + J(1,1)*df32 + J(1,2)*df33)
//               + gammaY4 * (J(1,0)*df41 + J(1,1)*df42 + J(1,2)*df43);
//     real dfs3 = gammaZ1 * (J(2,0)*df11 + J(2,1)*df12 + J(2,2)*df13)
//               + gammaZ2 * (J(2,0)*df21 + J(2,1)*df22 + J(2,2)*df23)
//               + gammaZ3 * (J(2,0)*df31 + J(2,1)*df32 + J(2,2)*df33)
//               + gammaZ4 * (J(2,0)*df41 + J(2,1)*df42 + J(2,2)*df43);
//
//     hourglassEnergyIncrement += dfs1*du(3*nodeIdx) + dfs2*du(3*nodeIdx + 1) + dfs3*du(3*nodeIdx + 2);
//   }
//   hourglassEnergy_ += hourglassEnergyIncrement;
//
//   // Step 8: Stabilize internal force
//   afb::VectorAlgebra::Sum(internalForce, 1.0, internalForce, 1.0, fStab);
// }


/**

Copyright (c) 2016, Aumann Florian, Heller Florian, Meißner Pascal
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "Transformation/transformationfile_manager_XML.h"
#include <iostream>
#include <fstream>
#include <string>
#include <ros/ros.h>
#ifndef Q_MOC_RUN
#include <boost/foreach.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#endif

using namespace std;

TransformationFile_Manager_XML::TransformationFile_Manager_XML (string filePath) : Abstract_TransformationFile_Manager(filePath) {}

bool TransformationFile_Manager_XML::writeToFile(Transformation_Data data)
{
    boost::property_tree::ptree pt;
    ifstream infile(this->filePath);
    if (infile.good()) boost::property_tree::read_xml(this->filePath, pt); //If file exists, append new data

    boost::property_tree::ptree dataNode;
    boost::property_tree::ptree transformLaserScan_Node;
    boost::property_tree::ptree transformPTU_Node;

    dataNode.put<double>("<xmlattr>.Pan", data.pan);
    dataNode.put<double>("<xmlattr>.Tilt", data.tilt);

    for (unsigned int i = 0; i < 4; i++)
    {
        for (unsigned int j = 0; j < 4; j++)
        {
            std::string name = "<xmlattr>.x_" + std::to_string(i) + std::to_string(j);
            transformLaserScan_Node.put<double>(name, data.LaserScan_Frame(i,j));
            transformPTU_Node.put<double>(name, data.PTU_Frame(i,j));
        }
    }
    dataNode.add_child("FrameLaserscan", transformLaserScan_Node);
    dataNode.add_child("FramePTU", transformPTU_Node);
    pt.add_child("TransformationData.DataSet", dataNode);
    //boost::property_tree::xml_writer_settings<char> settings('\t', 1);
    boost::property_tree::write_xml(this->filePath, pt,std::locale());
    return true;
}

std::vector<Transformation_Data> TransformationFile_Manager_XML::readFromFile(string filePath)
{
    std::vector<Transformation_Data> output;
    boost::property_tree::ptree pt;
    //open the archive
    read_xml(filePath, pt);

    BOOST_FOREACH(boost::property_tree::ptree::value_type &node, pt.get_child("TransformationData"))
    {
        if (node.first == "DataSet")
        {
            Transformation_Data data;
            data.pan = node.second.get<double>("<xmlattr>.Pan");
            data.tilt = node.second.get<double>("<xmlattr>.Tilt");
            boost::property_tree::ptree transformLaserScan_Node = node.second.get_child("FrameLaserscan");
            boost::property_tree::ptree transformPTU_Node = node.second.get_child("FramePTU");
            for (unsigned int i = 0; i < 4; i++)
            {
                for (unsigned int j = 0; j < 4; j++)
                {
                    std::string name = "<xmlattr>.x_" + std::to_string(i) + std::to_string(j);
                    data.LaserScan_Frame(i, j) = transformLaserScan_Node.get<double>(name);
                    data.PTU_Frame(i, j) = transformPTU_Node.get<double>(name);
                }
            }
            output.push_back(data);
        }
    }
    return output;
}

std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> TransformationFile_Manager_XML::readTransformationFromFile(string filePath)
{
    std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> re;
    return re;
}

bool TransformationFile_Manager_XML::writeTransformationToFile(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> transformation)
{
    return false;
}

bool TransformationFile_Manager_XML::writeCameraOffsetToFile(const PanTiltOffsetTupleList &offsetData)
{
    return true;
}


#pragma once

//          Copyright Rein Halbersma 2014-2021.
// 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)

#include <iostream.hpp>                 // operator<<, operator>>
#include <traits.hpp>                   // has_forward_iterator, has_hinted_insert, has_resize
#include <boost/test/unit_test.hpp>     // BOOST_CHECK, BOOST_CHECK_EQUAL, BOOST_CHECK_EQUAL_COLLECTIONS, BOOST_CHECK_LE, BOOST_CHECK_NE, BOOST_CHECK_THROW
#include <algorithm>                    // copy_if, equal, find, includes, lexicographical_compare,
                                        // set_difference, set_intersection, set_symmetric_difference, set_union, transform
#include <cstddef>                      // ptrdiff_t, size_t
#include <iterator>                     // distance, inserter
#include <memory>                       // addressof
#include <ranges>                       // filter, transform
#include <set>                          // set
#include <sstream>                      // stringstream
#include <stdexcept>                    // out_of_range

namespace xstd {

template<class BitSet>
struct constructor
{
        auto operator()() const noexcept
        {
                BitSet bs;
                BOOST_CHECK(none(bs));                                          // [bitset.cons]/1
        }

        // [bitset.cons]/2-7 describe constructors taking unsigned long long, basic_string and char const*
};

struct op_bitand_assign
{
        auto operator()(auto& lhs, auto const& rhs) const noexcept
        {
                BOOST_CHECK_EQUAL(fn_size(lhs), fn_size(rhs));
                auto const src = lhs;
                auto const& dst = lhs &= rhs;

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/1
                        BOOST_CHECK_EQUAL(at(dst, i), !at(rhs, i) ? false : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(lhs));    // [bitset.members]/2
        }
};

struct op_bitor_assign
{
        auto operator()(auto& lhs, auto const& rhs) const noexcept
        {
                BOOST_CHECK_EQUAL(fn_size(lhs), fn_size(rhs));
                auto const src = lhs;
                auto const& dst = lhs |= rhs;

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/3
                        BOOST_CHECK_EQUAL(at(dst, i), at(rhs, i) ? true : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(lhs));    // [bitset.members]/4
        }
};

struct op_xor_assign
{
        auto operator()(auto& lhs, auto const& rhs) const noexcept
        {
                BOOST_CHECK_EQUAL(fn_size(lhs), fn_size(rhs));
                auto const src = lhs;
                auto const& dst = lhs ^= rhs;

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/5
                        BOOST_CHECK_EQUAL(at(dst, i), at(rhs, i) ? !at(src, i) : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(lhs));    // [bitset.members]/6
        }
};

struct op_minus_assign
{
        auto operator()(auto& lhs, auto const& rhs) const noexcept
        {
                BOOST_CHECK_EQUAL(fn_size(lhs), fn_size(rhs));
                auto const src = lhs;
                auto const& dst = lhs -= rhs;

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                        BOOST_CHECK_EQUAL(at(dst, i), at(rhs, i) ? false : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(lhs));
        }
};

struct op_shift_left_assign
{
        auto operator()(auto& bs) const
        {
                auto N = fn_size(bs);
                left_shift_assign(bs, N);                                       // [bitset.members]/7.1
                BOOST_CHECK(none(bs));
        }

        auto operator()(auto& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto const src = bs;
                auto const& dst = left_shift_assign(bs, pos);

                for (auto N = fn_size(src), I = decltype(N){0}; I < N; ++I) {
                        if (I < pos) {
                                BOOST_CHECK(!at(dst, I));                       // [bitset.members]/7.1
                        } else {                                                // [bitset.members]/7.2
                                BOOST_CHECK_EQUAL(at(dst, I), at(src, I - pos));
                        }
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/8
        }
};

struct op_shift_right_assign
{
        auto operator()(auto& bs) const
        {
                auto N = fn_size(bs);
                right_shift_assign(bs, N);                                      // [bitset.members]/9.1
                BOOST_CHECK(none(bs));
        }

        auto operator()(auto& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto const src = bs;
                auto const& dst = right_shift_assign(bs, pos);

                for (auto N = fn_size(src), I = decltype(N){0}; I < N; ++I) {
                        if (pos >= N - I) {
                                BOOST_CHECK(!at(dst, I));                       // [bitset.members]/9.1
                        } else {                                                // [bitset.members]/9.2
                                BOOST_CHECK_EQUAL(at(dst, I), at(src, I + pos));
                        }
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/10
        }
};

struct mem_set
{
        auto operator()(auto& bs) const noexcept
        {
                auto const& dst = set(bs);

                BOOST_CHECK(all(bs));                                           // [bitset.members]/11
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/12
                BOOST_CHECK_THROW(set(bs, fn_size(bs)), std::out_of_range);     // [bitset.members]/13
        }

        auto operator()(auto& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto const src = bs;
                auto const& dst = set(bs, pos);

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/14
                        BOOST_CHECK_EQUAL(at(dst, i), i == pos ? true : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/15
        }

        auto operator()(auto& bs, std::size_t pos, bool val) const
        {
                auto const src = bs;
                auto const& dst = set(bs, pos, val);

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/14
                        BOOST_CHECK_EQUAL(at(dst, i), i == pos ? val : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/15
        }
};

struct mem_reset
{
        auto operator()(auto& bs) const noexcept
        {
                auto const& dst = reset(bs);

                BOOST_CHECK(none(bs));                                          // [bitset.members]/16
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/17
                BOOST_CHECK_THROW(reset(bs, fn_size(bs)), std::out_of_range);   // [bitset.members]/18
        }

        auto operator()(auto& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto const src = bs;
                auto const& dst = reset(bs, pos);

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/19
                        BOOST_CHECK_EQUAL(at(dst, i), i == pos ? false : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/20
        }
};

struct op_compl
{
        auto operator()(auto const& a) const noexcept
        {
                auto expected = a; flip(expected);
                auto const& ret = ~a;

                BOOST_CHECK_NE(std::addressof(ret), std::addressof(expected));  // [bitset.members]/21
                BOOST_CHECK_EQUAL(ret, expected);                               // [bitset.members]/22
                BOOST_CHECK_EQUAL(~ret, a);                                     // involution
        }

        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(~(a | b), (~a & ~b));                         // De Morgan's Laws
                BOOST_CHECK_EQUAL(~(a & b), (~a | ~b));                         // De Morgan's Laws
        }
};

struct mem_flip
{
        auto operator()(auto& bs) const noexcept
        {
                auto const src = bs;
                auto const& dst = flip(bs);

                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                        BOOST_CHECK_NE(at(dst, i), at(src, i));                 // [bitset.members]/23
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/24

                flip(bs);
                BOOST_CHECK_EQUAL(bs, src);                                     // involution
                BOOST_CHECK_THROW(flip(bs, fn_size(bs)), std::out_of_range);    // [bitset.members]/25
        }

        auto operator()(auto& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto const src = bs;
                auto const& dst = flip(bs, pos);
                for (auto N = fn_size(src), i = decltype(N){0}; i < N; ++i) {
                                                                                // [bitset.members]/26
                        BOOST_CHECK_EQUAL(at(dst, i), i == pos ? !at(src, i) : at(src, i));
                }
                BOOST_CHECK_EQUAL(std::addressof(dst), std::addressof(bs));     // [bitset.members]/27
        }
};

// [bitset.members]/28-33 describe to_ulong, to_ullong, to_string

struct mem_count
{
        template<class BitSet>
        auto operator()(BitSet const& bs) const noexcept
        {
                auto expected = decltype(count(bs)){0};
                for (auto N = fn_size(bs), i = decltype(N){0}; i < N; ++i) {
                        expected += at(bs, i);
                }
                BOOST_CHECK_EQUAL(count(bs), expected);                         // [bitset.members]/34

                if constexpr (tti::has_forward_iterator<BitSet>) {
                        BOOST_CHECK_EQUAL(static_cast<std::ptrdiff_t>(count(bs)), std::distance(begin(bs), end(bs)));
                }
        }
};

struct mem_size
{
        template<class BitSet>
        auto operator()(BitSet const& bs [[maybe_unused]]) const noexcept
        {
                if constexpr (!tti::has_resize<BitSet>) {
                        BOOST_CHECK_EQUAL(fn_size(bs), fn_size(BitSet()));      // [bitset.members]/35
                }
        }
};

struct op_equal_to
{
        template<class BitSet>
        auto operator()(BitSet const& a, BitSet const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(fn_size(a), fn_size(b));
                auto expected = true;
                for (auto N = fn_size(a), i = decltype(N){0}; i < N; ++i) {
                        expected &= at(a, i) == at(b, i);
                }
                BOOST_CHECK_EQUAL(a == b, expected);                            // [bitset.members]/36

                if constexpr (tti::has_forward_iterator<BitSet>) {
                        BOOST_CHECK_EQUAL(a == b, std::ranges::equal(a, b));
                }
        }
};

struct op_not_equal_to
{
        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(a != b, !(a == b));                           // [bitset.members]/37
        }
};

struct mem_test
{
        auto operator()(auto const& bs) const noexcept
        {
                BOOST_CHECK_THROW(test(bs, fn_size(bs)), std::out_of_range);    // [bitset.members]/38
        }

        template<class BitSet>
        auto operator()(BitSet const& bs, std::size_t pos) const noexcept
        {
                assert(pos < fn_size(bs));
                auto value = bs; reset(value); set(value, pos);
                for (auto N = fn_size(bs), i = decltype(N){0}; i < N; ++i) {
                        BOOST_CHECK_EQUAL(test(value, i), i == pos);            // [bitset.members]/39
                }

                if constexpr (tti::has_forward_iterator<BitSet>) {
                        using value_type = typename decltype(begin(bs))::value_type;
                        BOOST_CHECK_EQUAL(test(bs, pos), std::ranges::find(bs, static_cast<value_type>(pos)) != end(bs));
                }
        }
};

struct mem_all
{
        auto operator()(auto const& bs) const noexcept
        {
                BOOST_CHECK_EQUAL(all(bs), count(bs) == fn_size(bs));           // [bitset.members]/40
        }
};

struct mem_any
{
        auto operator()(auto const& bs) const noexcept
        {
                BOOST_CHECK_EQUAL(any(bs), count(bs) != 0);                     // [bitset.members]/41
        }
};

struct mem_none
{
        auto operator()(auto const& bs) const noexcept
        {
                BOOST_CHECK_EQUAL(none(bs), count(bs) == 0);                    // [bitset.members]/42
        }
};

struct op_shift_left
{
        auto operator()(auto const& bs) const
        {
                auto N = fn_size(bs);
                BOOST_CHECK(none(left_shift(bs, N)));                           // [bitset.members]/43
        }

        template<class BitSet>
        auto operator()(BitSet const& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto expected = bs; left_shift_assign(expected, pos);
                BOOST_CHECK_EQUAL(left_shift(bs, pos), expected);               // [bitset.members]/43

                if constexpr (tti::has_hinted_insert<BitSet>) {
                        auto const lhs = left_shift(bs, pos);
                        BitSet rhs;
                        std::ranges::copy(
                                bs |
                                std::ranges::views::transform([=](auto x) {
                                        return x + static_cast<value_t<BitSet>>(pos);
                                }) |
                                std::ranges::views::filter([&](auto x) {
                                        return x < static_cast<value_t<BitSet>>(fn_size(bs));
                                }),
                                std::inserter(rhs, rhs.end())
                        );
                        BOOST_CHECK_EQUAL_COLLECTIONS(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
                }
        }
};

struct op_shift_right
{
        auto operator()(auto const& bs) const
        {
                auto N = fn_size(bs);
                BOOST_CHECK(none(right_shift(bs, N)));                          // [bitset.members]/44
        }

        template<class BitSet>
        auto operator()(BitSet const& bs, std::size_t pos) const
        {
                assert(pos < fn_size(bs));
                auto expected = bs; right_shift_assign(expected, pos);
                BOOST_CHECK_EQUAL(right_shift(bs, pos), expected);              // [bitset.members]/44

                if constexpr (tti::has_hinted_insert<BitSet>) {
                        auto const lhs = right_shift(bs, pos);
                        BitSet rhs;
                        std::ranges::copy(
                                bs |
                                std::ranges::views::transform([=](auto x) {
                                        return x - static_cast<value_t<BitSet>>(pos);
                                }) |
                                std::ranges::views::filter([](auto x) {
                                        return 0 <= x;
                                }),
                                std::inserter(rhs, rhs.end())
                        );
                        BOOST_CHECK_EQUAL_COLLECTIONS(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
                }
        }
};

struct op_at
{
        auto operator()(auto const& bs, std::size_t pos) const
        {
                BOOST_CHECK(0 <= pos && pos < fn_size(bs));                     // [bitset.members]/45
                BOOST_CHECK_EQUAL(at(bs, pos), test(bs, pos));                  // [bitset.members]/46
                                                                                // [bitset.members]/47: at(bs, fn_size(bs)) does not throw
        }

        auto operator()(auto& bs, std::size_t pos) const
        {
                BOOST_CHECK(0 <= pos && pos < fn_size(bs));                     // [bitset.members]/48
                BOOST_CHECK_EQUAL(at(bs, pos), test(bs, pos));                  // [bitset.members]/49
                                                                                // [bitset.members]/50: at(bs, fn_size(bs), val) does not throw
        }

        auto operator()(auto& bs, std::size_t pos, bool val) const
        {
                BOOST_CHECK(0 <= pos && pos < fn_size(bs));                     // [bitset.members]/48
                auto src = bs; set(src, pos, val);
                at(bs, pos, val);
                BOOST_CHECK_EQUAL(bs, src);                                     // [bitset.members]/49
                                                                                // [bitset.members]/50: at(bs, fn_size(bs), val) does not throw
        }
};

// [bitset.hash]/1 stipulates a std::hash<std::bitset<N>> specialization

struct op_bitand
{
        auto operator()(auto const& a) const noexcept
        {
                BOOST_CHECK_EQUAL((a & a), a);                                  // idempotent
        }

        template<class BitSet>
        auto operator()(BitSet const& a, BitSet const& b) const noexcept
        {
                auto expected = a; expected &= b;
                BOOST_CHECK_EQUAL((a & b), expected);                           // [bitset.operators]/1
                BOOST_CHECK_EQUAL((a & b), (b & a));                            // commutative
                BOOST_CHECK(is_subset_of(a & b, a));
                BOOST_CHECK(is_subset_of(a & b, b));
                BOOST_CHECK_LE(count(a & b), std::min(count(a), count(b)));

                if constexpr (tti::has_hinted_insert<BitSet>) {
                        auto const lhs = a & b;
                        BitSet rhs;
                        std::ranges::set_intersection(a, b, std::inserter(rhs, rhs.end()));
                        BOOST_CHECK_EQUAL(lhs, rhs);
                }
        }

        auto operator()(auto const& a, auto const& b, auto const& c) const noexcept
        {
                BOOST_CHECK_EQUAL(((a & b) & c), (a & (b & c)));                // associative
                BOOST_CHECK_EQUAL((a & (b | c)), ((a & b) | (a & c)));          // distributive
        }
};

struct op_bitor
{
        auto operator()(auto const& a) const noexcept
        {
                BOOST_CHECK_EQUAL((a | a), a);                                  // idempotent
        }

        template<class BitSet>
        auto operator()(BitSet const& a, BitSet const& b) const noexcept
        {
                auto expected = a; expected |= b;
                BOOST_CHECK_EQUAL((a | b), expected);                           // [bitset.operators]/2
                BOOST_CHECK_EQUAL((a | b), (b | a));                            // commutative
                BOOST_CHECK(is_subset_of(a, a | b));
                BOOST_CHECK(is_subset_of(b, a | b));
                BOOST_CHECK_EQUAL(count(a | b), count(a) + count(b) - count(a & b));

                if constexpr (tti::has_hinted_insert<BitSet>) {
                        auto const lhs = a | b;
                        BitSet rhs;
                        std::ranges::set_union(a, b, std::inserter(rhs, rhs.end()));
                        BOOST_CHECK_EQUAL(lhs,rhs);
                }
        }

        auto operator()(auto const& a, auto const& b, auto const& c) const noexcept
        {
                BOOST_CHECK_EQUAL(((a | b) | c), (a | (b | c)));                // associative
                BOOST_CHECK_EQUAL((a | (b & c)), ((a | b) & (a | c)));          // distributive
        }
};

struct op_xor
{
        auto operator()(auto const& a) const noexcept
        {
                BOOST_CHECK(none(a ^ a));                                       // nilpotent
        }

        template<class BitSet>
        auto operator()(BitSet const& a, BitSet const& b) const noexcept
        {
                auto expected = a; expected ^= b;
                BOOST_CHECK_EQUAL((a ^ b), expected);                           // [bitset.operators]/3
                BOOST_CHECK_EQUAL((a ^ b), (b ^ a));                            // commutative
                BOOST_CHECK_EQUAL((a ^ b), ((a - b) | (b - a)));
                BOOST_CHECK_EQUAL((a ^ b), (a | b) - (a & b));

                if constexpr (tti::has_hinted_insert<BitSet>) {
                        auto const lhs = a ^ b;
                        BitSet rhs;
                        std::ranges::set_symmetric_difference(a, b, std::inserter(rhs, rhs.end()));
                        BOOST_CHECK_EQUAL(lhs, rhs);
                }
        }

        auto operator()(auto const& a, auto const& b, auto const& c) const noexcept
        {
                BOOST_CHECK_EQUAL(((a ^ b) ^ c), (a ^ (b ^ c)));                // associative
                BOOST_CHECK_EQUAL((a & (b ^ c)), ((a & b) ^ (a & c)));          // distributive
        }
};

struct op_minus
{
        auto operator()(auto const& a) const noexcept
        {
                BOOST_CHECK(none(a - a));                                       // nilpotent
        }

        template<class BitSet>
        auto operator()(BitSet const& a, BitSet const& b) const noexcept
        {
                auto expected = a; expected -= b;
                BOOST_CHECK_EQUAL(a - b, expected);

                BOOST_CHECK_EQUAL(a - b, (a & ~b));
                BOOST_CHECK_EQUAL(~(a - b), (~a | b));
                BOOST_CHECK_EQUAL(~a - ~b, b - a);
                BOOST_CHECK_EQUAL(a - b, (a | b) - b);
                BOOST_CHECK_EQUAL(a - b, a - (a & b));
                BOOST_CHECK(is_subset_of(a - b, a));
                BOOST_CHECK(is_disjoint(a - b, b));

                if constexpr (tti::has_hinted_insert<BitSet>) {
                        auto const lhs = a - b;
                        BitSet rhs;
                        std::ranges::set_difference(a, b, std::inserter(rhs, rhs.end()));
                        BOOST_CHECK_EQUAL(lhs, rhs);
                }
        }
};

struct mem_is_subset_of
{
        auto operator()(auto const& a) const noexcept
        {
                BOOST_CHECK(is_subset_of(a, a));                                // reflexive
        }

        template<class BitSet>
        auto operator()(BitSet const& a, BitSet const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(fn_size(a), fn_size(b));
                auto expected = true;
                for (auto N = fn_size(a), i = decltype(N){0}; i < N; ++i) {
                        expected &= !at(a, i) || at(b, i);
                }
                BOOST_CHECK_EQUAL(is_subset_of(a, b), expected);
                BOOST_CHECK_EQUAL(is_subset_of(a, b), none(a - b));
                BOOST_CHECK_EQUAL(is_subset_of(a, b), (a & b) == a);

                if constexpr (tti::has_forward_iterator<BitSet>) {
                        BOOST_CHECK_EQUAL(is_subset_of(a, b), std::ranges::includes(a, b));
                }
        }

        auto operator()(auto const& a, auto const& b, auto const& c) const noexcept
        {                                                                       // transitive
                BOOST_CHECK(!(is_subset_of(a, b) && is_subset_of(b, c)) || is_subset_of(a, c));
        }
};

struct mem_is_superset_of
{
        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(is_superset_of(a, b), is_subset_of(b, a));
        }
};

struct mem_is_proper_subset_of
{
        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(is_proper_subset_of(a, b), is_subset_of(a, b) && !is_subset_of(b, a));
                BOOST_CHECK_EQUAL(is_proper_subset_of(a, b), is_subset_of(a, b) && a != b);
        }
};

struct mem_is_proper_superset_of
{
        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(is_proper_superset_of(a, b), is_proper_subset_of(b, a));
        }
};

struct mem_intersects
{
        auto operator()(auto const& a) const noexcept
        {
                BOOST_CHECK(intersects(a, a) || none(a));
        }

        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(intersects(a, b), any(a & b));
                BOOST_CHECK_EQUAL(intersects(a, b), intersects(b, a));
        }
};

struct fn_is_disjoint
{
        auto operator()(auto const& a, auto const& b) const noexcept
        {
                BOOST_CHECK_EQUAL(is_disjoint(a, b), none(a & b));
                BOOST_CHECK_EQUAL(is_disjoint(a, b), !intersects(a, b));
        }
};

struct fn_iostream
{
        template<class BitSet>
        auto operator()(BitSet const& a) const noexcept
        {
                std::stringstream sstr;
                BitSet b;
                sstr << a;
                sstr >> b;
                BOOST_CHECK_EQUAL(a, b);                                        // [bitset.operators]/4-8
        }
};

}       // namespace xstd


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of float
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx_
                          , bd::scalar_<bd::unspecified_<Value> >
                          , bd::target_<bs::pack_<bd::single_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_ps( static_cast<float>(v) );
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of double
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx_
                          , bd::scalar_<bd::unspecified_<Value> >
                          , bd::target_<bs::pack_<bd::double_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_pd( static_cast<double>(v) );
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 8 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx_
                          , bd::scalar_<bd::unspecified_<Value>>
                          , bd::target_<bs::pack_<bd::ints8_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi8(v);
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 16 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx_
                          , bd::scalar_<bd::unspecified_<Value>>
                          , bd::target_<bs::pack_<bd::ints16_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi16(v);
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 32 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx_
                          , bd::scalar_<bd::unspecified_<Value>>
                          , bd::target_<bs::pack_<bd::ints32_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi32(v);
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 64 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx_
                          , bd::scalar_<bd::unspecified_<Value>>
                          , bd::target_<bs::pack_<bd::ints64_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi64x(v);
    }
  };
} } }

#endif


// Copyright (c) 2014 The Bitcoin Core developers
<<<<<<< HEAD
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "core.h"
#include "main.h"
#include "uint256.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

/**
 * the maximum block reward at a given height for a block without fees
 */
uint64_t expectedMaxSubsidy(int height) {
    if(height < 100000) {
        return 1000000 * COIN;
    } else if (height < 150000) {
        return 500000 * COIN;
    } else if (height < 200000) {
        return 250000 * COIN;
    } else if (height < 300000) {
        return 125000 * COIN;
    } else if (height < 400000) {
        return  62500 * COIN;
    } else if (height < 500000) {
        return  31250 * COIN;
    } else if (height < 600000) {
        return  15625 * COIN;
    } else {
        return  10000 * COIN;
    }
}

/**
 * the minimum possible value for the maximum block reward at a given height
 * for a block without fees
 */
uint64_t expectedMinSubsidy(int height) {
    if(height < 100000) {
        return 0;
    } else if (height < 150000) {
        return 0;
    } else if (height < 200000) {
        return 250000 * COIN;
    } else if (height < 300000) {
        return 125000 * COIN;
    } else if (height < 400000) {
        return  62500 * COIN;
    } else if (height < 500000) {
        return  31250 * COIN;
    } else if (height < 600000) {
        return  15625 * COIN;
    } else {
        return  10000 * COIN;
    }
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    int nHeight = 0;
    int nStepSize= 1;
    uint256 nSum = 0;

    for (nHeight = 0; nHeight <= 600000; nHeight += nStepSize) {
        uint64_t nSubsidy = GetBlockValue(nHeight, 0, 0);
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK(nSubsidy >= expectedMinSubsidy(nHeight));
        BOOST_CHECK(nSubsidy <= expectedMaxSubsidy(nHeight));

        nSum += nSubsidy * nStepSize;
    }

    //test sum +- ~10billion
    uint256 upperlimit = uint256("95e14ec776380000"); //108 billion doge
    BOOST_CHECK(nSum <= upperlimit);

    uint256 lowerlimit = uint256("7a1fe16027700000"); //88 billion doge
    BOOST_CHECK(nSum >= lowerlimit);

    //test infinitely increasing block rewards
    for (; nHeight < 700000; nHeight += nStepSize) {
        uint64_t nSubsidy = GetBlockValue(nHeight, 0, 0);
        BOOST_CHECK(MoneyRange(nSubsidy));
        BOOST_CHECK(nSubsidy >= expectedMinSubsidy(nHeight));
        BOOST_CHECK(nSubsidy <= expectedMaxSubsidy(nHeight));

        nSum += nSubsidy * nStepSize;
    }

    //make sure that MAX_MONEY is not what some people think it is
    BOOST_CHECK(nSum > MAX_MONEY);

    //test subsidy in 1000 years
    nHeight = 1000 * 365 * 24 * 60;
    uint64_t nSubsidy = GetBlockValue(nHeight, 0, 0);
    BOOST_CHECK(MoneyRange(nSubsidy));
    BOOST_CHECK(nSubsidy >= expectedMinSubsidy(nHeight));
    BOOST_CHECK(nSubsidy <= expectedMaxSubsidy(nHeight));
}

BOOST_AUTO_TEST_CASE(GetMinFee_test)
{
    uint64_t value = 1000 * COIN;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 100, false, GMF_RELAY) == CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 100, false, GMF_SEND) == CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_RELAY) == CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_SEND) == CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_RELAY) == CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_SEND) == CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_RELAY) == 2 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_SEND) == 2 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_RELAY) == 3 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_SEND) == 3 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_RELAY) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_SEND) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_dust_test)
{
    uint64_t value = 1000;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_RELAY) == 2 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_SEND) == 2 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_RELAY) == 2 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_SEND) == 2 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_RELAY) == 3 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_SEND) == 3 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_RELAY) == 4 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_SEND) == 4 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_RELAY) == (2+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_SEND) == (2+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_manydust_test)
{
    uint64_t value = 1000;

    CTransaction tx;
    CTxOut txout1(1000 * COIN, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    for(int i=0; i<100; i++) {
        CTxOut txoutn(value, (CScript)vector<unsigned char>(24, 0));
        tx.vout.push_back(txoutn);
    }

    CTxOut txout101(1000 * COIN, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout101);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_RELAY) == 101 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1, false, GMF_SEND) == 101 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_RELAY) == 101 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 999, false, GMF_SEND) == 101 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_RELAY) == 102 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 1000, false, GMF_SEND) == 102 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_RELAY) == 103 * CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, 2000, false, GMF_SEND) == 103 * CTransaction::nMinTxFee);

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_RELAY) == (101+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, false, GMF_SEND) == (101+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_relayfree_test)
{
    uint64_t value = 1000 * COIN;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 100, true, GMF_RELAY) == 0);
    BOOST_CHECK(GetMinFee(tx, 1000, true, GMF_RELAY) == 0);
    BOOST_CHECK(GetMinFee(tx, 25999, true, GMF_RELAY) == 0);

    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_RELAY) > 0);
    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_RELAY) == GetMinFee(tx, 26000, false, GMF_RELAY));

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, true, GMF_RELAY) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinRelayTxFee);
}

BOOST_AUTO_TEST_CASE(GetMinFee_createNoFree_test)
{
    uint64_t value = 1000 * COIN;

    CTransaction tx;
    CTxOut txout1(value, (CScript)vector<unsigned char>(24, 0));
    tx.vout.push_back(txout1);

    if(CTransaction::nMinTxFee == CTransaction::nMinRelayTxFee)
        CTransaction::nMinTxFee++;

    BOOST_CHECK(GetMinFee(tx, 100, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 100, true, GMF_SEND) == GetMinFee(tx, 100, false, GMF_SEND));
    BOOST_CHECK(GetMinFee(tx, 1000, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 1000, true, GMF_SEND) == GetMinFee(tx, 1000, false, GMF_SEND));
    BOOST_CHECK(GetMinFee(tx, 25999, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 25999, true, GMF_SEND) == GetMinFee(tx, 25999, false, GMF_SEND));

    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_SEND) > 0);
    BOOST_CHECK(GetMinFee(tx, 26000, true, GMF_SEND) == GetMinFee(tx, 26000, false, GMF_SEND));

    BOOST_CHECK(GetMinFee(tx, MAX_STANDARD_TX_SIZE, true, GMF_SEND) == (1+(MAX_STANDARD_TX_SIZE/1000))*CTransaction::nMinTxFee);
=======
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chainparams.h"
#include "main.h"

#include "test/test_bitcoin.h"

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    // tested in dogecoin_tests.cpp
    //int maxHalvings = 64;
    //CAmount nInitialSubsidy = 50 * COIN;

    //CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    //BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    //for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
    //    int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
    //    CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
    //    BOOST_CHECK(nSubsidy <= nInitialSubsidy);
    //    BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
    //    nPreviousSubsidy = nSubsidy;
    //}
    //BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

static void TestBlockSubsidyHalvings(int nSubsidyHalvingInterval)
{
    // tested in dogecoin_tests.cpp
    //Consensus::Params consensusParams;
    //consensusParams.nSubsidyHalvingInterval = nSubsidyHalvingInterval;
    //TestBlockSubsidyHalvings(consensusParams);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    // tested in dogecoin_tests.cpp
    //TestBlockSubsidyHalvings(Params(CBaseChainParams::MAIN).GetConsensus()); // As in main
    //TestBlockSubsidyHalvings(150); // As in regtest
    //TestBlockSubsidyHalvings(1000); // Just another interval
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    // tested in dogecoin_tests.cpp
    //const Consensus::Params& consensusParams = Params(CBaseChainParams::MAIN).GetConsensus();
    //CAmount nSum = 0;
    //for (int nHeight = 0; nHeight < 14000000; nHeight += 1000) {
    //    CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
    //    BOOST_CHECK(nSubsidy <= 50 * COIN);
    //    nSum += nSubsidy * 1000;
    //    BOOST_CHECK(MoneyRange(nSum));
    //}
    //BOOST_CHECK_EQUAL(nSum, 2099999997690000ULL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
>>>>>>> f568462ca04b73485d7e41266a2005155ff69707
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by root on 7/1/21.
//

#include <iostream>
#include <Eigen/Dense>
#include <aikido/constraint/Satisfied.hpp>
#include <aikido/planner/World.hpp>
#include <aikido/rviz/InteractiveMarkerViewer.hpp>
#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
#include <boost/program_options.hpp>
#include <dart/dart.hpp>
#include <dart/utils/urdf/DartLoader.hpp>
#include <Human.hpp>
#include "aikido/trajectory/Trajectory.hpp"
#include "aikido/trajectory/Interpolated.hpp"

#include <aikido/statespace/GeodesicInterpolator.hpp>

namespace po = boost::program_options;

using dart::dynamics::SkeletonPtr;
using dart::dynamics::MetaSkeletonPtr;

using aikido::statespace::dart::MetaSkeletonStateSpace;
using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
using aikido::robot::Robot;

static const std::string topicName("dart_markers");
static const std::string baseFrameName("map");

static const double planningTimeout{5.};

void waitForUser(const std::string &msg) {
    ROS_INFO(msg.c_str());
    std::cin.get();
}

Eigen::VectorXd getCurrentConfig(human::Human &human) {
    using namespace Eigen;
    IOFormat CommaInitFmt(
            StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";");
    // TODO (Tapo): Change this back once the robot vs. arm is cleared
    auto defaultPose = human.getRightArm()->getMetaSkeleton()->getPositions();
    ROS_INFO_STREAM("Current configuration" << defaultPose.format(CommaInitFmt));
    return defaultPose;
}

void executeTraj(
        human::Human &human,
        const MetaSkeletonStateSpacePtr &armSpace,
        const MetaSkeletonPtr &armSkeleton) {
    waitForUser("Plan to move hand. Press [Enter] to proceed.");

    auto satisfied = std::make_shared<aikido::constraint::Satisfied>(armSpace);

    std::shared_ptr<aikido::trajectory::Interpolated> traj;
    std::shared_ptr<aikido::statespace::GeodesicInterpolator> interpolator = std::make_shared<aikido::statespace::GeodesicInterpolator>(
            armSpace);
    traj = std::make_shared<aikido::trajectory::Interpolated>(armSpace, interpolator);

    Eigen::VectorXd positions = human.getRightArm()->getMetaSkeleton()->getPositions();
    Eigen::VectorXd positions2(7);
    Eigen::VectorXd positions3(7);
    Eigen::VectorXd positions4(7);
    Eigen::VectorXd positions5(7);
    Eigen::VectorXd positions6(7);
    positions2 << positions;
    positions3 << positions;
    positions4 << positions;
    positions5 << positions;
    positions6 << positions;
    positions2(0) = positions2(0) + 0.5;
    positions2(2) = positions2(2) + 0.2;

    positions3(0) = positions3(0) + 1;
    positions3(2) = positions3(2) + 0.4;

    positions4(1) = positions4(1) + 0.5;

    positions5(1) = positions5(1) + 0.5;
    positions5(3) = positions5(3) + 0.5;
//  positions4(2) = positions3(2) + 0.4;
//
//  positions5(0) = 3.2;
//  positions5(2) = positions3(2) + 0.4;
//
//  positions6(0) = 3.3;
//  positions6(2) = positions3(2) + 0.4;

    std::cout << positions2;
    std::cout << positions3;
    std::cout << positions4;

    auto state1 = armSpace->createState();
    armSpace->convertPositionsToState(positions, state1);

    auto state2 = armSpace->createState();
    armSpace->convertPositionsToState(positions2, state2);

    auto state3 = armSpace->createState();
    armSpace->convertPositionsToState(positions3, state3);

    auto state4 = armSpace->createState();
    armSpace->convertPositionsToState(positions4, state4);

    auto state5 = armSpace->createState();
    armSpace->convertPositionsToState(positions5, state5);

    auto state6 = armSpace->createState();
    armSpace->convertPositionsToState(positions6, state6);

    auto collision = human.getSelfCollisionConstraint(human.getRightArm()->getStateSpace(),
                                                      human.getRightArm()->getMetaSkeleton());
    aikido::constraint::dart::CollisionFreeOutcome collisionCheckOutcome;
    if (!collision->isSatisfied(state1, &collisionCheckOutcome)) {
        throw std::runtime_error(
                "Robot is in collison at state1: " + collisionCheckOutcome.toString());
    }
    if (!collision->isSatisfied(state2, &collisionCheckOutcome)) {
        throw std::runtime_error(
                "Robot is in collison at state2: " + collisionCheckOutcome.toString());
    }
    if (!collision->isSatisfied(state3, &collisionCheckOutcome)) {
        throw std::runtime_error(
                "Robot is in collison at state3: " + collisionCheckOutcome.toString());
    }

    traj->addWaypoint(0.0, state1);
    traj->addWaypoint(1.0, state2);
    traj->addWaypoint(2.0, state3);
    traj->addWaypoint(3.0, state4);
    traj->addWaypoint(4.0, state5);
//  traj->addWaypoint(5.0, state6);
    ROS_INFO_STREAM("Evaluate the found trajectory at half way");
    auto state = armSpace->createState();
    traj->evaluate(0.5, state);
    //robot.moveArmOnTrajectory(traj,ada::TrajectoryPostprocessType::PARABOLIC_RETIME);
    //auto smoothTrajectory
    //    = robot.smoothPath(armSkeleton, trajectory.get(), satisfied);
    std::cout << traj.get() << std::endl;
//  auto smoothTrajectory
//      = robot.smoothPath(robot.getArm()->getMetaSkeleton(), traj.get(), satisfied);
    //aikido::trajectory::TrajectoryPtr timedTrajectory
    //   = std::move(robot.retimePath(robot.getArm()->getMetaSkeleton(),traj.get()));
//  waitForUser("Press key to move arm to goal");

//  auto smoothTrajectory
//      = human.postProcessPath<aikido::planner::parabolic::ParabolicSmoother>(traj.get(), satisfied,
//                                                                             aikido::planner::parabolic::ParabolicSmoother::Params());
//  aikido::trajectory::TrajectoryPtr timedTrajectory
//      = std::move(robot.postProcessPath<aikido::planner::kunzretimer::KunzRetimer>(smoothTrajectory.get(),
//                                                                                satisfied,
//                                                                                ada::KunzParams()));
    auto future = human.executeTrajectory(std::move(traj));

    future.wait();
    getCurrentConfig(human);
}

int main(int argc, char **argv) {
    bool adaSim = true;

    // Default options for flags
    po::options_description po_desc("simple_human_trajectories options");
    po_desc.add_options()("help", "Produce help message")(
            "adasim,a", po::bool_switch(&adaSim), "Run ADA in sim");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, po_desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << po_desc << std::endl;
        return 0;
    }

    std::cout << "Simulation Mode: " << adaSim << std::endl;

    ROS_INFO("Starting ROS node.");
    ros::init(argc, argv, "simple_trajectories");
    ros::NodeHandle nh("~");

    // Create AIKIDO World
    aikido::planner::WorldPtr env(
            new aikido::planner::World("simple_trajectories"));

    // Load ADA either in simulation or real based on arguments
    ROS_INFO("Loading ADA.");
    std::string modelSrc = "icaros";
    human::Human human(env, adaSim, modelSrc);
    auto robotSkeleton = human.getMetaSkeleton();

    // Start Visualization Topic
    static const std::string execTopicName = topicName + "/simple_trajectories";

    // Start the RViz viewer.
    ROS_INFO_STREAM(
            "Starting viewer. Please subscribe to the '"
                    << execTopicName
                    << "' InteractiveMarker topic in RViz.");
    aikido::rviz::InteractiveMarkerViewer viewer(
            execTopicName, baseFrameName, env);

    auto space = human.getStateSpace();
    auto collision = human.getSelfCollisionConstraint(space, robotSkeleton);

    dart::dynamics::MetaSkeletonPtr metaSkeleton = human.getMetaSkeleton();
    auto metaSpace = std::make_shared<MetaSkeletonStateSpace>(metaSkeleton.get());

    auto armSkeleton = human.getRightArm()->getMetaSkeleton();
    auto armSpace = std::make_shared<MetaSkeletonStateSpace>(armSkeleton.get());

    if (adaSim) {
        Eigen::VectorXd home(Eigen::VectorXd::Zero(7));
//    home[1] = 3.14;
//    home[2] = 3.14;
        armSkeleton->setPositions(home);

        auto startState
                = space->getScopedStateFromMetaSkeleton(robotSkeleton.get());

        aikido::constraint::dart::CollisionFreeOutcome collisionCheckOutcome;
        if (!collision->isSatisfied(startState, &collisionCheckOutcome)) {
            throw std::runtime_error(
                    "Robot is in collison: " + collisionCheckOutcome.toString());
        }
    }

    // Add ADA to the viewer.
    viewer.setAutoUpdate(true);
    waitForUser("You can view ADA in RViz now. \n Press [ENTER] to proceed:");

//  if (!adaSim) {
//    ROS_INFO("Start trajectory executor");
//    robot.startTrajectoryExecutor();
//  }

    executeTraj(human, armSpace, armSkeleton);

    waitForUser("Press [ENTER] to exit. ");
    ros::shutdown();
    return 0;
}


#ifndef OFFVSETACFITTED_HPP
#define OFFVSETACFITTED_HPP

#include <vector>
#include <string>
#include <boost/serialization/list.hpp>
#include <boost/serialization/set.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/filesystem.hpp>
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>

#include "arch/AACAgent.hpp"
#include "bib/Seed.hpp"
#include "bib/Utils.hpp"
#include <bib/MetropolisHasting.hpp>
#include <bib/XMLEngine.hpp>
#include "MLP.hpp"
#include "kde.hpp"
#include "kdtree++/kdtree.hpp"

#define DOUBLE_COMPARE_PRECISION 1e-9

typedef struct _sample {
  std::vector<double> s;
  std::vector<double> pure_a;
  std::vector<double> a;
  std::vector<double> next_s;
  double r;
  bool goal_reached;

  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive& ar, const unsigned int) {
    ar& BOOST_SERIALIZATION_NVP(s);
    ar& BOOST_SERIALIZATION_NVP(pure_a);
    ar& BOOST_SERIALIZATION_NVP(a);
    ar& BOOST_SERIALIZATION_NVP(next_s);
    ar& BOOST_SERIALIZATION_NVP(r);
    ar& BOOST_SERIALIZATION_NVP(goal_reached);
  }

  //Used to store all sample into a tree, might be stochastic
  //only pure_a is negligate
  bool operator< (const _sample& b) const {
    for (uint i = 0; i < s.size(); i++) {
      if(fabs(s[i] - b.s[i])>=DOUBLE_COMPARE_PRECISION)
        return s[i] < b.s[i];
    }

    for (uint i = 0; i < a.size(); i++) {
      if(fabs(a[i] - b.a[i])>=DOUBLE_COMPARE_PRECISION)
        return a[i] < b.a[i];
    }

    for (uint i = 0; i < next_s.size(); i++) {
      if(fabs(next_s[i] - b.next_s[i])>=DOUBLE_COMPARE_PRECISION)
        return next_s[i] < b.next_s[i];
    }

    if(fabs(r - b.r)>=DOUBLE_COMPARE_PRECISION)
        return r < b.r;

    return goal_reached < b.goal_reached;
  }

  typedef double value_type;

  inline double operator[](size_t const N) const{
        return s[N];
  }

  bool same_state(const _sample& b) const{
    for (uint i = 0; i < s.size(); i++) {
      if(fabs(s[i] - b.s[i])>=DOUBLE_COMPARE_PRECISION)
        return false;
    }

    return true;
  }

} sample;

class OffVSetACFitted : public arch::AACAgent<MLP, arch::AgentProgOptions> {
 public:
  typedef MLP PolicyImpl;

  OffVSetACFitted(unsigned int _nb_motors, unsigned int _nb_sensors)
    : arch::AACAgent<MLP, arch::AgentProgOptions>(_nb_motors), nb_sensors(_nb_sensors) {

  }

  virtual ~OffVSetACFitted() {
    delete kdtree_s;

    delete vnn;
    delete ann;
  }

  const std::vector<double>& _run(double reward, const std::vector<double>& sensors,
                                 bool learning, bool goal_reached, bool) {

    vector<double>* next_action = ann->computeOut(sensors);

    if (last_action.get() != nullptr && learning){
      trajectory.insert({last_state, *last_pure_action, *last_action, sensors, reward, goal_reached});
      last_trajectory.insert( {last_state, *last_pure_action, *last_action, sensors, reward, goal_reached});
      proba_s.add_data(last_state);

      sample sa = {last_state, *last_pure_action, *last_action, sensors, reward, goal_reached};
      kdtree_s->insert(sa);
    }

    last_pure_action.reset(new vector<double>(*next_action));
    if(learning) {
      if(gaussian_policy){
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalGaussianWReject(*next_action, noise);
        delete next_action;
        next_action = randomized_action;
      } else if(bib::Utils::rand01() < noise){ //e-greedy
        for (uint i = 0; i < next_action->size(); i++)
          next_action->at(i) = bib::Utils::randin(-1.f, 1.f);
      }
    }
    last_action.reset(next_action);

    last_state.clear();
    for (uint i = 0; i < sensors.size(); i++)
      last_state.push_back(sensors[i]);

    return *next_action;
  }

  void _unique_invoke(boost::property_tree::ptree* pt, boost::program_options::variables_map*) override {
    hidden_unit_v           = pt->get<int>("agent.hidden_unit_v");
    hidden_unit_a           = pt->get<int>("agent.hidden_unit_a");
    noise                   = pt->get<double>("agent.noise");
    gaussian_policy         = pt->get<bool>("agent.gaussian_policy");
    lecun_activation        = pt->get<bool>("agent.lecun_activation");
    change_v_each           = pt->get<uint>("agent.change_v_each");
    strategy_u              = pt->get<uint>("agent.strategy_u");
    current_loaded_v        = pt->get<uint>("agent.index_starting_loaded_v");

    if(hidden_unit_v == 0)
      vnn = new LinMLP(nb_sensors + nb_motors , 1, 0.0, lecun_activation);
    else
      vnn = new MLP(nb_sensors + nb_motors, hidden_unit_v, nb_sensors, 0.0, lecun_activation);

    if(hidden_unit_a == 0)
      ann = new LinMLP(nb_sensors , nb_motors, 0.0, lecun_activation);
    else
      ann = new MLP(nb_sensors, hidden_unit_a, nb_motors, lecun_activation);

    kdtree_s = new kdtree_sample(nb_sensors);
  }

  void _start_episode(const std::vector<double>& sensors, bool) override {
    last_state.clear();
    for (uint i = 0; i < sensors.size(); i++)
      last_state.push_back(sensors[i]);

    last_action = nullptr;
    last_pure_action = nullptr;

    //trajectory.clear();
    last_trajectory.clear();

    fann_reset_MSE(vnn->getNeuralNet());

    if(episode == 0 || episode % change_v_each == 0){
      if (! boost::filesystem::exists( "vset."+std::to_string(current_loaded_v) ) ){
        LOG_ERROR("file doesn't exists "<< "vset."+std::to_string(current_loaded_v));
        exit(1);
      }

      vnn->load("vset."+std::to_string(current_loaded_v));
      current_loaded_v++;
      end_episode();
    }
  }

  void update_actor_old_lw(){
    if (last_trajectory.size() > 0) {
      struct fann_train_data* data = fann_create_train(last_trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      std::vector<double> deltas;
      for(auto it = last_trajectory.begin(); it != last_trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          for (uint i = 0; i < nb_motors; i++)
            data->output[n][i] = sm.a[i];

          deltas.push_back(target - mine);
          n++;
        }
      }

      if(n > 0) {
        double* importance = new double[n];
        double norm = *std::max_element(deltas.begin(), deltas.end());
        for (uint i =0 ; i < n; i++){
            importance[i] = deltas[i] / norm;
        }

        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch_lw(subdata, importance, 5000, 0, 0.0001);

        fann_destroy_train(subdata);

        delete[] importance;
      }
      fann_destroy_train(data);

    }
  }

  void update_actor_old_lw_all(){
    if (trajectory.size() > 0) {
      struct fann_train_data* data = fann_create_train(trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      std::vector<double> deltas;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          for (uint i = 0; i < nb_motors; i++)
            data->output[n][i] = sm.a[i];

          deltas.push_back(target - mine);
          n++;
        }
      }

      if(n > 0) {
        double* importance = new double[n];
        double norm = *std::max_element(deltas.begin(), deltas.end());
        for (uint i =0 ; i < n; i++){
            importance[i] = deltas[i] / norm;
        }

        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch_lw(subdata, importance, 5000, 0, 0.0001);

        fann_destroy_train(subdata);

        delete[] importance;
      }
      fann_destroy_train(data);

    }
  }

  void update_actor_old(){
     if (last_trajectory.size() > 0) {
      bool update_delta_neg;
      bool update_pure_ac;

      switch(strategy_u){
        case 0:
          update_pure_ac=false;
          update_delta_neg=false;
          break;
        case 1:
          update_pure_ac=true;
          update_delta_neg=false;
          break;
        case 2:
          update_pure_ac=false;
          update_delta_neg=true;
          break;
      }

      struct fann_train_data* data = fann_create_train(last_trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      for(auto it = last_trajectory.begin(); it != last_trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          if(update_pure_ac){
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
          } else {
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.a[i];
          }

          n++;
        } else if(update_delta_neg && !update_pure_ac){
            for (uint i = 0; i < nb_sensors ; i++)
              data->input[n][i] = sm.s[i];
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
            n++;
        }
      }

      if(n > 0) {
        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch(subdata, 5000, 0, 0.0001);

        fann_destroy_train(subdata);
      }
      fann_destroy_train(data);
    }
  }

  void update_actor_old_all(){
     if (trajectory.size() > 0) {
      bool update_pure_ac=false;
      bool update_delta_neg=false;

      struct fann_train_data* data = fann_create_train(trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        sample sm = *it;

        double target = 0.f;
        double mine = 0.f;

	target = sm.r;
	if (!sm.goal_reached) {
          std::vector<double> * next_action = ann->computeOut(sm.next_s);
	  double nextV = vnn->computeOutVF(sm.next_s, *next_action);
	  target += gamma * nextV;
          delete next_action;
	}
	mine = vnn->computeOutVF(sm.s, sm.a);

        if(target > mine) {
          for (uint i = 0; i < nb_sensors ; i++)
            data->input[n][i] = sm.s[i];
          if(update_pure_ac){
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
          } else {
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.a[i];
          }

          n++;
        } else if(update_delta_neg && !update_pure_ac){
            for (uint i = 0; i < nb_sensors ; i++)
              data->input[n][i] = sm.s[i];
            for (uint i = 0; i < nb_motors; i++)
              data->output[n][i] = sm.pure_a[i];
            n++;
        }
      }

      if(n > 0) {
        struct fann_train_data* subdata = fann_subset_train_data(data, 0, n);

        ann->learn_stoch(subdata, 5000, 0, 0.0001);

        fann_destroy_train(subdata);
      }
      fann_destroy_train(data);
    }
  }

   void update_actor_nfqca(){
    if(trajectory.size() > 0) {
      struct fann_train_data* data = fann_create_train(trajectory.size(), nb_sensors, nb_motors);

      uint n=0;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        sample sm = *it;

        for (uint i = 0; i < nb_sensors ; i++)
          data->input[n][i] = sm.s[i];

        for (uint i = 0; i < nb_motors; i++)
          data->output[n][i] = 0.f;//sm.a[i]; //don't care

        n++;
      }

      datann_derivative d = {vnn, (int)nb_sensors, (int)nb_motors};

//       auto iter = [&]() {
//         fann_train_epoch_irpropm_gradient(ann->getNeuralNet(), data, derivative_nn, &d);
//       };
//
//       auto eval = [&]() {
//         //compute weight sum
//         return ann->weight_l1_norm();
//       };
//
//       bib::Converger::determinist<>(iter, eval, 1, 0.0001, 25, "actor");
      fann_train_epoch_irpropm_gradient(ann->getNeuralNet(), data, derivative_nn, &d);

      fann_destroy_train(data);
    }
  }

  void end_episode() override {
    if(strategy_u <= 2)
      update_actor_old();
    else if(strategy_u <= 3)
      update_actor_old_lw();
    else if(strategy_u == 10)
      update_actor_nfqca();
    else if(strategy_u == 15)
      update_actor_old_lw_all();
    else if(strategy_u == 20)
      update_actor_old_all();
    else {
      LOG_ERROR("not implemented");
      exit(1);
    }

  }

  void end_instance(bool) override {
    episode++;
  }

  double criticEval(const std::vector<double>& perceptions, const std::vector<double>& actions) override {
    return vnn->computeOutVF(perceptions, actions);
  }

  arch::Policy<MLP>* getCopyCurrentPolicy() override {
        return new arch::Policy<MLP>(new MLP(*ann) , gaussian_policy ? arch::policy_type::GAUSSIAN : arch::policy_type::GREEDY, noise, decision_each);
  }

  void save(const std::string& path) override {
    ann->save(path+".actor");
    vnn->save(path+".critic");
    bib::XMLEngine::save<>(trajectory, "trajectory", "trajectory.data");
  }

  void load(const std::string& path) override {
    ann->load(path+".actor");
    vnn->load(path+".critic");
  }

 protected:
  void _display(std::ostream& out) const override {
    out << std::setw(12) << std::fixed << std::setprecision(10) << sum_weighted_reward << " " << std::setw(
          8) << std::fixed << std::setprecision(5) << vnn->error() << " " << noise << " " << trajectory.size() ;
  }

  void _dump(std::ostream& out) const override {
    out <<" " << std::setw(25) << std::fixed << std::setprecision(22) <<
        sum_weighted_reward << " " << std::setw(8) << std::fixed <<
        std::setprecision(5) << vnn->error() << " " << trajectory.size() ;
  }

 private:
  uint nb_sensors;

  uint episode = 0;
  uint current_loaded_v = 0;
  uint change_v_each;
  uint strategy_u;

  double noise;
  bool gaussian_policy, lecun_activation;
  uint hidden_unit_v;
  uint hidden_unit_a;

  std::shared_ptr<std::vector<double>> last_action;
  std::shared_ptr<std::vector<double>> last_pure_action;
  std::vector<double> last_state;

  std::set<sample> trajectory;
  std::set<sample> last_trajectory;
//     std::list<sample> trajectory;
  KDE proba_s;

  struct L1_distance
  {
    typedef double distance_type;

    double operator() (const double& __a, const double& __b, const size_t) const
    {
      double d = fabs(__a - __b);
      return d;
    }
  };
  typedef KDTree::KDTree<sample, KDTree::_Bracket_accessor<sample>, L1_distance> kdtree_sample;
  kdtree_sample* kdtree_s;

  MLP* ann;
  MLP* vnn;
};

#endif