#include <iostream>
#include <opencv2/opencv.hpp>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <vector>

using namespace std;
using namespace cv;

// 相机内参
#define L_FX 7.188560000000e+02
#define L_FY 7.188560000000e+02
#define L_CX 6.071928000000e+02
#define L_CY 1.852157000000e+02

#define R_FX 7.188560000000e+02
#define R_FY 7.188560000000e+02
#define R_CX 6.071928000000e+02
#define R_CY 1.852157000000e+02

#define FB   3.861448000000e+02

#define IMG_W 1024.0
#define IMG_H 768.0

void prefilterXSobel(const cv::Mat& src, cv::Mat& dst, int ftzero);

template <typename T> void filterSpecklesImpl(cv::Mat& img, int newVal, int maxSpeckleSize, int maxDiff, cv::Mat& _buf);

void initColor(void);

typedef struct _Color
{
//public:
    unsigned char r;
    unsigned char g;
    unsigned char b;

    /*
    _Color(unsigned char _r,unsigned char _g,unsigned char _b)
    {    r=_r;g=_g;b=_b;  }
    */

}Color;

vector<Color> HueCircle;

int main(int argc, char* argv[])
{
    cout<<"Kitti Streo Test."<<endl;
    cout<<"Complied at "<<__TIME__<<", "<<__DATE__<<"."<<endl;

    if(argc!=4)
    {
        cout<<"Usage: "<<argv[0]<<" img_left img_right img_dis"<<endl;
        return 1;
    }

    // 读入左右双目图像
    Mat imgLeft =imread(argv[1],IMREAD_GRAYSCALE);
    Mat imgRight=imread(argv[2],IMREAD_GRAYSCALE);

    if(imgLeft.empty())
    {
        cout<<"Error: img_left "<<argv[1]<<" is empty!"<<endl;
        return 2;
    }

    if(imgRight.empty())
    {
        cout<<"Error: img_left "<<argv[1]<<" is empty!"<<endl;
        return 2;
    }

    imshow("img_left",imgLeft);
    imshow("img_right",imgRight);

    waitKey(100);

    Mat imgLefted,imgRighted;

    // 最小视差
    int mindisparity = 0;
    // 视差搜索范围长度
	int ndisparities = 64;
    // SAD代价计算窗口大小
	int SADWindowSize = 11;
	//SGBM
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);

    // 能量函数参数
	int P1 = 8 * imgLeft.channels() * SADWindowSize* SADWindowSize;
    // 能量函数参数
	int P2 = 32 * imgRight.channels() * SADWindowSize* SADWindowSize;

    // 下面就是各种配置了
	sgbm->setP1(P1);
	sgbm->setP2(P2);
	sgbm->setPreFilterCap(15);
	sgbm->setUniquenessRatio(10);
	sgbm->setSpeckleRange(2);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setDisp12MaxDiff(1);
	//sgbm->setMode(cv::StereoSGBM::MODE_HH);

    // 对原始图像进行预处理
    // imgLeft.copyTo(imgLefted);
    // imgRight.copyTo(imgRighted);
    // prefilterXSobel(imgLeft, imgLefted, sgbm->getPreFilterCap());
    // prefilterXSobel(imgRight, imgRighted, sgbm->getPreFilterCap());

    // imshow("img_left",imgLefted);
    // imshow("img_right",imgRighted);

    // waitKey(0);

    //
    Mat disp;
	sgbm->compute(imgLeft, imgRight, disp);
	disp.convertTo(disp, CV_32F, 1.0 / 16);                //除以16得到真实视差值
	Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);       //显示
	normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);

    // display
    imshow("dis",disp8U);

    waitKey(100);

	imwrite(argv[3], disp8U);

    // 生成相机内参数矩阵
    Eigen::Matrix3d K;
    K<<L_FX,    0.0,    L_CX,
       0.0,     L_FY,   L_CY,
       0.0,     0.0,    1.0;
    Eigen::Matrix3d K_inv=K.inverse();

    // 准备深度着色
    double min_d,max_d;
    int max_id[2],min_id[2];
    minMaxIdx(disp,&min_d,&max_d,min_id,max_id);

    min_d=min_d<5 ? 5:min_d;

    double max_z=FB/min_d,min_z=FB/max_d;

    double factor=1.0*1536/(max_z-min_z);

    initColor();

    // ================================== 准备可视化 =================================
    pangolin::CreateWindowAndBind(
        "PointClouds",     //窗口标题
        IMG_W,        //窗口尺寸
        IMG_H);       //窗口尺寸
    glEnable(GL_DEPTH_TEST);

    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(
            IMG_W,IMG_H,            //相机图像的长和宽
            L_FX,L_FY,L_CX,L_CY,    //相机的内参,fu fv u0 v0
            0.2,1000),           //相机所能够看到的最浅和最深的像素
        pangolin::ModelViewLookAt(
            -2,2,-2,            //相机光心位置,NOTICE z轴不要设置为0
            0,0,0,              //相机要看的位置
            pangolin::AxisY)    //和观察的方向有关
    );

    // Create Interactive View in window
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(
                0.0, 1.0, 0.0, 1.0,     //表示整个窗口都可以观测到
                -IMG_W/IMG_H)         //窗口的比例
            .SetHandler(&handler);

    while( !pangolin::ShouldQuit() )
    {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        glClearColor(1,1,1,0.0);

        // Render OpenGL Cube
        //pangolin::glDrawColouredCube();

        //尝试按照谢晓佳的视频中给出的代码绘制
        // pangolin::glDrawAxis(3);

        glPointSize(1.0f);
        glBegin(GL_POINTS);

        // 绘制当前帧点云
        for(int x=0;x<disp.rows;++x)
        {
            for(int y=0;y<disp.cols;++y)
            {
                float d=disp.at<float>(x,y);

                if(d>=min_d)
                {
                    double z=FB/d;

                    Eigen::Vector3d position=z*K_inv*Eigen::Vector3d(y,x,1);
                    // cout<<"Debug: disp8U.at<uint8_t>(x,y)="<<(int)(disp8U.at<uint8_t>(x,y))<<endl;
                    // Color c=HueCircle[(int)(disp8U.at<char>(x,y))];
                    // if((size_t)(z*1000)<1535)
                    {
                        // cout<<"debug: z="<<z<<endl;
                        // Color c=HueCircle[(size_t)(z*10)];
                        // cout<<"debug: r="<<(int)c.r<<"\tg="<<(int)c.g<<"\tb="<<(int)c.b<<"\t(size_t)(z*10)="<<(size_t)(z*10)<<endl;
                        size_t index=(z-min_z)*factor;
                        Color c=HueCircle[index];

                        // 画点
                        glColor3f(0,0,0);
                        // glColor3f(c.r/255.0,c.g/255.0,c.b/255.0);
                        // glColor3f(c.r,c.g,c.b);
                        glVertex3f(-position[0],-position[1],position[2]);
                    }

                }

            }
        }

        glEnd();
        //不要忘记了这个东西!!!
        glFlush();

        // Swap frames and Process Events
        pangolin::FinishFrame();

        waitKey(1);

    }

    return 0;
}

void initColor(void)
{

     //颜色环初始化
    HueCircle.reserve(1536);
    HueCircle.resize(1536);

    for (int i = 0;i < 255;i++)
	{
		HueCircle[i].r = 255;
		HueCircle[i].g = i;
		HueCircle[i].b = 0;

		HueCircle[i+255].r = 255-i;
		HueCircle[i+255].g = 255;
		HueCircle[i+255].b = 0;

		HueCircle[i+511].r = 0;
		HueCircle[i+511].g = 255;
		HueCircle[i+511].b = i;

		HueCircle[i+767].r = 0;
		HueCircle[i+767].g = 255-i;
		HueCircle[i+767].b = 255;

		HueCircle[i+1023].r = i;
		HueCircle[i+1023].g = 0;
		HueCircle[i+1023].b = 255;

		HueCircle[i+1279].r = 255;
		HueCircle[i+1279].g = 0;
		HueCircle[i+1279].b = 255-i;
	}

	HueCircle[1534].r = 0;
	HueCircle[1534].g = 0;
	HueCircle[1534].b = 0;

	HueCircle[1535].r = 255;
	HueCircle[1535].g = 255;
	HueCircle[1535].b = 255;

    // while(1)
    // {
    //     int index;
    //     cout<<"Index? ";
    //     cin>>index;
    //     cout<<"\tr="<<()HueCircle[index].r
    //         <<"\tg="<<HueCircle[index].g
    //         <<"\tb="<<HueCircle[index].b<<endl;
    // }

}


/* ============================================================================
 * Copyright 2021 The University of Utah
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of BlueQuartz Software, the US Air Force, the University of Utah 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.
 *
 * The code contained herein was partially funded by the following contracts:
 *
 *
 * This code contained herein is based upon work supported by the following grants:
 *    DOE Office of Nuclear Energy's Nuclear Energy University Program Grant No.: DE-NE0008799
 *    DOD Office of Economic Adjustment Grant No.: ST1605-19-03
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "ComputeFeatureEigenstrains.h"

#include <cmath>

#include <QtCore/QTextStream>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>

#include "EbsdLib/Core/Orientation.hpp"
#include "EbsdLib/Core/OrientationTransformation.hpp"

#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/DataContainers/DataContainer.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/AbstractFilterParametersReader.h"
#include "SIMPLib/FilterParameters/DataArraySelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/FloatFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedBooleanFilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedPathCreationFilterParameter.h"
#include "SIMPLib/FilterParameters/SeparatorFilterParameter.h"
#include "SIMPLib/Geometry/ImageGeom.h"
#include "SIMPLib/Math/SIMPLibMath.h"

#include "DREAM3DReview/DREAM3DReviewConstants.h"
#include "DREAM3DReview/DREAM3DReviewVersion.h"

#include "DREAM3DReview/DREAM3DReviewFilters/util/EigenstrainsHelper.hpp"

namespace SIMPLMath = SIMPLib::Constants;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ComputeFeatureEigenstrains::ComputeFeatureEigenstrains() = default;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
ComputeFeatureEigenstrains::~ComputeFeatureEigenstrains() = default;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::initialize()
{
  clearErrorCode();
  clearWarningCode();
  setCancel(false);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setupFilterParameters()
{
  FilterParameterVectorType parameters;

  // Poisson's ratio user input
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Poisson's Ratio", PoissonRatio, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));

  std::vector<QString> linkedProps;
  linkedProps.push_back("AxisLengthsArrayPath");
  linkedProps.push_back("AxisEulerAnglesArrayPath");

  parameters.push_back(
      SIMPL_NEW_LINKED_BOOL_FP("Use Ellipsoidal Grains (versus spherical assumption)", UseEllipsoidalGrains, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains, linkedProps));

  // Correctional matrix beta user inputs
  linkedProps.clear();
  linkedProps.push_back("Beta11");
  linkedProps.push_back("Beta22");
  linkedProps.push_back("Beta33");
  linkedProps.push_back("Beta23");
  linkedProps.push_back("Beta13");
  linkedProps.push_back("Beta12");
  parameters.push_back(SIMPL_NEW_LINKED_BOOL_FP("Use Correctional Matrix", UseCorrectionalMatrix, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains, linkedProps));

  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta11", Beta11, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta22", Beta22, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta33", Beta33, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta23", Beta23, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta13", Beta13, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));
  parameters.push_back(SIMPL_NEW_FLOAT_FP("Beta12", Beta12, FilterParameter::Category::Parameter, ComputeFeatureEigenstrains));

  // Axis lengths and euler angles 3xN feature arrays
  parameters.push_back(SeparatorFilterParameter::Create("Cell Feature Data", FilterParameter::Category::RequiredArray));

  {
    DataArraySelectionFilterParameter::RequirementType req =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, 3, AttributeMatrix::Type::CellFeature, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Axis Lengths", AxisLengthsArrayPath, FilterParameter::Category::RequiredArray, ComputeFeatureEigenstrains, req));
  }

  {
    DataArraySelectionFilterParameter::RequirementType req =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, 3, AttributeMatrix::Type::CellFeature, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Axis Euler Angles", AxisEulerAnglesArrayPath, FilterParameter::Category::RequiredArray, ComputeFeatureEigenstrains, req));
  }

  // Elastic strain 6xN feature array
  {
    DataArraySelectionFilterParameter::RequirementType req =
        DataArraySelectionFilterParameter::CreateRequirement(SIMPL::TypeNames::Float, 6, AttributeMatrix::Type::CellFeature, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_DA_SELECTION_FP("Elastic Strains (Voigt Notation)", ElasticStrainsArrayPath, FilterParameter::Category::RequiredArray, ComputeFeatureEigenstrains, req));
  }

  // Output 6xN eigenstrain feature array
  parameters.push_back(SeparatorFilterParameter::Create("Cell Feature Data", FilterParameter::Category::CreatedArray));
  parameters.push_back(
      SIMPL_NEW_DA_WITH_LINKED_AM_FP("Eigenstrains", EigenstrainsArrayName, ElasticStrainsArrayPath, ElasticStrainsArrayPath, FilterParameter::Category::CreatedArray, ComputeFeatureEigenstrains));

  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::dataCheck()
{
  clearErrorCode();
  clearWarningCode();
  initialize();
  DataArrayPath tempPath;

  // Negative Poisson's ratio works in the calculations but warn
  if(getPoissonRatio() < 0.0f)
  {
    QString ss = QObject::tr("Poisson's ratio is negative");
    setWarningCondition(-94001, ss);
  }

  // Incompressible v=0.5 results in singular matrix
  if(getPoissonRatio() > 0.49999999f)
  {
    QString ss = QObject::tr("Poisson's ratio cannot be 0.5 or greater");
    setErrorCondition(-94002, ss);
  }

  // Check correction values and warn if they are large (potential typos)
  if(m_UseCorrectionalMatrix)
  {
    if(getBeta11() < 0.5f || getBeta11() > 2.0f)
    {
      QString ss = QObject::tr("Beta11 correction is pretty large; this may be a typo");
      setWarningCondition(-94003, ss);
    }

    if(getBeta22() < 0.5f || getBeta22() > 2.0f)
    {
      QString ss = QObject::tr("Beta22 correction is pretty large; this may be a typo");
      setWarningCondition(-94004, ss);
    }

    if(getBeta33() < 0.5f || getBeta33() > 2.0f)
    {
      QString ss = QObject::tr("Beta33 correction is pretty large; this may be a typo");
      setWarningCondition(-94005, ss);
    }

    if(getBeta23() < 0.5f || getBeta23() > 2.0f)
    {
      QString ss = QObject::tr("Beta23 correction is pretty large; this may be a typo");
      setWarningCondition(-94006, ss);
    }

    if(getBeta13() < 0.5f || getBeta13() > 2.0f)
    {
      QString ss = QObject::tr("Beta13 correction is pretty large; this may be a typo");
      setWarningCondition(-94007, ss);
    }

    if(getBeta12() < 0.5f || getBeta12() > 2.0f)
    {
      QString ss = QObject::tr("Beta12 correction is pretty large; this may be a typo");
      setWarningCondition(-94008, ss);
    }
  }

  // Check Required Objects
  std::vector<size_t> cDims(1, 1);
  if(m_UseEllipsoidalGrains)
  {
    cDims[0] = 3;
    m_AxisLengthsPtr = getDataContainerArray()->getPrereqArrayFromPath<FloatArrayType>(this, getAxisLengthsArrayPath(), cDims);
    if(getErrorCode() < 0)
    {
      return;
    }

    cDims[0] = 3;
    m_AxisEulerAnglesPtr = getDataContainerArray()->getPrereqArrayFromPath<FloatArrayType>(this, getAxisEulerAnglesArrayPath(), cDims);
    if(getErrorCode() < 0)
    {
      return;
    }
  }

  cDims[0] = 6;
  m_ElasticStrainsPtr = getDataContainerArray()->getPrereqArrayFromPath<FloatArrayType>(this, getElasticStrainsArrayPath(), cDims);
  if(getErrorCode() < 0)
  {
    return;
  }

  // Check Eigenstrain output
  cDims[0] = 6;
  tempPath.update(getElasticStrainsArrayPath().getDataContainerName(), getElasticStrainsArrayPath().getAttributeMatrixName(), getEigenstrainsArrayName());
  m_EigenstrainsPtr = getDataContainerArray()->createNonPrereqArrayFromPath<FloatArrayType>(this, tempPath, 0, cDims, "", 1);
  if(getErrorCode() < 0)
  {
    return;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::execute()
{
  initialize();
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }

  if(getCancel())
  {
    return;
  }

  find_eigenstrains();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::find_eigenstrains()
{
  size_t numfeatures = m_ElasticStrainsPtr.lock()->getNumberOfTuples();

  double phi1 = 0.0;
  double theta = 0.0;
  double phi2 = 0.0;

  double semiAxisA = 0.0;
  double semiAxisB = 0.0;
  double semiAxisC = 0.0;

  double E11 = 0.0;
  double E22 = 0.0;
  double E33 = 0.0;
  double E23 = 0.0;
  double E13 = 0.0;
  double E12 = 0.0;

  Eigen::Matrix<double, 3, 3> beta;
  beta.setOnes(3, 3); // Default no correction
  if(m_UseCorrectionalMatrix)
  {
    // clang-format off
    beta << m_Beta11, m_Beta12, m_Beta13,
            m_Beta12, m_Beta22, m_Beta23,
            m_Beta13, m_Beta23, m_Beta33;
    // clang-format on
  }

  OrientationD orientationMatrix;
  Eigen::Matrix<double, 3, 3> OM;
  Eigen::Matrix<double, 3, 3> OMT;
  OM.setIdentity(3, 3); // Defaults to no orientation identity
  OMT.setIdentity(3, 3);

  Eigen::Matrix<double, 3, 3> elasticStrainTensor;
  Eigen::Matrix<double, 3, 3> elasticStrainTensorRot;

  EigenstrainsHelper::Tensor4DType eshelbyTensor;
  EigenstrainsHelper::Tensor4DType eshelbyInverse;

  Eigen::Matrix<double, 9, 9> eshelbyTensor99;
  Eigen::Matrix<double, 9, 9> eshelbyInverse99;
  Eigen::Matrix<double, 9, 9> I9;
  I9.setIdentity(9, 9);

  Eigen::Matrix<double, 3, 3> eigenstrainTensorRot;
  Eigen::Matrix<double, 3, 3> eigenstrainTensor;
  Eigen::Matrix<double, 3, 3> eigenstrainTensorCorrected;

  FloatArrayType& axisEulerAngles = *(m_AxisEulerAnglesPtr.lock().get());
  FloatArrayType& axisLengths = *(m_AxisLengthsPtr.lock().get());
  FloatArrayType& elasticStrains = *(m_ElasticStrainsPtr.lock().get());
  FloatArrayType& eigenstrains = *(m_EigenstrainsPtr.lock().get());

  // small eps term added to euler angle bounds check as FindFeatureShapes has some small noise outside the bounds
  double eps = 0.001;
  double eulerPhiMax = 2 * SIMPLMath::k_PiD + eps;
  double eulerThetaMax = SIMPLMath::k_PiD + eps;
  double eulerMin = 0 - eps;

  for(size_t feature = 0; feature < numfeatures; feature++)
  {
    if(m_UseEllipsoidalGrains)
    {
      phi1 = axisEulerAngles[feature * 3 + 0];
      theta = axisEulerAngles[feature * 3 + 1];
      phi2 = axisEulerAngles[feature * 3 + 2];

      if(std::isnan(phi1) || std::isnan(theta) || std::isnan(phi2))
      {
        QString ss = QObject::tr("NaN Axis Euler angle found in feature ID #%1, skipping").arg(feature);
        notifyStatusMessage(ss);
        eigenstrains[feature * 6 + 0] = 0;
        eigenstrains[feature * 6 + 1] = 0;
        eigenstrains[feature * 6 + 2] = 0;
        eigenstrains[feature * 6 + 3] = 0;
        eigenstrains[feature * 6 + 4] = 0;
        eigenstrains[feature * 6 + 5] = 0;
        continue;
      }

      if(phi1 > eulerPhiMax || phi1 < eulerMin)
      {
        QString ss = QObject::tr("Feature %1 euler angle phi1=%2 out of bounds 2pi >= phi1 >= 0. Euler angles may be in degrees").arg(feature).arg(phi1);
        setErrorCondition(-94000, ss);
        return;
      }

      if(theta > eulerThetaMax || theta < eulerMin)
      {
        QString ss = QObject::tr("Feature %1 euler angle theta=%2 out of bounds pi >= theta >= 0. Euler angles may be in degrees").arg(feature).arg(theta);
        setErrorCondition(-94000, ss);
        return;
      }

      if(phi2 > eulerPhiMax || phi2 < eulerMin)
      {
        QString ss = QObject::tr("Feature %1 euler angle phi2=%2 out of bounds 2pi >= phi2 >= 0. Euler angles may be in degrees").arg(feature).arg(phi2);
        setErrorCondition(-94000, ss);
        return;
      }

      semiAxisA = axisLengths[feature * 3 + 0];
      semiAxisB = axisLengths[feature * 3 + 1];
      semiAxisC = axisLengths[feature * 3 + 2];

      if(std::isnan(semiAxisA) || std::isnan(semiAxisB) || std::isnan(semiAxisC))
      {
        QString ss = QObject::tr("NaN Axis length found in feature ID #%1, skipping").arg(feature);
        notifyStatusMessage(ss);
        eigenstrains[feature * 6 + 0] = 0;
        eigenstrains[feature * 6 + 1] = 0;
        eigenstrains[feature * 6 + 2] = 0;
        eigenstrains[feature * 6 + 3] = 0;
        eigenstrains[feature * 6 + 4] = 0;
        eigenstrains[feature * 6 + 5] = 0;
        continue;
      }

      if(semiAxisB > semiAxisA)
      {
        QString ss = QObject::tr("Feature %1 semi-axis b=%2 is greater than semi-axis a=%3. Criteria a>=b>=c must be satisfied").arg(feature).arg(semiAxisB).arg(semiAxisA);
        setErrorCondition(-94000, ss);
        return;
      }

      if(semiAxisC > semiAxisB)
      {
        QString ss = QObject::tr("Feature %1 semi-axis c=%2 is greater than semi-axis b=%3. Criteria a>=b>=c must be satisfied").arg(feature).arg(semiAxisC).arg(semiAxisB);
        setErrorCondition(-94000, ss);
        return;
      }
    }

    E11 = elasticStrains[feature * 6 + 0];
    E22 = elasticStrains[feature * 6 + 1];
    E33 = elasticStrains[feature * 6 + 2];
    E23 = elasticStrains[feature * 6 + 3];
    E13 = elasticStrains[feature * 6 + 4];
    E12 = elasticStrains[feature * 6 + 5];

    // clang-format off
    elasticStrainTensor << E11, E12, E13,
                           E12, E22, E23,
                           E13, E23, E33;
    // clang-format on

    // Check if the elastic strains are zero (or negligible) then so are the eigenstrains
    if(elasticStrainTensor.isMuchSmallerThan(1e-10))
    {
      eigenstrains[feature * 6 + 0] = 0;
      eigenstrains[feature * 6 + 1] = 0;
      eigenstrains[feature * 6 + 2] = 0;
      eigenstrains[feature * 6 + 3] = 0;
      eigenstrains[feature * 6 + 4] = 0;
      eigenstrains[feature * 6 + 5] = 0;
      continue;
    }

    if(m_UseEllipsoidalGrains)
    {
      orientationMatrix = OrientationTransformation::eu2om<OrientationD, OrientationD>({phi1, theta, phi2});

      // clang-format off
      OM << orientationMatrix[0], orientationMatrix[1], orientationMatrix[2],
            orientationMatrix[3], orientationMatrix[4], orientationMatrix[5],
            orientationMatrix[6], orientationMatrix[7], orientationMatrix[8];
      OMT = OM.transpose();
      // clang-format on
    }

    // Change basis into ellipsoid reference frame | e' = Q e Q^T
    elasticStrainTensorRot = OM * elasticStrainTensor * OMT;

    // Calculate Eshelby tensor | S = f(a, b, c, nu)
    eshelbyTensor = EigenstrainsHelper::find_eshelby(semiAxisA, semiAxisB, semiAxisC, m_PoissonRatio, m_UseEllipsoidalGrains);

    // Map Eshelby tensor into 9x9 matrix
    size_t col = 0;
    size_t row = 0;
    for(size_t i = 0; i < 3; i++)
    {
      for(size_t j = 0; j < 3; j++)
      {
        for(size_t k = 0; k < 3; k++)
        {
          for(size_t l = 0; l < 3; l++)
          {
            eshelbyTensor99(col, row) = eshelbyTensor(i, j, k, l);
            col++;
          }
        }
        row++;
        col = 0;
      }
    }

    // Calculate inverse | (S-I)^-1
    eshelbyInverse99 = (eshelbyTensor99 - I9).inverse();

    // Remap inverse back into a 3x3x3x3 tensor
    col = 0;
    row = 0;
    for(size_t i = 0; i < 3; i++)
    {
      for(size_t j = 0; j < 3; j++)
      {
        for(size_t k = 0; k < 3; k++)
        {
          for(size_t l = 0; l < 3; l++)
          {
            eshelbyInverse(i, j, k, l) = eshelbyInverse99(col, row);
            col++;
          }
        }
        row++;
        col = 0;
      }
    }

    // Calculate eigenstrain tensor | e*' = (S-I)^-1 e'
    eigenstrainTensorRot.setZero(3, 3);
    for(size_t i = 0; i < 3; i++)
    {
      for(size_t j = 0; j < 3; j++)
      {
        for(size_t k = 0; k < 3; k++)
        {
          for(size_t l = 0; l < 3; l++)
          {
            eigenstrainTensorRot(i, j) += eshelbyInverse(i, j, k, l) * elasticStrainTensorRot(k, l);
          }
        }
      }
    }

    // Change basis back to global reference frame | e* = Q^T e*' Q
    eigenstrainTensor = OMT * eigenstrainTensorRot * OM;

    // Add correction | e*c = B e*
    eigenstrainTensorCorrected = eigenstrainTensor.cwiseProduct(beta);

    eigenstrains[feature * 6 + 0] = eigenstrainTensorCorrected(0, 0);
    eigenstrains[feature * 6 + 1] = eigenstrainTensorCorrected(1, 1);
    eigenstrains[feature * 6 + 2] = eigenstrainTensorCorrected(2, 2);
    eigenstrains[feature * 6 + 3] = eigenstrainTensorCorrected(1, 2);
    eigenstrains[feature * 6 + 4] = eigenstrainTensorCorrected(0, 2);
    eigenstrains[feature * 6 + 5] = eigenstrainTensorCorrected(0, 1);
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
AbstractFilter::Pointer ComputeFeatureEigenstrains::newFilterInstance(bool copyFilterParameters) const
{
  ComputeFeatureEigenstrains::Pointer filter = ComputeFeatureEigenstrains::New();
  if(copyFilterParameters)
  {
    copyFilterParameterInstanceVariables(filter.get());
  }
  return filter;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getCompiledLibraryName() const
{
  return DREAM3DReviewConstants::DREAM3DReviewBaseName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getBrandingString() const
{
  return "DREAM3DReview";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getFilterVersion() const
{
  QString version;
  QTextStream vStream(&version);
  vStream << DREAM3DReview::Version::Major() << "." << DREAM3DReview::Version::Minor() << "." << DREAM3DReview::Version::Patch();
  return version;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getGroupName() const
{
  return DREAM3DReviewConstants::FilterGroups::DREAM3DReviewFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getSubGroupName() const
{
  return DREAM3DReviewConstants::FilterSubGroups::RegistrationFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getHumanLabel() const
{
  return "Compute Eigenstrains by Feature (Grain/Inclusion)";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid ComputeFeatureEigenstrains::getUuid() const
{
  return QUuid("{879e1eb8-40dc-5a5b-abe5-7e0baa77ed73}");
}

// -----------------------------------------------------------------------------
ComputeFeatureEigenstrains::Pointer ComputeFeatureEigenstrains::NullPointer()
{
  return Pointer(static_cast<Self*>(nullptr));
}

// -----------------------------------------------------------------------------
std::shared_ptr<ComputeFeatureEigenstrains> ComputeFeatureEigenstrains::New()
{
  struct make_shared_enabler : public ComputeFeatureEigenstrains
  {
  };
  std::shared_ptr<make_shared_enabler> val = std::make_shared<make_shared_enabler>();
  val->setupFilterParameters();
  return val;
}

// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getNameOfClass() const
{
  return QString("ComputeFeatureEigenstrains");
}

// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::ClassName()
{
  return QString("ComputeFeatureEigenstrains");
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setPoissonRatio(float value)
{
  m_PoissonRatio = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getPoissonRatio() const
{
  return m_PoissonRatio;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setUseEllipsoidalGrains(bool value)
{
  m_UseEllipsoidalGrains = value;
}

// -----------------------------------------------------------------------------
bool ComputeFeatureEigenstrains::getUseEllipsoidalGrains() const
{
  return m_UseEllipsoidalGrains;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setUseCorrectionalMatrix(bool value)
{
  m_UseCorrectionalMatrix = value;
}

// -----------------------------------------------------------------------------
bool ComputeFeatureEigenstrains::getUseCorrectionalMatrix() const
{
  return m_UseCorrectionalMatrix;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta11(float value)
{
  m_Beta11 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta11() const
{
  return m_Beta11;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta22(float value)
{
  m_Beta22 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta22() const
{
  return m_Beta22;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta33(float value)
{
  m_Beta33 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta33() const
{
  return m_Beta33;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta23(float value)
{
  m_Beta23 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta23() const
{
  return m_Beta23;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta13(float value)
{
  m_Beta13 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta13() const
{
  return m_Beta13;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setBeta12(float value)
{
  m_Beta12 = value;
}

// -----------------------------------------------------------------------------
float ComputeFeatureEigenstrains::getBeta12() const
{
  return m_Beta12;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setAxisLengthsArrayPath(const DataArrayPath& value)
{
  m_AxisLengthsArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ComputeFeatureEigenstrains::getAxisLengthsArrayPath() const
{
  return m_AxisLengthsArrayPath;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setAxisEulerAnglesArrayPath(const DataArrayPath& value)
{
  m_AxisEulerAnglesArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ComputeFeatureEigenstrains::getAxisEulerAnglesArrayPath() const
{
  return m_AxisEulerAnglesArrayPath;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setElasticStrainsArrayPath(const DataArrayPath& value)
{
  m_ElasticStrainsArrayPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath ComputeFeatureEigenstrains::getElasticStrainsArrayPath() const
{
  return m_ElasticStrainsArrayPath;
}

// -----------------------------------------------------------------------------
void ComputeFeatureEigenstrains::setEigenstrainsArrayName(const QString& value)
{
  m_EigenstrainsArrayName = value;
}

// -----------------------------------------------------------------------------
QString ComputeFeatureEigenstrains::getEigenstrainsArrayName() const
{
  return m_EigenstrainsArrayName;
}


#include "Tudat/Astrodynamics/Relativity/jw_acceleration.h"

#include <iostream>

#include <Eigen/Geometry>

namespace tudat {

namespace gnv {

    Eigen::Vector3d ext_pot_acceleration(
        const Eigen::Vector3d& relativePosition,
        const Eigen::Vector3d& relativePosition_cb_wrt_primary,
        const double commonCorrectionTerm,
        const double mu_primary
    ) {
        double rab = relativePosition_cb_wrt_primary.norm();
        return (commonCorrectionTerm*mu_primary/rab)*relativePosition;
    }

    Eigen::Vector3d kinetic_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor
    ) {
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa   = state_sa.segment(0,3);

        Eigen::Vector3d vel_a    = state_actor.segment(3,3);

        const double c = tudat::physical_constants::SPEED_OF_LIGHT;

        double beta_a = vel_a.norm()/c;

        double r = pos_sa.norm();

        return -(2.0*beta_a*beta_a*mu_actor/(r*r*r))*pos_sa;

    }

    Eigen::Vector3d schwarzschild_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor,
        double ppn_gamma
    ) {
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa = state_sa.segment(0,3);
        Eigen::Vector3d vel_sa = state_sa.segment(3,3);

        const double c = tudat::physical_constants::SPEED_OF_LIGHT;
        double r = pos_sa.norm();

        double mu_c2_r3 = mu_actor / (c*c*r*r*r);

        return mu_c2_r3 * (
            (4.0*mu_actor/r - vel_sa.dot(vel_sa)) * pos_sa +
            4*(pos_sa.dot(vel_sa)) * vel_sa
        );

//        Eigen::Vector3d output_vector;
//        output_vector.setZero();
//        return output_vector;
    }

    Eigen::Vector3d cb_velocity_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor
    ) {
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa = state_sa.segment(0,3);
        //Eigen::Vector3d vel_sa = state_sa.segment(3,3);
        Eigen::Vector3d vel_a  = state_actor.segment(3,3);

        double r = pos_sa.norm();
        double r_dot_v = pos_sa.dot(vel_a);
        double c = tudat::physical_constants::SPEED_OF_LIGHT;
        double pre_factor = (mu_actor*r_dot_v)/(2.0*c*c*r*r*r);

        return pre_factor * ( ((3.0*r_dot_v)/(r*r)) * pos_sa - 2.0*vel_a);
    }

    Eigen::Vector3d lense_thirring_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        Eigen::Vector3d angular_momentum_actor
    ) {
//        std::cout << __FILE__ << "\n Line " << __LINE__ << std::endl;
//        throw std::runtime_error("Lense Thirring function not implemented");
//        return Eigen::Vector3d::Zero();

        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa = state_sa.segment(0,3);
        Eigen::Vector3d vel_sa = state_sa.segment(3,3);

        double r = pos_sa.norm();

        double G = tudat::physical_constants::GRAVITATIONAL_CONSTANT;
        double c = tudat::physical_constants::SPEED_OF_LIGHT;

        return (2*G/(c*c*r*r*r)) * (
            (3.0/(r*r)) * pos_sa.dot(angular_momentum_actor) * pos_sa.cross(vel_sa) +
            vel_sa.cross(angular_momentum_actor)
        );
    }

    Eigen::Vector3d wavi_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        double mu_actor
    ) {
//        std::cout << __FILE__ << "\n Line " << __LINE__ << std::endl;
//        throw std::runtime_error("Wavi function not implemented");
//        return Eigen::Vector3d::Zero();

        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector3d pos_sa   = state_sa.segment(0,3);
        Eigen::Vector3d vel_sa   = state_sa.segment(3,3);
        Eigen::Vector3d vel_a    = state_actor.segment(3,3);

        const double c = tudat::physical_constants::SPEED_OF_LIGHT;

        double r = pos_sa.norm();

        return 4*mu_actor / (c*c*r*r*r) * (
            vel_a.dot(vel_sa) * pos_sa -
            pos_sa.dot(vel_sa) * vel_a
        );
    }

    Eigen::Vector3d de_sitter_acceleration(
        Eigen::Vector6d state_subject,
        Eigen::Vector6d state_actor,
        Eigen::Vector6d state_primary,
        double mu_primary
    ) {
//        std::cout << __FILE__ << "\n Line " << __LINE__ << std::endl;
//        throw std::runtime_error("de Sitter function not implemented");
//        return Eigen::Vector3d::Zero();
        Eigen::Vector6d state_sa = state_subject - state_actor;
        Eigen::Vector6d state_ap = state_actor - state_primary;

        Eigen::Vector3d v_sa = state_sa.segment(3,3);
        Eigen::Vector3d r_ap = state_ap.segment(0,3);
        Eigen::Vector3d v_ap = state_ap.segment(3,3);

        double c = tudat::physical_constants::SPEED_OF_LIGHT;

        //double r = r_sa.norm();
        double R = r_ap.norm();

        double pre_factor = 3.0*mu_primary/(c*c*R*R*R);

        return pre_factor * (
            (v_ap.cross(r_ap)).cross(v_sa)
        );
    }

}

}


#include<iostream>
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include<iomanip>
#include <boost/program_options.hpp>

using namespace boost::program_options;
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
   using boost::program_options::value;
   int M{};
  int L{};
    double T{};
  double t0{};
  double omega{};
  double gamma{};
  double cutoff{};
  bool PB{};

  std::string sM{};
  std::string sL{};
  std::string star{};
  std::string sT{};
  std::string st0{};
  std::string somega{};
  std::string sgamma{};
  std::string starget{};
  std::string scutoff{};
  std::string sPB{};
  std::string filename="exT";
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("T", value(&T)->default_value(0.1), "T")
      ("M,m", value(&M)->default_value(2), "M")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(std::sqrt(2)), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("pb", value(&PB)->default_value(0), "pb");
  boost::program_options::variables_map vm;
  boost::program_options::store(parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<int>() << '\n';
      	sL="L"+std::to_string(vm["L"].as<int>());
	filename+=sL;
      }
         if (vm.count("M"))
      {      std::cout << "M: " << vm["M"].as<int>() << '\n';
      	sM="M"+std::to_string(vm["M"].as<int>());
	filename+=sM;
      }
      	 if (vm.count("t0"))
      {      std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      	st0="t0"+std::to_string(vm["t0"].as<double>()).substr(0, 3);
      	filename+=st0;
      }
      	 	 if (vm.count("omg"))
      {      std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      	somega="omg"+std::to_string(vm["omg"].as<double>()).substr(0, 3);
      		filename+=somega;
      }

      		 if (vm.count("gam"))
      {      std::cout << "gamma: " << vm["gam"].as<double>() << '\n';
      	sgamma="gam"+std::to_string(vm["gam"].as<double>()).substr(0, 3);
      		filename+=sgamma;
      }
      // 		 if (vm.count("T"))
      // {      std::cout << "T: " << vm["T"].as<double>() << '\n';
      // 	sT="T"+std::to_string(vm["T"].as<double>()).substr(0, 3);
      // 	filename+=sT;
      // }
		 if (vm.count("pb"))
      {      std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      	sPB="PB"+std::to_string(vm["pb"].as<bool>());
      		filename+=sPB;
      }
      }
    }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }
  filename+=".bin";
  double mean=0.5*L*M*omega;
  double dt=0.1;
  ElectronState estate1(L, 1);
  ElectronBasis e( L, 1);
  std::cout<< e<<std::endl;

  PhononBasis ph(L, M);
  //                 std::cout<< ph<<std::endl;
      HolsteinBasis TP(e, ph);
      std::cout<< TP.dim << std::endl;
         Eigen::VectorXcd inistate(TP.dim);

   	 Mat O=Operators::NumberOperator(TP, ph, 1,  PB);
   inistate.setZero();

   inistate[0]=1;
   	          Eigen::VectorXcd i0=inistate;

   		  std::cout<< TP.dim << std::endl;
      Mat E1=Operators::EKinOperatorL(TP, e, t0,PB);
       Mat Ebdag=Operators::NBosonCOperator(TP, ph, gamma, PB);
       Mat Eb=Operators::NBosonDOperator(TP, ph, gamma, PB);
      Mat Eph=Operators::NumberOperator(TP, ph, omega,  PB);

      Eigen::VectorXd eigenVals(TP.dim);
      Mat H=E1+Eb+Eph+Ebdag;
   auto J=Operators::CurOperatorL(TP, e, t0, PB);
Eigen::MatrixXcd Jcp=J*std::complex<double>(0,1);

    Eigen::MatrixXd HH=Eigen::MatrixXd(H);

    //Eigen::MatrixXcd HHcp=HH;
    Eigen::MatrixXcd HHcp=Eigen::MatrixXd(HH);
             Eigen::MatrixXcd M2=HHcp*Jcp;
             Eigen::MatrixXcd M1=Jcp*HHcp;
	     Eigen::MatrixXcd COMM=M1-M2;
	     //                  std::cout<< M1<<std::endl;
	     //           std::cout<< COMM<<std::endl;
	     //   std::cout<< M2<<std::endl;
	       COMM*=Jcp;

		  //           std::cout<< HHcp<<std::endl;
		  //           std::cout<< Jcp<<std::endl;
        Eigen::MatrixXd N=Eigen::MatrixXd(Eph);
	  Many_Body::diagMat(HH, eigenVals);
     std::cout<<"MIN E "<<std::setprecision(15)<< eigenVals(0)<< std::endl;
     std::cout<< "MAX "<<eigenVals(eigenVals.size()-1)<< std::endl;
    		      std::cout<<endl<<eigenVals.mean()-mean<<std::endl;
         Eigen::VectorXd energy(TP.dim);
    	   std::vector<double> obs(TP.dim);
    	   std::vector<double> obs2(TP.dim);
	   std::vector<double> obs3(TP.dim);
	   std::vector<double> obs4(TP.dim);
	   std::vector<double> obs5(TP.dim);
	   std::vector<double> obs6(TP.dim);
	   //std::vector<double> obs(TP.dim);
    	   std::vector<double> ensvec(TP.dim);

	   Eigen::MatrixXd PHD2=HH.adjoint()*N*HH;
	   Eigen::MatrixXd EKIN=HH.adjoint()*E1*HH;
	   Eigen::MatrixXd EC=HH.adjoint()*(Ebdag+Eb)*HH;
	   Eigen::MatrixXcd Jmat=HH.adjoint()*(Jcp*HH);
	   Eigen::MatrixXcd JJmat=Jmat*Jmat;
	   Eigen::MatrixXcd COMMmat=HH.adjoint()*(COMM*HH);
	   //	   std::complex<double> J_in=(newIn.adjoint()*(Jcp*newIn))(0);
	   // std::complex<double> JJsq_in=(newIn.adjoint()*(Jcp*Jcp*newIn))(0);
	    Eigen::VectorXd v=PHD2.diagonal();
	    Eigen::VectorXd v2=EKIN.diagonal();
	    Eigen::VectorXd v3=EC.diagonal();
	    Eigen::VectorXd v4=(JJmat.diagonal().real());
	     Eigen::VectorXd v5=(Jmat.diagonal().real());
	     Eigen::VectorXd v6=(COMMmat.diagonal().real());
	    std::cout<< "GS "<<eigenVals(0)<<std::endl;
    	   for(int i=0; i<energy.size(); i++)
    {
      //obs[i]=real(obstot[i]);

      ensvec[i]=eigenVals(i);
      obs[i]=v(i);
      obs2[i]=v2(i);
      obs3[i]=v3(i);
      obs4[i]=v4(i);
      obs5[i]=v5(i);
      obs6[i]=v6(i);
      std::cout<< COMMmat(i,i)<<std::endl;
      if(gamma!=0)
	{	obs3[i]/=gamma;}

      //std::cout<< ensvec[i]<<'\n';
    }// 	   	    std::cout<< "gS "<<   eigenVals[0] <<std::endl;
    		    std::cout<< "mean "<<   eigenVals.mean() <<std::endl;
    	   std::vector<double> Ovec;
	    std::vector<double> Ovec2;
	     std::vector<double> Ovec3;
	     std::vector<double> Ovec4;
	     	     std::vector<double> Ovec5;

   	    	   std::vector<double> Evec;
   	    	   //		   std::vector<double> Tr={0.01, 0.05, 0.1, 0.15, 0.2, 0.25};
   	    	   		   std::vector<double> Tr;
   	    	   for(int i=1; i<22; i++)
   	    	     {
   	    	       Tr.push_back(i*0.05);

   	    	     }

   	    	  	  for(auto t: Tr){

   	    	 // //	      std::cout<< l<<std::endl;
   	         // bin_write(filename, l);
   	    	 // n++;
			    double e1=expvalCan(ensvec, ensvec,  t);
			    double o=expvalCan(ensvec, obs,  t);
			    double o2=expvalCan(ensvec, obs2,  t);
			    double o3=expvalCan(ensvec, obs3,  t);
			    double o4=expvalCan(ensvec, obs4,  t);
			     double o5=expvalCan(ensvec, obs5,  t);
			     double o6=expvalCan(ensvec, obs6,  t);
   	    		    std::cout<< " at T "<< t<<std::endl;
			    std::cout<<std::setprecision(15)<<e1 <<"  "<< o+o2+o3*gamma <<" Nph "<<o << " JJ " <<o4 << "o 6 2 "<< o6<< std::endl;
			    Ovec.push_back(o);
			    Ovec2.push_back(o2);
			    Ovec3.push_back(o3);
			    Ovec4.push_back(o4);
			    			    Ovec5.push_back(o5);
			    Evec.push_back(e1);

   	    }

   			  int pb=PB;
   		  	  std::string Hs="H";
			  std::string Ts="T";
			  std::string phds="PHD";

		      bin_write("E"+filename, Evec);
		      bin_write("Nph"+filename, Ovec);
		      bin_write("EK"+filename, Ovec2);
		      bin_write("nX"+filename, Ovec3);
		      bin_write("JJ"+filename, Ovec4);
		      bin_write("J"+filename, Ovec5);
		      bin_write("temp"+filename, Tr);
  return 0;
}


#include <iostream>
#include <NTL/RR.h>
#include "ring.h"
#include "she.h"
#include "util.h"
#include "params.h"

using namespace std;
using namespace NTL;

void encode_test (param& prm) {
  cout << "--- encode_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText x(prm);
    x.uniform();

    PlainText z(prm);
    encode_pt(prm, z, x);

    PlainText x1(prm);
    decode_pt(prm, x1, z);

    if (x != x1) {
      cout << "NG: encode_test: (" << cnt << ")" << endl;
      cout << " x = " << x << endl;
      cout << " z = " << z << endl;
      cout << "x1 = " << x1 << endl;
      exit(-1);
    }
  }

  cout << "OK: encode_test" << endl;
}

void dec_test (param& prm, const SKey& s) {
  cout << "--- dec_test for " << prm.label << " ---" << endl;

  for (long i = 0; i < 5; ++i) {
    PlainText m(prm), m1(prm);
    m.uniform();

    CipherText ct(prm);
    encrypt(prm, ct, s, m);

    if (dec_ng(prm, m1, s, ct, m)) {
      cout << "NG: dec" << endl;
      cout << "m  = " << m << endl;
      cout << "m1 = " << m1 << endl;
      exit(-1);
    }
  }

  cout << "OK: dec_test" << endl;
}

void div_by_2_test (param& prm, const SKey& s) {
  cout << "--- div_by_2_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m(prm), m1(prm), m2(prm);
    m.uniform();
    for (long i = 0; i < prm.rg.gR; ++i) m[i] = m[i] - (m[i] % 2);
    for (long i = 0; i < prm.rg.gR; ++i) m2[i] = m[i]/2;

    CipherText ct(prm);
    encrypt(prm, ct, s, m);

    CipherText dt(prm);
    div_by_2(dt, ct);

    ZZ v = decrypt(prm, m1, s, dt);
    for (long i = 0; i < dt.dim; ++i) {
      if (((m2[i] - m1[i]) % (ct.t/2)) != 0) {
	cout << "NG: div_by_2" << endl;
	cout << "be: " << VectorCopy(m2.data, 10) << endl;
	cout << "is: " << VectorCopy(m1.data, 10) << endl;
	exit(-1);
      }
    }
  }

  cout << "OK: div_by_2_test" << endl;
}

void add_test (param& prm, const SKey& s) {
  cout << "--- add_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    add(m3, m1, m2);

    CipherText ct1(prm), ct2(prm), ct3(prm);
    encrypt(prm, ct1, s, m1);
    encrypt(prm, ct2, s, m2);

    add_ct(ct3, ct1, ct2);
    if (dec_ng(prm, _m, s, ct3, m3)) {
      cout << "NG: add" << endl;
      cout << "  be: " << m3 << endl;
      cout << "  is: " << _m << endl;
      exit(-1);
    }
  }

  cout << "OK: add_test" << endl;
}

void plain_add_test (param& prm, const SKey& s) {
  cout << "--- plain_add_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    add(m3, m1, m2);

    CipherText ct2(prm), ct3(prm);
    encrypt(prm, ct2, s, m2);

    plain_add_ct(ct3, m1, ct2);
    if (dec_ng(prm, _m, s, ct3, m3)) {
      cout << "NG: plain_add_test" << endl;
      cout << "  be: " << VectorCopy(m3.data, 10) << endl;
      cout << "  is: " << VectorCopy(_m.data, 10)  << endl;
      exit(-1);
    }
  }

  cout << "OK: plain_add_test" << endl;
}

void plain_mult_test (param& prm, const SKey& s) {
  cout << "--- plain_mult_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    mult(m3, m1, m2);

    CipherText ct2(prm), ct3(prm);
    encrypt(prm, ct2, s, m2);

    plain_mult_ct(ct3, m1, ct2);
    if (dec_ng(prm, _m, s, ct3, m3)) {
      cout << "NG: plain_mult_test" << endl;
      cout << "  be: " << m3 << endl;
      cout << "  is: " << _m << endl;
      exit(-1);
    }
  }

  cout << "OK: plain_mult_test" << endl;
}

void hint_gen_test (param& prm, const SKey& s) {
  cout << "--- hint_gen_test for " << prm.label << " ---" << endl;
  ZZ_p::init(prm.qq);
  ZZ& sq = prm.sR;

  long test_num = 1;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    ZZ w(1);
    for (long j = 0; j < prm.lw; ++j) {
      cout << "-- " << j << " ------" << endl;
      vec_ZZ_p e;
      e.SetLength(prm.rg.gR);

      // e = H[j][0] + H[j][1] s.x
      mult(e, conv<vec_ZZ_p>(s.Hint.H[j][1]), conv<vec_ZZ_p>(s.x));
      add(e, conv<vec_ZZ_p>(s.Hint.H[j][0]), e);
      //prm.applyGammaInv(e, e);
      prm.applyOmegaInv(e, e);

      // f = sR s2 g^T
      vec_ZZ_p f =  conv<ZZ_p>(w) * conv<vec_ZZ_p>(s.s2);
      f = conv<ZZ_p>(sq) * f;

      e = e - f;

      vec_ZZ ee;
      ee.SetLength(prm.rg.gR);
      center_lift(ee, e);
      cout << "|e| = " << inf_norm(ee) << endl;

      w *= prm.w;
    }
  }

  //cout << "OK: hint_gen_test" << endl;
}

void hint_gen_test2 (param& prm, const SKey& s) {
  cout << "--- hint_gen_test2 for " << prm.label << " ---" << endl;
  ZZ& q0 = prm.qq;
  ZZ& q = prm.q;

  long test_num = 1;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    ZZ w(1);
    for (long j = 0; j < prm.lw; ++j) {
      cout << "-- " << j << " ------" << endl;

      ZZ_p::init(q0);

      vec_ZZ_p _e;
      _e.SetLength(prm.rg.gR);

      // e = H[j][0] + H[j][1] s.x
      mult(_e, conv<vec_ZZ_p>(s.Hint.H[j][1]), conv<vec_ZZ_p>(s.x));
      add(_e, conv<vec_ZZ_p>(s.Hint.H[j][0]), _e);
      //prm.applyGammaInv(_e, _e);
      prm.applyOmegaInv(_e, _e);

      vec_ZZ e;
      e.SetLength(prm.rg.gR);
      rescale(e, conv<vec_ZZ>(_e), q0, q);

      ZZ_p::init(q);

      // f = s2 g^T
      vec_ZZ_p f =  conv<ZZ_p>(w) * conv<vec_ZZ_p>(s.s2);

      vec_ZZ_p e1 = conv<vec_ZZ_p>(e) - f;
      vec_ZZ ee;
      ee.SetLength(prm.rg.gR);
      center_lift(ee, e1);
      cout << "|e| = " << inf_norm(ee) << endl;

      w *= prm.w;
    }
  }

  //cout << "OK: hint_gen_test2" << endl;
}

void decomp_test (param& prm) {
  cout << "--- decomp_test for " << prm.label << " ---" << endl;

  ZZ& q = prm.qq;
  ZZ_p::init(q);

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    vec_ZZ c;
    c.SetLength(prm.rg.gR);
    for (long i = 0; i < c.length(); ++i) c[i] = RandomBnd(q);

    Vec<vec_ZZ> d;
    d.SetLength(prm.lw);
    for (long l = 0; l < d.length(); ++l) d[l].SetLength(prm.rg.gR);
    Decomp(prm, d, c);

    vec_ZZ_p y;
    y.SetLength(prm.rg.gR);

    ZZ w(1);
    for (long l = 0; l < prm.lw; ++l) {
      vec_ZZ_p dd = conv<ZZ_p>(w) * conv<vec_ZZ_p>(d[l]);
      y += dd;
      w *= prm.w;
    }

    if (conv<vec_ZZ>(y) != c) {
      cout << "NG: decomp" << endl;
      cout << "  be: " << c[0] << endl;
      cout << "  is: " << (conv<vec_ZZ>(y))[0] << endl;
      exit(-1);
    }
  }

  cout << "OK: decomp_test" << endl;
}

// Typically, sin = s2x (s2=s^2), sout = sx
// -- H : encryption of sin
// -- d = apply_hint(H, c)  --> d(sout) ~ sin c
void hint_apply_test (param& prm, const hint Hint, const vec_ZZ& sin, const vec_ZZ& sout) {
  cout << "--- hint_apply_test for " << prm.label << " ---" << endl;
  long test_num = 3;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    ZZ& q = prm.q;
    ZZ_p::init(q);

    vec_ZZ a, b, c;
    c.SetLength(prm.rg.gR);
    for (long i = 0; i < c.length(); ++i) c[i] = RandomBnd(q);

    apply_hint(prm, a, b, Hint, c, q);

    vec_ZZ_p _a, _b, _e;
    _e.SetLength(prm.rg.gR);

    //prm.applyGamma(_a, conv<vec_ZZ_p>(a));
    prm.applyOmega(_a, conv<vec_ZZ_p>(a));
    prm.applyOmega(_b, conv<vec_ZZ_p>(b));
    mult(_e, _b, conv<vec_ZZ_p>(sout));
    _e = _a + _e;

    vec_ZZ_p _c;
    prm.applyOmega(_c, conv<vec_ZZ_p>(c));
    mult(_c, conv<vec_ZZ_p>(sin), _c);

    _c = _c - _e;
    //prm.applyGammaInv(_c, _c);
    prm.applyOmegaInv(_c, _c);

    center_lift(c, _c);
    cout << "inf_norm(noise) = " << inf_norm(c) << endl;
  }

  //cout << "OK: hint_apply_test" << endl;
}

void key_switch_test (param& prm, const SKey& s, const SKey& t, const hint& Hint) {
  cout << "--- key_switch_test for " << prm.label << " ---" << endl;

  long test_num = 3;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m(prm), m1(prm);
    m.uniform();
    CipherText ct(prm);
    encrypt(prm, ct, t, m);

    // ct/t --> dt/s
    CipherText dt(prm);
    key_switch(dt, Hint, ct);

    if (dec_ng(prm, m1, s, dt, m)) {
      cout << "NG: dec" << endl;
      cout << "be: " << VectorCopy(m.data, 10) << endl;
      cout << "is: " << VectorCopy(m1.data, 10) << endl;
      exit(-1);
    }
  }

  cout << "OK: key_switch_test" << endl;
}

void direct_mult_test (param& prm, const SKey& s) {
  cout << "--- direct_mult_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    mult(m3, m1, m2);

    CipherText ct1(prm), ct2(prm);
    encrypt(prm, ct1, s, m1);
    encrypt(prm, ct2, s, m2);

    CipherText2 dt(prm);
    direct_mult_ct(dt, ct1, ct2);

    if (dec_ng2(prm, _m, s, dt, m3)) {
      cout << "NG: direct_mult_test" << endl;
      cout << "  be: " << m3 << endl;
      cout << "  is: " << _m << endl;
      exit(-1);
    }
  }

  cout << "OK: direct_mult_test" << endl;
}

void mult_test (param& prm, const hint& Hint, const SKey& s) {
  cout << "--- mult_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m2(prm), m3(prm), _m(prm);
    m1.uniform();
    m2.uniform();
    mult(m3, m1, m2);

    CipherText ct1(prm), ct2(prm);
    encrypt(prm, ct1, s, m1);
    encrypt(prm, ct2, s, m2);

    CipherText dt(prm);
    mult_ct(dt, Hint, ct1, ct2);

    if (dec_ng(prm, _m, s, dt, m3)) {
      cout << "NG: mult_test" << endl;
      cout << "  be: " << VectorCopy(m3.data, 10) << endl;
      cout << "  is: " << VectorCopy(_m.data, 10) << endl;
      exit(-1);
    }
  }

  cout << "OK: mult_test" << endl;
}

void square_test (param& prm, const hint& Hint, const SKey& s) {
  cout << "--- square_test for " << prm.label << " ---" << endl;

  long test_num = 5;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    PlainText m1(prm), m3(prm), _m(prm);
    m1.uniform();
    mult(m3, m1, m1);

    CipherText ct1(prm);
    encrypt(prm, ct1, s, m1);

    CipherText dt(prm);
    //square_ct(dt, Hint, ct1);
    square_ct_debug(dt, Hint, ct1, s);

    if (dec_ng(prm, _m, s, dt, m3)) {
      cout << "NG: square_test" << endl;
      cout << "  be: " << VectorCopy(m3.data, 10) << endl;
      cout << "  is: " << VectorCopy(_m.data, 10) << endl;
      exit(-1);
    }
  }

  cout << "OK: square_test" << endl;
}

void power_test (param& prm, const hint& Hint, const SKey& s, const long n) {
  cout << "--- power_test for " << prm.label << " ---" << endl;

  long test_num = 3;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    cout << "[" << cnt << "]-----------------------" << endl;

    Vec<PlainText> m;
    for (long i = 0; i < n + 1; ++i) {
      PlainText _m(prm);
      append(m, _m);
    }

    Vec<CipherText> ct;
    for (long i = 0; i < n + 1; ++i) {
      CipherText _ct(prm);
      append(ct, _ct);
    }

    m[0].uniform();
    encrypt(prm, ct[0], s, m[0]);

    PlainText _m(prm);
    if (dec_ng(prm, _m, s, ct[0], m[0])) {
      cout << "NG: power_test" << endl;
      cout << "  be: " << VectorCopy(m[0].data,10) << endl;
      cout << "  is: " << VectorCopy(_m.data,10) << endl;
      exit(-1);
    }

    for (long k = 1; k < n+1; ++k) {
      cout << "-- " << k << " --------------------" << endl;
      mult(m[k], m[k-1], m[k-1]);
      //cout << "m[" << k << "] = " << VectorCopy(m[k].data,10) << endl;

      //square_ct_debug(ct[k], Hint, ct[k-1], s);
      square_ct(ct[k], Hint, ct[k-1]);
      if (dec_ng(prm, _m, s, ct[k], m[k])) {
	cout << "NG: power_test" << endl;
	cout << "  be: " << VectorCopy(m[k].data,10) << endl;
	cout << "  is: " << VectorCopy(_m.data,10) << endl;
	exit(-1);
      }
    }
  }

  cout << "OK: power_test" << endl;
}

void hom_test (param& prm, const hint& Hint, const SKey& s, long L) {
  cout << "--- hom_test for " << prm.label << " --- : level = " << L << endl;

  long test_num = 1;
  for (long cnt = 0; cnt < test_num; ++cnt) {
    cout << "[" << cnt << "]-----------------------" << endl;

    Vec<Vec<long>> op;
    op.SetLength(L+1);
    for (long j = 0; j < L + 1; ++j) op[j].SetLength(pow(2,L));

    // plain computation
    Vec<Vec<PlainText>> m;
    for (long j = 0; j < L + 1; ++j) {
      Vec<PlainText> _mm;
      for (long i = 0; i < pow(2,L); ++i) {
	PlainText _m(prm);
	append(_mm, _m);
      }
      append(m, _mm);
    }

    for (long i = 0; i < pow(2,L); ++i) m[0][i].uniform();

    for (long j = 1; j < L + 1; ++j) {
      for (long i = 0; i < pow(2,L-j); ++i) {
	op[j][i] = RandomBnd(2);
	if (op[j][i]) add(m[j][i], m[j-1][2*i], m[j-1][2*i+1]);
	else mult(m[j][i], m[j-1][2*i], m[j-1][2*i+1]);
      }
    }

    // homomorphic computation
    Vec<Vec<CipherText>> ct;
    for (long j = 0; j < L + 1; ++j) {
      Vec<CipherText> _cc;
      for (long i = 0; i < pow(2,L); ++i) {
	CipherText _ct(prm);
	append(_cc, _ct);
      }
      append(ct, _cc);
    }

    for (long i = 0; i < pow(2,L); ++i) encrypt(prm, ct[0][i], s, m[0][i]);

    for (long j = 1; j < L + 1; ++j) {
      cout << "-- level " << j << endl;
      for (long i = 0; i < pow(2,L-j); ++i) {
	if (op[j][i]) {
	  cout << "ct[" << j-1 << "][" << 2*i << "] + ct[" << j-1 << "][" << 2*i+1 << "]" << endl;
	  add_ct(ct[j][i], ct[j-1][2*i], ct[j-1][2*i+1]);
	}
	else {
	  cout << "ct[" << j-1 << "][" << 2*i << "] * ct[" << j-1 << "][" << 2*i+1 << "]" << endl;
	  mult_ct(ct[j][i], Hint, ct[j-1][2*i], ct[j-1][2*i+1]);
	}

	PlainText _m(prm);
	if (dec_ng(prm, _m, s, ct[j][i], m[j][i])) {
	  cout << "NG: hom_test" << endl;
	  cout << "  be: " << m[j][i] << endl;
	  cout << "  is: " << _m << endl;
	  exit(-1);
	}
      }
    }
  }

  cout << "OK: hom_test" << endl;
}

void she_test (param& prm) {
  cout << "--- she_test for " << prm << " ---" << endl;

  cout << "generating a secret key..." << flush;
  SKey s(prm);
  KeyGen(prm, s);
  cout << "done" << endl;

  encode_test(prm);
  dec_test(prm, s);

  div_by_2_test(prm, s);

  add_test(prm, s);
  plain_add_test(prm, s);

  if (prm.level > 1) {
    plain_mult_test(prm, s);

    hint_gen_test(prm, s);
    hint_gen_test2(prm, s);
    decomp_test(prm);

    hint_apply_test(prm, s.Hint, s.s2x, s.x);

    SKey t(prm);
    KeyGen(prm, t);

    hint Hint;
    Hint.hint_gen(prm, t.data, s.x);
    hint_apply_test(prm, Hint, t.x, s.x);
    key_switch_test(prm, s, t, Hint);

    direct_mult_test(prm, s);
    mult_test(prm, s.Hint, s);
    square_test(prm, s.Hint, s);

    //hom_test(prm, s.Hint, s, prm.level-1);
    power_test(prm, s.Hint, s, prm.level-1);
  }
}

void check_modulus_switch (param& prm, param& prm_tmp) {
  cout << "check_modulus_switch: " << flush;

  // the original secret key
  SKey s(prm);
  KeyGen(prm, s);

  // the corresponding secret key in prm_tmp
  SKey s_tmp(prm_tmp);
  KeyGen(prm_tmp, s_tmp, s.data);  // sx = s_tmp.x

  PlainText m(prm);
  CipherText ct(prm), dt(prm_tmp);

  m.uniform();
  encrypt(prm, ct, s, m);

  modulus_switch(dt, prm_tmp, prm_tmp.q, prm, ct);

  // check the result
  PlainText _m(prm_tmp);
  if (dec_ng(prm_tmp, _m, s_tmp, dt, m)) {
    cout << "NG: check_modulus_switch: " << endl;
    cout << "  be: " << VectorCopy(m.data,10) << endl;
    cout << "  is: " << VectorCopy(_m.data,10) << endl;
    exit(-1);
  }
  cout << "-- OK" << endl;
}

int main (int argc, char *argv[]) {
  /* Do Test */
  //long prec = 2*left_param_big.r - left_param_big.l;
  //long prec = 2*right_param_big.r - right_param_big.l;
  long prec = 2*composed_param_big.r - composed_param_big.l;

  // left_param.init(prec);
  // she_test(left_param);

  // left_param_tmp.init(prec);
  // she_test(left_param_tmp);

  // left_param_big.init(prec);
  // she_test(left_param_big);

  // right_param.init(prec);
  // she_test(right_param);

  // right_param_tmp.init(prec);
  // she_test(right_param_tmp);

  // right_param_big.init(prec);
  // she_test(right_param_big);

  composed_param.init(prec);
  she_test(composed_param);

  composed_param_tmp.init(prec);
  she_test(composed_param_tmp);

  composed_param_big.init(prec);
  she_test(composed_param_big);

  check_modulus_switch(composed_param, composed_param_tmp);
}


#pragma once

#include <Eigen/Dense>
#include <cmath>

/**
 * @brief shift zero frequency component to the center of the spectrum
 *
 * @param dst
 * @param src
 * @param dim (default both directions), 0: shift along rows, 1: shift along
 * columns
 */
template <typename DERIVED1, typename DERIVED2>
inline void
fftshift(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, int dim = -1)
{
  // nx, ny: position of the smallest negative frequency
  unsigned int nx = std::ceil(src.cols() / 2.);
  unsigned int ny = std::ceil(src.rows() / 2.);

  unsigned int Nx = src.cols();
  unsigned int Ny = src.rows();

  assert(dst.rows() == src.rows() && dst.cols() == src.cols());

  if (dim == -1 && dst.cols() > 1 && dst.rows() > 1) {
    // diagonal blocks
    dst.block(0, 0, Ny - ny, Nx - nx) = src.bottomRightCorner(Ny - ny, Nx - nx);
    dst.bottomRightCorner(ny, nx) = src.block(0, 0, ny, nx);

    // off-diagonal blocks
    dst.topRightCorner(Ny - ny, nx) = src.bottomLeftCorner(Ny - ny, nx);
    dst.bottomLeftCorner(ny, Nx - nx) = src.topRightCorner(ny, Nx - nx);
  } else if (dim == 0) {
    assert(Ny > 1);
    /*  shift along rows  */
    // 2d, shift along rows (y-dim)
    dst.topRows(Ny - ny) = src.bottomRows(Ny - ny);
    dst.bottomRows(ny) = src.topRows(ny);
  } else if (dim == 1) {
    assert(Nx > 1);
    /* shift along columns  */
    // 2d, shift along columns (x-dim)
    // negative frequencies
    dst.leftCols(Nx - nx) = src.rightCols(Nx - nx);
    // positive frequencies
    dst.rightCols(nx) = src.leftCols(nx);
  } else {
    assert(false);
  }
}

/**
 * @brief undoes the effect of fftshift
 *
 * @param dst
 * @param src
 * @param dim
 */
template <typename DERIVED1, typename DERIVED2>
inline void
ifftshift(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, int dim = -1)
{
  // nx, ny: position of the smallest negative frequency
  unsigned int nx = std::floor(src.cols() / 2.);
  unsigned int ny = std::floor(src.rows() / 2.);

  unsigned int Nx = src.cols();
  unsigned int Ny = src.rows();

  assert(dst.rows() == src.rows() && dst.cols() == src.cols());

  if (dim == -1 && dst.cols() > 1 && dst.rows() > 1) {
    // diagonal blocks
    dst.block(0, 0, Ny - ny, Nx - nx) = src.bottomRightCorner(Ny - ny, Nx - nx);
    dst.bottomRightCorner(ny, nx) = src.block(0, 0, ny, nx);

    // off-diagonal blocks
    dst.topRightCorner(Ny - ny, nx) = src.bottomLeftCorner(Ny - ny, nx);
    dst.bottomLeftCorner(ny, Nx - nx) = src.topRightCorner(ny, Nx - nx);
  } else if (dim == 0) {
    // 2d, shift along rows (y-dim)
    dst.topRows(Ny - ny) = src.bottomRows(Ny - ny);
    dst.bottomRows(ny) = src.topRows(ny);
  } else if (dim == 1) {
    // 2d, shift along columns (x-dim)
    // negative frequencies
    dst.leftCols(Nx - nx) = src.rightCols(Nx - nx);
    // positive frequencies
    dst.rightCols(nx) = src.leftCols(nx);
  }
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <tuple>
#include <random>
#include <iterator>
#include <numeric>

#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "VNS.hpp"
#include "MODI.hpp"
#include "Solution.hpp"
#include "Helper.hpp"

using namespace std;
using namespace t_simplex;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*                            UPDATE XIJ PROCEDURES                              */
/*********************************************************************************/

// CFLP with Modi procedure (gets optimal solution in deterministic time)
bool VNS::updateXij(
	ublas::matrix<double> &cij,
	const vector<double> &dj,
	const vector<double> &bi,
	const vector<bool> &yi,
	const unsigned int &update_xij_mode,
	double &fx)
{
	if (!canUpdateXij(bi, yi, m_sum_dj))
		return false;

	unsigned int open_facilities = 0, i_out = 0, flow_vars = 0;
	double transportation_cost = 0.0, gap = 0.0, sum_bi = 0.0;
	bool check = false;

	open_facilities = accumulate(yi.begin(), yi.end(), 0);

	ublas::matrix<double> custom_cij(open_facilities, m_customerNumber, 0.0);

	int *customers = new int[m_customerNumber];
	double *demand = new double[m_customerNumber];
	int *facilities = new int[open_facilities];
	double *capacity = new double[open_facilities];

	for (size_t j = 0; j != cij.size2(); ++j)
	{
		customers[j] = j;
		demand[j] = dj[j];
	}

	for (size_t i = 0; i != cij.size1(); ++i)
	{
		if (yi[i] == 1)
		{
			facilities[i_out] = i;
			capacity[i_out] = bi[i];
			sum_bi += bi[i];

			ublas::matrix_row<ublas::matrix<double>> row_custom_cij(custom_cij, i_out);
			ublas::matrix_row<ublas::matrix<double>> row_cij(cij, i);
			row_custom_cij = row_cij;

			i_out++;
		}
	}

	// Signature of Facilities and Customers
	TsSignature *facility = new TsSignature(open_facilities, facilities, capacity);
	TsSignature *customer = new TsSignature(m_customerNumber, customers, demand);
	// Save Stepping Stone Path
	TsFlow *flow = new TsFlow[open_facilities + m_customerNumber - 1];

	// Result value
	transportation_cost = t_simplex::transportSimplex(update_xij_mode, custom_cij, flow,
		&flow_vars, facility, customer ,m_sum_dj, sum_bi, dj);

	// f(x) and Return Value
	fx = f(yi, m_fi, transportation_cost);

	// Flow Xij for checkSolution
	m_flow_tpl.resize(flow_vars);
	for (size_t i = 0; i < flow_vars; ++i)
		m_flow_tpl[i] = make_tuple(facilities[flow[i].from], flow[i].to, flow[i].amount);

	check = checkSolution(yi, m_flow_tpl, flow_vars, cij, bi, dj, m_sum_dj);

	delete[] capacity;
	capacity = NULL;
	delete[] demand;
	demand = NULL;
	delete[] facilities;
	facilities = NULL;
	delete[] customers;
	customers = NULL;
	delete facility;
	facility = NULL;
	delete customer;
	customer = NULL;

	return check;
}


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CLAPACK_HPP
#define BOOST_NUMERIC_BINDINGS_CLAPACK_HPP

#include <cassert>
#include <new>

#include <boost/numeric/bindings/traits/traits.hpp>
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
#  include <boost/numeric/bindings/traits/detail/symm_herm_traits.hpp>
#endif
#include <boost/numeric/bindings/atlas/cblas_enum.hpp>

// see libs/numeric/bindings/atlas/doc/index.html, section 2.5.2
//#define BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

#include <boost/numeric/bindings/atlas/clapack_overloads.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/same_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace atlas {

    /////////////////////////////////////////////////////////////////////
    //
    // general system of linear equations A * X = B
    //
    /////////////////////////////////////////////////////////////////////

    // gesv(): 'driver' function
    //
    // [comments from 'clapack_dgesv.c':]
    /* clapack_xgesv computes the solution to a system of linear equations
     *   A * X = B,
     * where A is an N-by-N matrix and X and B are N-by-NRHS matrices.
     */
    // [but ATLAS FAQ says:]
    /* What's the deal with the RHS in the row-major factorization/solves?
     * Most users are confused by the row major factorization and related
     * solves. The right-hand side vectors are probably the biggest source
     * of confusion. The RHS array does not represent a matrix in the
     * mathematical sense, it is instead a pasting together of the various
     * RHS into one array for calling convenience. As such, RHS vectors are
     * always stored contiguously, regardless of the row/col major that is
     * chosen. This means that ldb/ldx is always independent of NRHS, and
     * dependant on N, regardless of the row/col major setting.
     */
    // That is, it seems that, if B is row-major, it should be NRHS-by-N,
    // and RHS vectors should be its rows, not columns.
    //
    // [comments from 'clapack_dgesv.c':]
    /* The LU factorization used to factor A is dependent on the Order
     * parameter, as detailed in the leading comments of clapack_dgetrf.
     * The factored form of A is then used to solve the system of equations
     *   A * X = B.
     * A is overwritten with the appropriate LU factorization, and B [...]
     * is overwritten with the solution X on output.
     */
    // If B is row-major, solution vectors are its rows.
    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int gesv (MatrA& a, IVec& ipiv, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      int const nrhs = stor_ord == CblasColMajor
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      assert (n == traits::matrix_size2 (a));
      assert (n == (stor_ord == CblasColMajor
                    ? traits::matrix_size1 (b)
                    : traits::matrix_size2 (b)));
      assert (n == traits::vector_size (ipiv));

      return detail::gesv (stor_ord, n, nrhs,
                           traits::matrix_storage (a),
                           traits::leading_dimension (a),
                           traits::vector_storage (ipiv),
                           traits::matrix_storage (b),
                           traits::leading_dimension (b));
    }

    template <typename MatrA, typename MatrB>
    inline
    int gesv (MatrA& a, MatrB& b) {
      // with 'internal' pivot vector
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      int const nrhs = stor_ord == CblasColMajor
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      assert (n == traits::matrix_size2 (a));
      assert (n == (stor_ord == CblasColMajor
                    ? traits::matrix_size1 (b)
                    : traits::matrix_size2 (b)));

      int *ipiv = new (std::nothrow) int[n];
      int ierr = -101;
      // clapack_dgesv() errors:
      //   if (ierr == 0), successful
      //   if (ierr < 0), the -ierr argument had an illegal value
      //   -- we will use -101 if allocation fails
      //   if (ierr > 0), U(i-1,i-1) (or L(i-1,i-1)) is exactly zero

      if (ipiv) {
        ierr = detail::gesv (stor_ord, n, nrhs,
                             traits::matrix_storage (a),
                             traits::leading_dimension (a),
                             ipiv,
                             traits::matrix_storage (b),
                             traits::leading_dimension (b));
        delete[] ipiv;
      }
      return ierr;
    }

    template <typename MatrA, typename MatrB>
    inline
    int lu_solve (MatrA& a, MatrB& b) {
      return gesv (a, b);
    }

    // getrf(): LU factorization of A
    // [comments from 'clapack_dgetrf.c':]
    /* Computes one of two LU factorizations based on the setting of
     * the Order parameter, as follows:
     * ---------------------------------------------------------------
     *                     Order == CblasColMajor
     * Column-major factorization of form
     *   A = P * L * U
     * where P is a row-permutation matrix, L is lower triangular with
     * unit diagonal elements (lower trapezoidal if M > N), and U is
     * upper triangular (upper trapezoidal if M < N).
     * ---------------------------------------------------------------
     *                     Order == CblasRowMajor
     * Row-major factorization of form
     *   A = P * L * U
     * where P is a column-permutation matrix, L is lower triangular
     * (lower trapezoidal if M > N), and U is upper triangular with
     * unit diagonals (upper trapezoidal if M < N).
     */
    template <typename MatrA, typename IVec>
    inline
    int getrf (MatrA& a, IVec& ipiv) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (traits::vector_size (ipiv) == (m < n ? m : n));

      return detail::getrf (stor_ord, m, n,
                            traits::matrix_storage (a),
                            traits::leading_dimension (a),
                            traits::vector_storage (ipiv));
    }

    template <typename MatrA, typename IVec>
    inline
    int lu_factor (MatrA& a, IVec& ipiv) {
      return getrf (a, ipiv);
    }

    // getrs(): solves a system of linear equations
    //          A * X = B  or  A' * X = B
    //          using the LU factorization previously computed by getrf()
    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int getrs (CBLAS_TRANSPOSE const Trans,
               MatrA const& a, IVec const& ipiv, MatrB& b)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        typename traits::matrix_traits<MatrB>::ordering_type
      >::value));
#endif

      assert (Trans == CblasNoTrans
              || Trans == CblasTrans
              || Trans == CblasConjTrans);

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      int const nrhs = stor_ord == CblasColMajor
        ? traits::matrix_size2 (b)
        : traits::matrix_size1 (b);
      assert (n == traits::matrix_size2 (a));
      assert (n == (stor_ord == CblasColMajor
                    ? traits::matrix_size1 (b)
                    : traits::matrix_size2 (b)));
      assert (n == traits::vector_size (ipiv));

      return detail::getrs (stor_ord, Trans, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::matrix_storage (a),
#else
                            traits::matrix_storage_const (a),
#endif
                            traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::vector_storage (ipiv),
#else
                            traits::vector_storage_const (ipiv),
#endif
                            traits::matrix_storage (b),
                            traits::leading_dimension (b));
    }

    // getrs(): solves A * X = B (after getrf())
    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int getrs (MatrA const& a, IVec const& ipiv, MatrB& b) {
      return getrs (CblasNoTrans, a, ipiv, b);
    }

    template <typename MatrA, typename MatrB, typename IVec>
    inline
    int lu_substitute (MatrA const& a, IVec const& ipiv, MatrB& b) {
      return getrs (CblasNoTrans, a, ipiv, b);
    }

    // getri(): computes the inverse of a matrix A
    //          using the LU factorization previously computed by getrf()
    template <typename MatrA, typename IVec>
    inline
    int getri (MatrA& a, IVec const& ipiv) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_ORDER const stor_ord
        = enum_cast<CBLAS_ORDER const>
        (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
           typename traits::matrix_traits<MatrA>::ordering_type
#else
           typename MatrA::orientation_category
#endif
         >::value);

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (traits::vector_size (ipiv) == n);

      return detail::getri (stor_ord, n,
                            traits::matrix_storage (a),
                            traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::vector_storage (ipiv)
#else
                            traits::vector_storage_const (ipiv)
#endif
                            );
    }

    template <typename MatrA, typename IVec>
    inline
    int lu_invert (MatrA& a, IVec& ipiv) {
      return getri (a, ipiv);
    }

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A symmetric or Hermitian positive definite matrix
    //
    /////////////////////////////////////////////////////////////////////

#ifndef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
    // posv(): 'driver' function
    //
    // [from 'dposv.f' (slightly edited):]
    /* XPOSV computes the solution to a system of linear equations
     *    A * X = B,
     * where A is an N-by-N symmetric/Hermitian positive definite matrix
     * and X and B are N-by-NRHS matrices. [See also comments of gesv().]
     *
     * A -- On entry, the symmetric/Hermitian matrix A.
     * If UPLO = 'U', the leading N-by-N upper triangular part of A
     * contains the upper triangular part of the matrix A, and the
     * strictly lower triangular part of A is not referenced.
     * If UPLO = 'L', the leading N-by-N lower triangular part of A
     * contains the lower triangular part of the matrix A, and the
     * strictly upper triangular part of A is not referenced.
     *
     * On exit, if INFO = 0, the factor U or L from the Cholesky
     * factorization A = U**T*U or A = L*L**T
     * [or A = U**H*U or A = L*L**H].
     *
     * B -- On entry, the right hand side matrix B.
     * On exit, if INFO = 0, the solution matrix X.
     */
    namespace detail {

      template <typename SymmA, typename MatrB>
      inline
      int posv (CBLAS_UPLO const uplo, SymmA& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::ordering_type,
          typename traits::matrix_traits<MatrB>::ordering_type
        >::value));
#endif

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        int const nrhs = stor_ord == CblasColMajor
          ? traits::matrix_size2 (b)
          : traits::matrix_size1 (b);
        assert (n == traits::matrix_size2 (a));
        assert (n == (stor_ord == CblasColMajor
                      ? traits::matrix_size1 (b)
                      : traits::matrix_size2 (b)));

        return posv (stor_ord, uplo, n, nrhs,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::matrix_storage (b),
                     traits::leading_dimension (b));
      }

    } // detail

    template <typename SymmA, typename MatrB>
    inline
    int posv (CBLAS_UPLO const uplo, SymmA& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::posv (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int posv (SymmA& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::posv (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int cholesky_solve (SymmA& a, MatrB& b) { return posv (a, b); }
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

    // potrf(): Cholesky factorization of A
    namespace detail {

      template <typename SymmA>
      inline
      int potrf (CBLAS_UPLO const uplo, SymmA& a) {

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));

        return potrf (stor_ord, uplo, n,
                      traits::matrix_storage (a),
                      traits::leading_dimension (a));
      }

    } // detail

    template <typename SymmA>
    inline
    int potrf (CBLAS_UPLO const uplo, SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::potrf (uplo, a);
    }

    template <typename SymmA>
    inline
    int potrf (SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::potrf (uplo, a);
    }

    template <typename SymmA>
    inline
    int cholesky_factor (SymmA& a) { return potrf (a); }

    // potrs(): solves a system of linear equations A * X = B
    //          using the Cholesky factorization computed by potrf()
    namespace detail {

      template <typename SymmA, typename MatrB>
      inline
      int potrs (CBLAS_UPLO const uplo, SymmA const& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::ordering_type,
          typename traits::matrix_traits<MatrB>::ordering_type
        >::value));
#endif

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        int const nrhs = stor_ord == CblasColMajor
          ? traits::matrix_size2 (b)
          : traits::matrix_size1 (b);
        assert (n == traits::matrix_size2 (a));
        assert (n == (stor_ord == CblasColMajor
                      ? traits::matrix_size1 (b)
                      : traits::matrix_size2 (b)));

#ifndef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
        return potrs (stor_ord, uplo, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                      traits::matrix_storage (a),
#else
                      traits::matrix_storage_const (a),
#endif
                      traits::leading_dimension (a),
                      traits::matrix_storage (b),
                      traits::leading_dimension (b));
#else // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
        int ierr;
        if (stor_ord == CblasColMajor)
          ierr = potrs (stor_ord, uplo, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                        traits::matrix_storage (a),
#else
                        traits::matrix_storage_const (a),
#endif
                        traits::leading_dimension (a),
                        traits::matrix_storage (b),
                        traits::leading_dimension (b));
        else // ATLAS bug with CblasRowMajor
          ierr = potrs_bug (stor_ord, uplo, n, nrhs,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::matrix_storage (a),
#else
                            traits::matrix_storage_const (a),
#endif
                            traits::leading_dimension (a),
                            traits::matrix_storage (b),
                            traits::leading_dimension (b));
        return ierr;
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
      }

    } // detail

    template <typename SymmA, typename MatrB>
    inline
    int potrs (CBLAS_UPLO const uplo, SymmA const& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::potrs (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int potrs (SymmA const& a, MatrB& b) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::potrs (uplo, a, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int cholesky_substitute (SymmA const& a, MatrB& b) { return potrs (a, b); }

#ifdef BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG
    // posv(): 'driver' function
    template <typename SymmA, typename MatrB>
    inline
    int posv (CBLAS_UPLO const uplo, SymmA& a, MatrB& b) {
      int ierr = potrf (uplo, a);
      if (ierr == 0)
        ierr = potrs (uplo, a, b);
      return ierr;
    }

    template <typename SymmA, typename MatrB>
    inline
    int posv (SymmA& a, MatrB& b) {
      int ierr = potrf (a);
      if (ierr == 0)
        ierr = potrs (a, b);
      return ierr;
    }

    template <typename SymmA, typename MatrB>
    inline
    int cholesky_solve (SymmA& a, MatrB& b) {
      return posv (a, b);
    }
#endif // BOOST_NUMERIC_BINDINGS_ATLAS_POTRF_BUG

    // potri(): computes the inverse of a symmetric or Hermitian positive
    //          definite matrix A using the Cholesky factorization
    //          previously computed by potrf()
    namespace detail {

      template <typename SymmA>
      inline
      int potri (CBLAS_UPLO const uplo, SymmA& a) {

        CBLAS_ORDER const stor_ord
          = enum_cast<CBLAS_ORDER const>
          (storage_order<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
            typename traits::matrix_traits<SymmA>::ordering_type
#else
            typename SymmA::orientation_category
#endif
           >::value);

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));

        return potri (stor_ord, uplo, n,
                      traits::matrix_storage (a),
                      traits::leading_dimension (a));
      }

    } // detail

    template <typename SymmA>
    inline
    int potri (CBLAS_UPLO const uplo, SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (uplo == CblasUpper || uplo == CblasLower);
      return detail::potri (uplo, a);
    }

    template <typename SymmA>
    inline
    int potri (SymmA& a) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        typename traits::detail::symm_herm_t<val_t>::type
      >::value));
#endif

      CBLAS_UPLO const uplo
        = enum_cast<CBLAS_UPLO const>
        (uplo_triang<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
          typename traits::matrix_traits<SymmA>::uplo_type
#else
          typename SymmA::packed_category
#endif
         >::value);

      return detail::potri (uplo, a);
    }

    template <typename SymmA>
    inline
    int cholesky_invert (SymmA& a) { return potri (a); }

  } // namespace atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CLAPACK_HPP