// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_POWER_EXPLICIT_INCLUDE
#define MTL_POWER_EXPLICIT_INCLUDE

#include <boost/numeric/linear_algebra/algebraic_concepts.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>

namespace mtl {

template <typename Op, typename Element, typename Exponent>
  _GLIBCXX_WHERE( std::Integral<Exponent>
	    && std::Callable2<Op, Element, Element>
	    && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>)
inline Element power(const Element& base, Exponent n, Op op)
{
    if (n < 1) throw "In power: exponent must be greater than 0";
    // std::cout << "[Magma] ";

    Element value= base;
    for (; n > 1; --n)
	value= op(value, base);
    return value;
}

# ifndef __GXX_CONCEPTS__
#   ifdef LA_SHOW_WARNINGS
#     warning "Automatic dispatching only works with concept compiler"
#     warning "If structure is a Monoid you can call square_and_multiply directly"
#   endif
# else

template <typename Op, typename Element, typename Exponent>
    where algebra::SemiGroup<Op, Element> && std::Integral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
inline Element power(const Element& base, Exponent n, Op op)
{
    // std::cout << "[SemiGroup] ";

    if (n <= 0) throw "In recursive_multiply_and_square: exponent must greater than 0";

    Exponent half= n >> 1;

    // If halt is 0 then n must be 1 and the result is base
    if (half == 0)
	return base;

    // compute power of downward rounded exponent and square the result
    Element value= power(base, half, op);
    value= op(value, value);

    // if odd another multiplication with base is needed
    if (n & 1)
	value= op(value, base);
    return value;
}

// {Op, Element} must be a Monoid
template <typename Op, typename Element, typename Exponent>
    where algebra::Monoid<Op, Element>
          && std::Integral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
// && std::Assignable<Element, algebdra::Monoid<Op, Element>::identity_result_type>
          && std::Assignable<Element, Element>
inline Element multiply_and_square(const Element& base, Exponent n, Op op)
{
    // Same as the simpler form except that the first multiplication is made before
    // the loop and one squaring is saved this way
    if (n < 0) throw "In multiply_and_square: negative exponent";

    using math::identity;
    Element value= identity(op, base), square= identity(op, base);

    if (n & 1)
        value= base;

    for (n>>= 1; n > 0; n>>= 1) {
	square= op(square, square);
	if (n & 1)
	    value= op(value, square);
    }
    return value;
}

template <typename Op, typename Element, typename Exponent>
    where algebra::Monoid<Op, Element> && std::Integral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
          && std::Assignable<Element, Element>
inline Element power(const Element& base, Exponent n, Op op)
{
    return multiply_and_square(base, n, op);
}

template <typename Op, typename Element, typename Exponent>
    where algebra::Group<Op, Element> && std::SignedIntegral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
          && std::Assignable<Element, Element>
inline Element power(const Element& base, Exponent n, Op op)
{
    using math::inverse;

    return n >= 0 ? multiply_and_square(base, n, op)
	          : multiply_and_square(inverse(op, base), -n, op);
}

# endif   // __GXX_CONCEPTS__

} // namespace mtl

#endif // MTL_POWER_EXPLICIT_INCLUDE


#define BOOST_TEST_MODULE "test_lennard_jones_wall_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/external/LennardJonesWallPotential.hpp>

BOOST_AUTO_TEST_CASE(LennardJonesWallPotential_double)
{
    using real_type = double;
    constexpr static std::size_t N   = 10000;
    constexpr static real_type   h   = 1e-6;
    constexpr static real_type   tol = 1e-5;

    const real_type sigma   = 1.0;
    const real_type epsilon = 1.0;
    const std::vector<std::pair<std::size_t, std::pair<real_type, real_type>>>
        sigma_epsilon{{0u, {sigma, epsilon}}};

    mjolnir::LennardJonesWallPotential<real_type> ljw(2.5, sigma_epsilon);

    const real_type cutoff_length = ljw.max_cutoff_length();
    const real_type z_min         = sigma * 0.5;
    const real_type z_max         = cutoff_length;
    const real_type dz            = (z_max - z_min) / N;

    real_type z = z_min;
    for(std::size_t i = 0; i < N; ++i)
    {
        const real_type pot1 = ljw.potential(0, z + h);
        const real_type pot2 = ljw.potential(0, z - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = ljw.derivative(0, z);

        if(std::abs(deri) > tol)
        {
            BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
        }
        else
        {
            BOOST_TEST(deri == 0.0, boost::test_tools::tolerance(tol));
        }
        z += dz;
    }
}


// BSD 3-Clause License

// Copyright (c) 2020, Chenyu
// All rights reserved.

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

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

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

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

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

#include "clustering/spectral_cluster.h"

#include <Spectra/SymEigsSolver.h>

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

#include "clustering/kmeans.h"
// #include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
// #include <Spectra/MatOp/SparseGenMatProd.h>
#include <glog/logging.h>

#include "util/timer.h"

namespace DAGSfM {

std::unordered_map<int, int> SpectralCluster::ComputeCluster(
    const std::vector<std::pair<int, int>>& edges,
    const std::vector<int>& weights, const int num_partitions) {
  if (num_partitions == 1) {
    for (auto node : nodes_) {
      labels_[node] = 0;
    }
    return labels_;
  }

  colmap::Timer timer;
  std::vector<Eigen::Triplet<double>> s_triplets;
  std::unordered_map<int, int> degrees;
  const int k = num_partitions;
  cluster_num_ = num_partitions;

  // 1. Compute similarity graph.
  for (uint i = 0; i < nodes_.size(); i++) {
    node_mapper_[nodes_[i]] = i;
  }

  timer.Start();
  const int N = nodes_.size();
  Eigen::SparseMatrix<double> S(N, N);
  for (uint i = 0; i < edges.size(); i++) {
    int src = node_mapper_[edges[i].first], dst = node_mapper_[edges[i].second];
    s_triplets.push_back(Eigen::Triplet<double>(src, dst, weights[i]));
    s_triplets.push_back(Eigen::Triplet<double>(dst, src, weights[i]));
    degrees[src] += 1;
    degrees[dst] += 1;
  }

  S.setFromTriplets(s_triplets.begin(), s_triplets.end());
  S.makeCompressed();
  timer.Pause();
  LOG(INFO) << "1. Similarity Graph Computation Time: "
            << timer.ElapsedSeconds();

  // 2. Compute Laplacian matrix.
  timer.Start();
  Eigen::SparseMatrix<double> L = ComputeLaplacian(S, degrees);
  L.makeCompressed();
  timer.Pause();
  LOG(INFO) << "2. Laplacian Matrix Computation Time: "
            << timer.ElapsedSeconds();

  // 3. Compute the top-k smallest eigen values and corresponding eigen vectors.
  timer.Start();
  Spectra::SparseSymMatProd<double> op(L);
  Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE,
                         Spectra::SparseSymMatProd<double>>
      eigs(&op, k, std::min(2 * k, N));
  eigs.init();
  int nconv = eigs.compute();

  Eigen::VectorXd eigen_values;
  Eigen::MatrixXd eigen_vectors;
  if (eigs.info() == Spectra::SUCCESSFUL) {
    eigen_values = eigs.eigenvalues();
    eigen_vectors = eigs.eigenvectors();
  }

  timer.Pause();
  LOG(INFO) << "3. EigenValue Computation Time: " << timer.ElapsedSeconds();

  // 4. Reverse original eigen vectors(as it stored in descending order)
  timer.Start();
  uint i = 0, j = k - 1;
  while (i < j) {
    const Eigen::VectorXd tmp = eigen_vectors.col(i);
    eigen_vectors.col(i) = eigen_vectors.col(j);
    eigen_vectors.col(j) = tmp;
    i++;
    j--;
  }

  std::vector<Eigen::VectorXd> source_data;
  source_data.reserve(eigen_vectors.rows());
  for (uint i = 0; i < eigen_vectors.rows(); i++) {
    source_data.push_back(eigen_vectors.row(i));
  }
  timer.Pause();
  LOG(INFO) << "4. Eigen Vectors Reverse Time: " << timer.ElapsedSeconds();

  // 5. Invert K-Means for clustering.
  timer.Start();
  std::vector<uint32_t> cluster_assignment;
  std::vector<Eigen::VectorXd> centers;
  KMeans(source_data, cluster_assignment, centers, k);
  timer.Pause();
  LOG(INFO) << "5. KMeans Time: " << timer.ElapsedSeconds();

  for (uint i = 0; i < cluster_assignment.size(); i++) {
    labels_[nodes_[i]] = cluster_assignment[i];
  }
  return labels_;
}

Eigen::SparseMatrix<double> SpectralCluster::ComputeLaplacian(
    const Eigen::SparseMatrix<double>& S,
    const std::unordered_map<int, int>& degrees) const {
  // Compute degree matrix.
  const int N = degrees.size();
  Eigen::SparseMatrix<double> D(N, N);
  // Eigen::SparseMatrix<double> D_inv(N, N);
  // Eigen::SparseMatrix<double> D_sqrt(N, N);
  for (auto it = degrees.begin(); it != degrees.end(); ++it) {
    int id = it->first;
    D.insert(id, id) = it->second;
    // D.insert(node_mapper_.at(id), node_mapper_.at(id)) = it->second;
    // D_inv.insert(node_mapper_.at(id), node_mapper_.at(id)) = 1.0 /
    // it->second; D_sqrt.insert(node_mapper_.at(id), node_mapper_.at(id)) =
    // -1.0 / sqrt(it->second);
  }
  D.makeCompressed();
  // D_inv.makeCompressed();
  // D_sqrt.makeCompressed();

  // Compute Laplacian matrix.
  Eigen::SparseMatrix<double> L = D - S;
  // Eigen::MatrixXd L_random = D_inv * L;
  // Eigen::MatrixXd L_sym = D_sqrt * L * D_sqrt;

  return L;
}

}  // namespace DAGSfM


#ifndef SSMPACK_MODEL_SWITCHING_ADDITIVE_LINEAR_GAUSSIAN_HPP
#define SSMPACK_MODEL_SWITCHING_ADDITIVE_LINEAR_GAUSSIAN_HPP

#include <armadillo>

namespace ssmkit {
namespace map {

struct SwitchingAdditiveLinearGaussian {
  using TParameter = std::tuple<arma::vec, arma::mat>;
  using TConditionVAR = arma::vec;

  SwitchingAdditiveLinearGaussian(arma::mat trans, arma::mat cov, arma::mat b)
      : biases{b}, transfer{trans}, covariance{cov} {}
  // should not be overloaded, should not be template
  TParameter operator()(const TConditionVAR &x, const int &k) const {
    return std::make_tuple(transfer * x + biases.col(k), covariance);
  }

  arma::mat biases;
  arma::mat transfer;
  arma::mat covariance;
};

} // namespace map
} // namespace ssmkit

#endif // SSMPACK_MODEL_SWITCHING_ADDITIVE_LINEAR_GAUSSIAN_HPP


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
//
#include <popsolver/Model.hpp>
#define BOOST_TEST_MODULE Mod
#include <boost/test/unit_test.hpp>

using namespace popsolver;

BOOST_AUTO_TEST_CASE(Mod) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.addConstant(1);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(Mod0) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.addConstant(0);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  // Can't divide by zero
  BOOST_CHECK(!s.validSolution());
}

BOOST_AUTO_TEST_CASE(ModAEqualB) {
  Model m;
  auto a = m.addConstant(4);
  auto b = m.addConstant(4);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(ModBEqual1) {
  Model m;
  auto a = m.addConstant(4);
  auto b = m.addConstant(1);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(ModAGreaterThanB) {
  Model m;
  auto a = m.addConstant(8);
  auto b = m.addConstant(4);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(ModAGreaterThanBNonZero) {
  Model m;
  auto a = m.addConstant(7);
  auto b = m.addConstant(4);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{3});
}

BOOST_AUTO_TEST_CASE(ModALessThanB) {
  Model m;
  auto a = m.addConstant(4);
  auto b = m.addConstant(8);
  auto c = m.mod(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK_EQUAL(s[c], DataType{4});
}


#include "Calibration.h"
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <QDir>
#include <QTime>

using namespace std;
using namespace cv;

extern struct VISION_PARAM vision_param;

//https://github.com/CompileSense/StereoCamera
const int imageWidth = 752; //摄像头的分辨率
const int imageHeight = 480;
const int boardWidth = 9;                         //横向的角点数目
const int boardHeight = 6;                        //纵向的角点数据
const int boardCorner = boardWidth * boardHeight; //总的角点数据
const int squareSize = 25;                        //标定板黑白格子的大小 单位mm
const Size imageSize = Size(imageWidth, imageHeight);

const Size boardSize = Size(boardWidth, boardHeight);
Mat intrinsicL;                   //相机内参数
Mat distortion_coeffL;            //相机畸变参数
vector<Mat> rvecsL;               //旋转向量
vector<Mat> tvecsL;               //平移向量
vector<vector<Point2f>> cornersL; //各个图像找到的角点的集合 和objRealPoint 一一对应

Mat intrinsicR;                   //相机内参数
Mat distortion_coeffR;            //相机畸变参数
vector<Mat> rvecsR;               //旋转向量
vector<Mat> tvecsR;               //平移向量
vector<vector<Point2f>> cornersR; //各个图像找到的角点的集合 和objRealPoint 一一对应
vector<Mat> intrinsics, distortion_coeffs;

vector<vector<Point3f>> objRealPoint; //各副图像的角点的实际物理坐标集合

vector<Point2f> cornerL; //某一副图像找到的角点
vector<Point2f> cornerR; //某一副图像找到的角点

Mat R, T, E, F;                 //R 旋转矢量 T平移矢量 E本征矩阵 F基础矩阵
Mat Rl, Rr, Pl, Pr, Q;          //校正旋转矩阵R，投影矩阵P 重投影矩阵Q (下面有具体的含义解释）
Mat mapLx, mapLy, mapRx, mapRy; //映射表
Rect validROIL, validROIR;      //图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域

//相机内参矩阵
Mat cameraMatrixL;
Mat distCoeffL;
Mat cameraMatrixR;
Mat distCoeffR;

Matrix34d PS[CAM_NUM_MAX];
Matrix33d RS[CAM_NUM_MAX];
Vector3d TS[CAM_NUM_MAX];

vector<vector<Point2f>> corners; //各个图像找到的角点的集合 和objRealPoint 一一对应
vector<float> vrms;

Calibration::Calibration()
{
}

/*计算标定板上模块的实际物理坐标*/
static void calRealPoint(vector<vector<Point3f>> &obj, int boardwidth, int boardheight, int imgNumber, int squaresize)
{
    //  Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));
    vector<Point3f> imgpoint;
    for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
    {
        for (int colIndex = 0; colIndex < boardwidth; colIndex++)
        {
            imgpoint.push_back(Point3f(colIndex * squaresize, rowIndex * squaresize, 0));
        }
    }
    for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
    {
        obj.push_back(imgpoint);
    }
}

QStringList Calibration::checkFile(QString path)
{
    char buff[100];
    QString dirPath;
    QStringList nameFilters;
    nameFilters << "*.png";
    QStringList files;
    for (int i = 0; i < setting->camNumber; i++)
    {
        dirPath = path + "/camera" + QString::number(i);
        if (EasyTool::isDirExist(dirPath) == false)
        {
            msg("calibr check not found " + dirPath + ",exit");
            return files;
        }
        else
        {
            if (i == 0)
            {
                QDir dir(dirPath);
                files = dir.entryList(nameFilters, QDir::Files | QDir::Readable, QDir::Name);
                if (files.size() < 1)
                {
                    msg("calibr check error,files.size < 1,exit");
                }
            }
            else
            {
                for (int ii = 0; ii < files.size(); ii++)
                {
                    if (EasyTool::isFileExist(dirPath + "/" + files.at(ii)) == false)
                    {
                        msg("calibr check not found " + dirPath + "/" + files.at(ii) + ",exit");
                        return files;
                    }
                }
            }
        }
    }
    return files;
}

void Calibration::calibrStart(QString path)
{
    int goodFrameCount = 0;
    Mat imgL, boardimgL;
    Mat imgR, boardimgR;
    Matrix33d cameraMatrix;
    Mat img;
    Matrix43d PSTMP;
    Matrix43d RTS43d;
    double calibrateCameraError[setting->camNumber];

    vector<QMap<uint8_t, vector<Point2f>>> camcorners;

    //指定亚像素计算迭代标注
    cv::TermCriteria criteria = cv::TermCriteria(
        cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS,
        300, 0.01);

    msg("Calibration Start");

    QStringList files = checkFile(path);
    if (files.size() == 0)
    {
        goto exit;
    }

    for (int i = 0; i < setting->camNumber; i++)
    {
        QMap<uint8_t, vector<Point2f>> corners;
        for (int ii = 0; ii < files.size(); ii++)
        {
            QString imgPath = path + "/camera" + QString::number(i) + "/" + files.at(ii);
            if (EasyTool::isFileExist(imgPath) == false)
            {
                msg("calibr not not found " + imgPath + ",exit");
                return;
            }
            img = imread(string((const char *)imgPath.toLocal8Bit()), IMREAD_GRAYSCALE);
            if (img.data)
            {
                vector<Point2f> corner;
                bool isFind = findChessboardCornersSB(img, boardSize, corner,
                                                      CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY);

                if (isFind)
                {

                    corners.insert(ii, corner);
                    msg("findChessboardCorners " + imgPath + " success " + QString::number(corner.size()));

                    // if(i == 0)
                    // {
                    //     drawChessboardCorners(img, boardSize, corner, isFind);
                    //     namedWindow(string((const char *)imgPath.toLocal8Bit()));
                    //     imshow(string((const char *)imgPath.toLocal8Bit()), img);
                    // }
                }
                else
                {
                    msg("findChessboardCorners " + imgPath + " fail");
                }
            }
        }
        camcorners.push_back(corners);
    }

    msg("camcorners size " + QString::number(camcorners.size()));

    if (camcorners.size() != setting->camNumber)
    {
        return;
    }

    for (int i = 0; i < camcorners.size(); i++)
    {
        msg("corners " + QString::number(i) + " : " + QString::number(camcorners.at(i).size()));
    }

    intrinsics.clear();
    distortion_coeffs.clear();

    for (int i = 0; i < camcorners.size(); i++)
    {
        goodFrameCount = camcorners.at(i).size();
        /*计算实际的校正点的三维坐标*/
        objRealPoint.clear();
        calRealPoint(objRealPoint, boardWidth, boardHeight, goodFrameCount, squareSize);
        corners.clear();
        for (int ii = 0; ii < files.size(); ii++)
        {
            if (camcorners.at(i).contains(ii))
            {
                corners.push_back(camcorners.at(i).find(ii).value());
            }
        }
        Mat intrinsic, distortion_coeff;
        vector<Mat> rvecs; //旋转向量
        vector<Mat> tvecs; //平移向量
        Matrix33d cami;
        Matrix<double, 1, 5> camk;

        msg("calibrate camera" + QString::number(i) + " start..");

        calibrateCameraError[i] = calibrateCamera(objRealPoint, corners, imageSize, intrinsic, distortion_coeff, rvecs, tvecs);

        cv2eigen(intrinsic, cami);
        cv2eigen(distortion_coeff, camk);
        //msg("corners size" + QString::number(i) + ": \r\n" + QString::number(corners.size()));
        msg("intrinsic" + QString::number(i) + ": \r\n" + EasyTool::MatToString(cami));
        msg("distortion_coeff" + QString::number(i) + ": \r\n" + EasyTool::MatToString(camk));
        msg("cailbr : camera" + QString::number(i) + " error : " + QString::number(calibrateCameraError[i]));

        intrinsics.push_back(intrinsic);
        distortion_coeffs.push_back(distortion_coeff);
    }

    vrms.clear();
    for (int i = 0; i < camcorners.size() - 1; i++)
    {
        cornersL.clear();
        cornersR.clear();
        for (int ii = 0; ii < files.size(); ii++)
        {
            if (camcorners.at(0).contains(ii) && camcorners.at(i + 1).contains(ii))
            {
                cornersL.push_back(camcorners.at(0).find(ii).value());
                cornersR.push_back(camcorners.at(i + 1).find(ii).value());
            }
        }

        msg("cailbr : camera" + QString::number(0) + " to camera" + QString::number(i + 1) + " start..");
        goodFrameCount = cornersL.size();

        /*计算实际的校正点的三维坐标*/
        objRealPoint.clear();
        calRealPoint(objRealPoint, boardWidth, boardHeight, goodFrameCount, squareSize);

        QTime time;
        time.start();

        // calibrateCamera(objRealPoint, cornersL, Size(boardWidth, boardHeight), intrinsicL, distortion_coeffL, rvecsL, tvecsL, 0);
        // calibrateCamera(objRealPoint, cornersR, Size(boardWidth, boardHeight), intrinsicR, distortion_coeffR, rvecsR, tvecsR, 0);

        //标定摄像头
        float rms = stereoCalibrate(objRealPoint, cornersL, cornersR,
                                    intrinsics.at(0), distortion_coeffs.at(0),
                                    intrinsics.at(i + 1), distortion_coeffs.at(i + 1),
                                    imageSize, R, T, E, F,
                                    CALIB_FIX_INTRINSIC,
                                    TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 1000, 1e-20));

        // float rms = stereoCalibrate(objRealPoint, cornersL, cornersR,
        //                             intrinsics.at(0), distortion_coeffs.at(0),
        //                             intrinsics.at(i + 1), distortion_coeffs.at(i + 1),
        //                             Size(imageWidth, imageHeight), R, T, E, F,
        //                             0,
        //                             TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 1000, 1e-20));

        vrms.push_back(rms);

        //立体校正的时候需要两幅图像共面并且行对准 以使得立体匹配更加的可靠
        //使得两幅图像共面的方法就是把两个摄像头的图像投影到一个公共成像面上，这样每幅图像从本图像平面投影到公共图像平面都需要一个旋转矩阵R
        //stereoRectify 这个函数计算的就是从图像平面投影都公共成像平面的旋转矩阵Rl,Rr。 Rl,Rr即为左右相机平面行对准的校正旋转矩阵。
        //左相机经过Rl旋转，右相机经过Rr旋转之后，两幅图像就已经共面并且行对准了。
        //其中Pl,Pr为两个相机的投影矩阵，其作用是将3D点的坐标转换到图像的2D点的坐标:P*[X Y Z 1]' =[x y w]
        //Q矩阵为重投影矩阵，即矩阵Q可以把2维平面(图像平面)上的点投影到3维空间的点:Q*[x y d 1] = [X Y Z W]。其中d为左右两幅图像的时差

        // stereoRectify(
        // intrinsics.at(0), distortion_coeffs.at(0),
        // intrinsics.at(i + 1), distortion_coeffs.at(i + 1), imageSize, R, T, Rl, Rr, Pl, Pr, Q,
        //               CALIB_ZERO_DISPARITY, -1, imageSize, &validROIL, &validROIR);

        // cout << "R: " << R << endl;
        // cout << "T: " << T << endl;
        // cout << "Pl: " << Pl << endl;
        // cout << "Pr: " << Pr << endl;
        // cout << "intrinsics: " << intrinsics.at(i) << endl;
        // cout << "distortion_coeffs: " << distortion_coeffs.at(i) << endl;

        if (i == 0)
        {
            RS[0] << 1, 0, 0,
                0, 1, 0,
                0, 0, 1;
            TS[0] << (double)0.0, (double)0.0, (double)0.0;
            RS[0].transposeInPlace();
            RTS43d = EasyTool::getRT43d(RS[0], TS[0]);
            cv2eigen(intrinsics.at(0), cameraMatrix);
            PS[0] = (RTS43d * cameraMatrix.transpose()).transpose();
        }

        cv2eigen(R, RS[i + 1]);
        cv2eigen(T, TS[i + 1]);
        RS[i + 1].transposeInPlace();

        RTS43d = EasyTool::getRT43d(RS[i + 1], TS[i + 1]);
        cv2eigen(intrinsics.at(i + 1), cameraMatrix);
        PS[i + 1] = (RTS43d * cameraMatrix.transpose()).transpose();

        if (i == 0)
        {
            vision_param.R[i] << RS[i];
            vision_param.T[i] << TS[i];
            vision_param.P[i] << PS[i];
            vision_param.ERR[i] = calibrateCameraError[i];
            cv2eigen(intrinsics.at(i), vision_param.I[i]);
            cv2eigen(distortion_coeffs.at(i), vision_param.K[i]);

            msg("I" + QString::number(i) + ": \r\n" + EasyTool::MatToString(vision_param.I[i]));
            msg("K" + QString::number(i) + ": \r\n" + EasyTool::MatToString(vision_param.K[i]));
            msg("P" + QString::number(i) + ": \r\n" + EasyTool::MatToString(vision_param.P[i]));
            msg("R" + QString::number(i) + ": \r\n" + EasyTool::MatToString(vision_param.R[i]));
            msg("T" + QString::number(i) + ": \r\n" + EasyTool::MatToString(vision_param.T[i]));

            // cout << std::setprecision(16) << "P" << i << ": " << vision_param.P[i] << endl;
            // cout << std::setprecision(16) << "R" << i << ": " << vision_param.R[i] << endl;
            // cout << std::setprecision(16) << "T" << i << ": " << vision_param.T[i] << endl;
        }

        vision_param.R[i + 1] << RS[i + 1];
        vision_param.T[i + 1] << TS[i + 1];
        vision_param.P[i + 1] << PS[i + 1];
        vision_param.ERR[i + 1] = calibrateCameraError[i + 1];
        cv2eigen(intrinsics.at(i + 1), vision_param.I[i + 1]);
        cv2eigen(distortion_coeffs.at(i + 1), vision_param.K[i + 1]);

        msg("I" + QString::number(i + 1) + ": \r\n" + EasyTool::MatToString(vision_param.I[i + 1]));
        msg("K" + QString::number(i + 1) + ": \r\n" + EasyTool::MatToString(vision_param.K[i + 1]));
        msg("P" + QString::number(i + 1) + ": \r\n" + EasyTool::MatToString(vision_param.P[i + 1]));
        msg("R" + QString::number(i + 1) + ": \r\n" + EasyTool::MatToString(vision_param.R[i + 1]));
        msg("T" + QString::number(i + 1) + ": \r\n" + EasyTool::MatToString(vision_param.T[i + 1]));

        // cout << std::setprecision(16) << "P" << i + 1 << ": " << vision_param.P[i + 1] << endl;
        // cout << std::setprecision(16) << "R" << i + 1 << ": " << vision_param.R[i + 1] << endl;
        // cout << std::setprecision(16) << "T" << i + 1 << ": " << vision_param.T[i + 1] << endl;

        vision_param.RMS[i] = vrms.at(i);

        msg("take time " + QString::number(time.elapsed() / 1000.0) + "s");
        msg("Stereo Calibration done with RMS error = " + QString::number(rms, 'f', 6));
    }

    for (int i = 0; i < camcorners.size() - 1; i++)
    {
        msg(QString::number(0) + " to " + QString::number(i + 1) + " rms error = " + QString::number(vrms.at(i), 'f', 6));
    }

exit:
    msg("Calibration Exit");
}

void Calibration::msg(QString msg)
{
    mlog->show(msg);
    emit logSignal(msg);
}


/**
 * \file boost/numeric/ublasx/operation/cond.hpp
 *
 * \brief Matrix condition number with respect to inversion.
 *
 * The condition number of a function, with respect to an argument, measures
 * the asymptotically worst case of how much the function can change in
 * proportion to small changes in the argument.
 * The "function" is the solution of a problem and the "arguments" are the data
 * in the problem.
 * A problem with a low condition number is said to be
 * well-conditioned, while a problem with a high condition number is
 * said to be ill-conditioned.
 * As a general rule of thumb, if the condition number \f$\kappa(A) = 10k\f$,
 * then you lose \f$k\f$ digits of accuracy on top of what would be lost to the
 * numerical method due to loss of precision from arithmetic methods.
 *
 * The condition number of a matrix measures the sensitivity of the solution of
 * a system of linear equations to errors in the data.
 * It gives an indication of the accuracy of the results from matrix inversion
 * and the linear equation solution.
 * Condition numbers near 1 indicate a well-conditioned matrix.
 * Mathematically, the condition number of a matrix \f$A\f$, with respect to a
 * given matrix p-norm, is defined as:
 * \f{equation}
 *  \kappa(A) = \begin{cases}
 *  			 \|A\|_p \|A^{-1}\|_p & A \text{ is not singular},\\
 *  			 +\infty, & A \text{ is singular}
 *  			\end{cases}
 * \f}
 *
 *

 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_COND_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_COND_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/any.hpp>
#include <boost/numeric/ublasx/operation/inv.hpp>
#include <boost/numeric/ublasx/operation/max.hpp>
#include <boost/numeric/ublasx/operation/min.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/svd.hpp>
#include <functional>
#include <limits>
#include <stdexcept>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail { namespace /*<unnamed>*/ {

enum norm_categories
{
	norm_inf_category = -1,
	norm_frobenius_category = 0,
	norm_1_category = 1,
	norm_2_category = 2
};

template <int Norm, typename MatrixExprT>
typename type_traits<typename matrix_traits<MatrixExprT>::value_type>::real_type cond_impl(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;

	// pre: A is square OR (A is rectangular AND norm is 2)
	if (num_rows(A) != num_columns(A) && Norm != norm_2_category)
	{
		throw ::std::invalid_argument("[boost::numeric::ublasx::detail::cond_impl] For rectangular matrices use the 2 norm.");
	}

	real_type c;

	switch (Norm)
	{
		case norm_frobenius_category: // Frobenius norm
			try
			{
				c = norm_frobenius(A)*norm_frobenius(inv(A));
			}
			catch (...)
			{
				c = ::std::numeric_limits<real_type>::infinity();
			}
			break;
		case norm_inf_category: // Infinity norm
			try
			{
				c = norm_inf(A)*norm_inf(inv(A));
			}
			catch (...)
			{
				c = ::std::numeric_limits<real_type>::infinity();
			}
			break;
		case norm_1_category: // 1-norm
			try
			{
				c = norm_1(A)*norm_1(inv(A));
			}
			catch (...)
			{
				c = ::std::numeric_limits<real_type>::infinity();
			}
			break;
		case norm_2_category: // 2-norm
			{
				vector<real_type> s = svd_values(A);
				if (any(s, ::std::bind2nd(::std::equal_to<real_type>(), 0)))
				{
					// Singular matrix
					c = ::std::numeric_limits<real_type>::infinity();
				}
				else
				{
					c = max(s)/min(s);
				}
			}
			break;
	}

	return c;
}

}} // Namespace detail::<unnamed>

/**
 * \brief The 1-norm matrix condition number with respect to inversion.
 *
 * \tparam MatrixExprT The matrix expression type.
 * \param A The input matrix expression.
 * \return The 1-norm condition number if \a A is not singular; otherwise,
 *  \f$+\infty\f$.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename type_traits<typename matrix_traits<MatrixExprT>::value_type>::real_type cond_1(matrix_expression<MatrixExprT> const& A)
{
	return detail::cond_impl<detail::norm_1_category>(A);
}

/**
 * \brief The 2-norm matrix condition number with respect to inversion.
 *
 * \tparam MatrixExprT The matrix expression type.
 * \param A The input matrix expression.
 * \return The 2-norm condition number if \a A is not singular; otherwise,
 *  \f$+\infty\f$.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename type_traits<typename matrix_traits<MatrixExprT>::value_type>::real_type cond_2(matrix_expression<MatrixExprT> const& A)
{
	return detail::cond_impl<detail::norm_2_category>(A);
}

/**
 * \brief The infinity norm matrix condition number with respect to inversion.
 *
 * \tparam MatrixExprT The matrix expression type.
 * \param A The input matrix expression.
 * \return The infinity norm condition number if \a A is not singular;
 *  otherwise, \f$+\infty\f$.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename type_traits<typename matrix_traits<MatrixExprT>::value_type>::real_type cond_inf(matrix_expression<MatrixExprT> const& A)
{
	return detail::cond_impl<detail::norm_inf_category>(A);
}

/**
 * \brief The Frobenius norm matrix condition number with respect to inversion.
 *
 * \tparam MatrixExprT The matrix expression type.
 * \param A The input matrix expression.
 * \return The Frobenius norm condition number if \a A is not singular;
 *  otherwise, \f$+\infty\f$.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename type_traits<typename matrix_traits<MatrixExprT>::value_type>::real_type cond_frobenius(matrix_expression<MatrixExprT> const& A)
{
	return detail::cond_impl<detail::norm_frobenius_category>(A);
}

/**
 * \brief The 2-norm matrix condition number with respect to inversion.
 *
 * \tparam MatrixExprT The matrix expression type.
 * \param A The input matrix expression.
 * \return The 2-norm condition number if \a A is not singular; otherwise,
 *  \f$+\infty\f$.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename type_traits<typename matrix_traits<MatrixExprT>::value_type>::real_type cond(matrix_expression<MatrixExprT> const& A)
{
	return detail::cond_impl<detail::norm_2_category>(A);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_COND_HPP


/**
 * @file cv_test.cpp
 *
 * Unit tests for the cross-validation module.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <type_traits>

#include <mlpack/core/cv/meta_info_extractor.hpp>
#include <mlpack/core/cv/metrics/accuracy.hpp>
#include <mlpack/core/cv/metrics/mse.hpp>
#include <mlpack/core/optimizers/rmsprop/rmsprop.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/zero_init.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/decision_tree/decision_tree.hpp>
#include <mlpack/methods/hoeffding_trees/hoeffding_tree.hpp>
#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression.hpp>

#include <boost/test/unit_test.hpp>

using namespace mlpack::ann;
using namespace mlpack::cv;
using namespace mlpack::optimization;
using namespace mlpack::regression;
using namespace mlpack::tree;

BOOST_AUTO_TEST_SUITE(CVTest);

/*
 * Test the accuracy metric.
 */
BOOST_AUTO_TEST_CASE(AccuracyTest)
{
  // Making linearly separable data.
  arma::mat data =
    arma::mat("1 0; 2 0; 3 0; 4 0; 5 0; 1 1; 2 1; 3 1; 4 1; 5 1").t();
  arma::Row<size_t> trainingLabels("0 0 0 0 0 1 1 1 1 1");

  LogisticRegression<> lr(data, trainingLabels);

  arma::Row<size_t> labels("0 0 1 0 0 1 0 1 0 1"); // 70%-correct labels

  BOOST_REQUIRE_CLOSE(Accuracy::Evaluate(lr, data, labels), 0.7, 1e-5);
}

/*
 * Test the mean squared error.
 */
BOOST_AUTO_TEST_CASE(MSETest)
{
  // Making two points that define the linear function f(x) = x - 1
  arma::mat trainingData("0 1");
  arma::rowvec trainingResponses("-1 0");

  LinearRegression lr(trainingData, trainingResponses);

  // Making three responses that differ from the correct ones by 0, 1, and 2
  // respectively
  arma::mat data("2 3 4");
  arma::rowvec responses("1 3 5");

  double expectedMSE = (0 * 0 + 1 * 1 + 2 * 2) / 3.0;

  BOOST_REQUIRE_CLOSE(MSE::Evaluate(lr, data, responses), expectedMSE, 1e-5);
}

/*
 * Test the mean squared error with matrix responses.
 */
BOOST_AUTO_TEST_CASE(MSEMatResponsesTest)
{
  arma::mat data("1 2");
  arma::mat trainingResponses("1 2; 3 4");

  FFN<MeanSquaredError<>, ZeroInitialization> ffn;
  ffn.Add<Linear<>>(1, 2);
  ffn.Add<IdentityLayer<>>();

  RMSProp opt(0.2);
  opt.Shuffle() = false;
  ffn.Train(data, trainingResponses, opt);

  // Making four responses that differ from the correct ones by 0, 1, 2 and 3
  // respectively
  arma::mat responses("1 3; 5 7");

  double expectedMSE = (0 * 0 + 1 * 1 + 2 * 2 + 3 * 3) / 4.0;

  BOOST_REQUIRE_CLOSE(MSE::Evaluate(ffn, data, responses), expectedMSE, 1e-1);
}

template<typename Class,
         typename ExpectedPT,
         typename PassedMT = arma::mat,
         typename PassedPT = arma::Row<size_t>>
void CheckPredictionsType()
{
  using Extractor = MetaInfoExtractor<Class, PassedMT, PassedPT>;
  using ActualPT = typename Extractor::PredictionsType;
  static_assert(std::is_same<ExpectedPT, ActualPT>::value,
      "Should be the same");
}

BOOST_AUTO_TEST_CASE(PredictionsTypeTest)
{
  CheckPredictionsType<LinearRegression, arma::rowvec>();
  // CheckPredictionsType<FFN<>, arma::mat>();

  CheckPredictionsType<LogisticRegression<>, arma::Row<size_t>>();
  CheckPredictionsType<SoftmaxRegression, arma::Row<size_t>>();
  CheckPredictionsType<HoeffdingTree<>, arma::Row<size_t>, arma::mat>();
  CheckPredictionsType<HoeffdingTree<>, arma::Row<size_t>, arma::imat>();
  CheckPredictionsType<DecisionTree<>, arma::Row<size_t>, arma::mat,
      arma::Row<size_t>>();
  CheckPredictionsType<DecisionTree<>, arma::Row<char>, arma::mat,
      arma::Row<char>>();
}

template<typename Class,
         typename ExpectedWT,
         typename PassedMT = arma::mat,
         typename PassedPT = arma::Row<size_t>,
         typename PassedWT = arma::rowvec>
void CheckWeightsType()
{
  using Extractor = MetaInfoExtractor<Class, PassedMT, PassedPT, PassedWT>;
  using ActualWT = typename Extractor::WeightsType;
  static_assert(std::is_same<ExpectedWT, ActualWT>::value,
      "Should be the same");
}

BOOST_AUTO_TEST_CASE(WeightsTypeTest)
{
  CheckWeightsType<LinearRegression, arma::rowvec>();
  CheckWeightsType<DecisionTree<>, arma::rowvec>();
  CheckWeightsType<DecisionTree<>, arma::Row<float>, arma::mat,
      arma::Row<size_t>, arma::Row<float>>();

  CheckWeightsType<FFN<>, void>();
  CheckWeightsType<LARS, void>();
  CheckWeightsType<LogisticRegression<>, void>();
}

BOOST_AUTO_TEST_CASE(TakesDatasetInfoTest)
{
  static_assert(MetaInfoExtractor<DecisionTree<>>::TakesDatasetInfo,
      "Value should be true");
  static_assert(!MetaInfoExtractor<LinearRegression>::TakesDatasetInfo,
      "Value should be false");
  static_assert(!MetaInfoExtractor<SoftmaxRegression>::TakesDatasetInfo,
      "Value should be false");
}

BOOST_AUTO_TEST_CASE(TakesNumClassesTest)
{
  static_assert(MetaInfoExtractor<DecisionTree<>>::TakesNumClasses,
      "Value should be true");
  static_assert(MetaInfoExtractor<SoftmaxRegression>::TakesNumClasses,
      "Value should be true");
  static_assert(!MetaInfoExtractor<LinearRegression>::TakesNumClasses,
      "Value should be false");
  static_assert(!MetaInfoExtractor<LARS>::TakesNumClasses,
      "Value should be false");
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include <polyfem/Common.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>

#include <polyfem/AutodiffTypes.hpp>

#include <Eigen/Dense>
#include <functional>

namespace polyfem
{
	class LinearElasticity
	{
	public:
		// res is R^{dim²}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 9, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void compute_von_mises_stresses(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;

		inline int &size() { return size_; }
		inline int size() const { return size_; }

		inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

		void set_parameters(const json &params);
	private:
		int size_ = 2;
		double mu_ = 1;
		double lambda_ = 1;

		void assign_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};
}


#include "fitline.h"
#include "ransac.h"
#include <cstdlib>
#include <cstdio>
#include <ctime>
#include <sstream>
#include <fstream>
#include <string>
#include <iostream>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
#define PI (3.1415926535897932346f)

int main()
{
    // Read calibration data
    double id, score, x, y, z, roll, yaw, pitch;
    double  x_0, y_0, z_0, roll_0, yaw_0, pitch_0;

    ifstream calib_FileA("../data/calib_data.txt");
    int filecount = 0;
    while (!calib_FileA.eof())
    {
        calib_FileA >> id >> score >> x >> y >> z >> roll >> pitch >> yaw;
        filecount++;
    }
    calib_FileA.close();

    cout << "Read " << filecount << " data" << endl;

    ifstream calib_File("../data/calib_data.txt");
    int count = 0;

    Point2D32f points_x[filecount];
    Point2D32f points_y[filecount];
    Point2D32f points_z[filecount];

    Point2D32f points_roll[filecount];
    Point2D32f points_pitch[filecount];
    Point2D32f points_yaw[filecount];

    while (!calib_File.eof())
    {
        calib_File >> id >> score >> x >> y >> z >> roll >> pitch >> yaw;

        points_x[count].x = id;
        points_x[count].y = x;

        points_y[count].x = id;
        points_y[count].y = y;

        points_z[count].x = id;
        points_z[count].y = z;

        points_roll[count].x = id;
        points_roll[count].y = roll;

        points_pitch[count].x = id;
        points_pitch[count].y = pitch;

        points_yaw[count].x = id;
        points_yaw[count].y = yaw;

        x_0+=x;
        y_0+=y;
        z_0+=z;
        roll_0+=roll;
        pitch_0+=pitch;
        yaw_0+=yaw;

        count++;
    }

    x_0=x_0/filecount;
    y_0=y_0/filecount;
    z_0=z_0/filecount;
    roll_0=roll_0/filecount;
    pitch_0=pitch_0/filecount;
    yaw_0=yaw_0/filecount;

     /*   float lines[4] = {0.0}; //line parameters
    int numForEstimate = 5;
    float successProbability = 0.9999f;
    float maxOutliersPercentage = 0.9; //(float)outlierCnt/COUNT; 0.9
    float a, b;

    Ransac(points_x, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
    a = lines[1] / lines[0];
    b = lines[3] - a * lines[2];
    printf("x ransac fit(including outliers): a: %f  x: %f\n", a, b);
    x = b;

    Ransac(points_y, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
    a = lines[1] / lines[0];
    b = lines[3] - a * lines[2];
    printf("y ransac fit(including outliers): a: %f  y: %f\n", a, b);
    y = b;

    Ransac(points_z, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
    a = lines[1] / lines[0];
    b = lines[3] - a * lines[2];
    printf("z ransac fit(including outliers): a: %f  z: %f\n", a, b);
    z = b;

    Ransac(points_roll, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
    a = lines[1] / lines[0];
    b = lines[3] - a * lines[2];
    printf("roll ransac fit(including outliers): a: %f  roll: %f\n", a, b);
    roll = b;

    Ransac(points_pitch, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
    a = lines[1] / lines[0];
    b = lines[3] - a * lines[2];
    printf("pitch ransac fit(including outliers): a: %f  pitch: %f\n", a, b);
    pitch = b;

    Ransac(points_yaw, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
    a = lines[1] / lines[0];
    b = lines[3] - a * lines[2];
    printf("yaw ransac fit(including outliers): a: %f  yaw: %f\n", a, b);
    yaw = b;
    */

    calib_File.close();
    cout << endl;
    cout << "RANSAC Result (x,y,z,roll,pitch,yaw): " << x << ", " << y << ", " << z << ", " << roll << ", " << pitch << ", " << yaw << endl;
    Eigen::Matrix<double, 3, 1> T;

    //EulerAngles to RotationMatrix
    ::Eigen::Vector3d ea0(yaw, pitch, roll);
    ::Eigen::Matrix3d R;
    R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ()) * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY()) * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());

    // cout << R << endl << endl;
    T << x, y, z;
    Eigen::Matrix4d tf = Eigen::Matrix4d::Identity();
    tf.block(0, 0, 3, 3) = R;
    tf.block(0, 3, 3, 1) = T;

    cout << "----------------------------------" << endl;
    cout << "Result Matrix:" << endl;
    cout << tf << endl;
    return 0;
}


#include <vector>
#include "aabb.h"
#include <iostream>
#include "model.h"
#include <Eigen/Core>
using namespace std;
using namespace Eigen;

int main(int argc, char** argv){
  const string filename = argv[1];
  Scene scene(filename.c_str());

  for(auto& model_ptr : scene.models){
    size_t num_tris = model_ptr->tris_.size();
    vector<tri_aabb> aabbs(num_tris);

    #pragma omp parallel for
    for(size_t i = 0; i < num_tris; ++i){
      aabbs[i] = tri_aabb(model_ptr->tris_[i]->id_, model_ptr->tris_[i]->p_.col(0),model_ptr->tris_[i]->p_.col(1), model_ptr->tris_[i]->p_.col(2));
    }
    #pragma omp parallel for
    for(size_t i = 0; i < num_tris; ++i){
      for(size_t j = 0; j < 3; ++j){
        for(size_t k = 0; k < 3; ++k){
          assert(model_ptr->tris_[i]->p_(k, j) <= aabbs[i].up_bd_(k) &&
                 model_ptr->tris_[i]->p_(k, j) >= aabbs[i].low_bd_(k));
        }
      }
    }
  }
  cout <<"pass" << endl;

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_EXPONENTIAL_CONSTANTS_EXP_1_HPP_INCLUDED
#define NT2_TOOLBOX_EXPONENTIAL_CONSTANTS_EXP_1_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup expon_constant
 * \defgroup expon_constant_exp_1 Exp_1
 *
 * \par Description
 * Constant exp_1 : \f$e = \exp(1)\f$ constant.
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/exp_1.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::exp_1_(A0)>::type
 *     exp_1();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Exp_1
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Exp_1, double
                                , 2, 0x402df854
                                , 0x4005bf0a8b145769LL
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Exp_1, Exp_1);
}

#endif


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU 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 ANU OR THE 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.                                                               *
 ******************************************************************************/

/**
 * \file Sturm.hpp
 * \brief Class for evaluating Sturm chains on polynomials, and bracketing the
 *        real roots.
 */

#ifndef OPENGV_STURM_HPP_
#define OPENGV_STURM_HPP_

#include <memory>
#include <stdlib.h>
#include <vector>
#include <list>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>

/**
 * \brief The namespace of this library.
 */
namespace opengv
{
/**
 * \brief The namespace of the math tools.
 */
namespace math
{

class Bracket
{
public:
  typedef std::shared_ptr<Bracket> Ptr;
  typedef std::shared_ptr<const Bracket> ConstPtr;

  Bracket( double lowerBound, double upperBound );
  Bracket( double lowerBound, double upperBound, size_t changes, bool setUpperBoundChanges );
  virtual ~Bracket();

  bool dividable( double eps ) const;
  void divide( std::list<Ptr> & brackets ) const;
  double lowerBound() const;
  double upperBound() const;
  bool lowerBoundChangesComputed() const;
  bool upperBoundChangesComputed() const;
  void setLowerBoundChanges( size_t changes );
  void setUpperBoundChanges( size_t changes );
  size_t numberRoots() const;

private:
  double _lowerBound;
  double _upperBound;
  bool _lowerBoundChangesComputed;
  bool _upperBoundChangesComputed;
  size_t _lowerBoundChanges;
  size_t _upperBoundChanges;
};

/**
 * Sturm is initialized over polynomials of arbitrary order, and used to compute
 * the real roots of the polynomial.
 */
class Sturm
{

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /** A pair of values bracketing a real root */
  typedef std::pair<double,double> bracket_t;

  /**
   * \brief Contructor.
   * \param[in] p The polynomial coefficients (poly = p(0,0)*x^n + p(0,1)*x^(n-1) ...).
   */
  Sturm( const Eigen::MatrixXd & p );
  /**
   * \brief Contructor.
   * \param[in] p The polynomial coefficients (poly = p[0]*x^n + p[1]*x^(n-1) ...).
   */
  Sturm( const std::vector<double> & p );
  /**
   * \brief Destructor.
   */
  virtual ~Sturm();

  void findRoots2( std::vector<double> & roots, double eps_x = 0.001, double eps_val = 0.001 );
  /**
   * \brief Finds the roots of the polynomial.
   * \return An array with the real roots of the polynomial.
   */
  std::vector<double> findRoots();
  /**
   * \brief Finds brackets for the real roots of the polynomial.
   * \return A list of brackets for the real roots of the polynomial.
   */
  void bracketRoots( std::vector<double> & roots, double eps = -1.0 );
  /**
   * \brief Evaluates the Sturm chain at a single bound.
   * \param[in] bound The bound.
   * \return The number of sign changes on the bound.
   */
  size_t evaluateChain( double bound );
  /**
   * \brief Evaluates the Sturm chain at a single bound.
   * \param[in] bound The bound.
   * \return The number of sign changes on the bound.
   */
  size_t evaluateChain2( double bound );
  /**
   * \brief Composes an initial bracket for all the roots of the polynomial.
   * \return The maximum of the absolute values of the bracket-values (That's
   *         what the Lagrangian bound is able to find).
   */
  double computeLagrangianBound();

private:
  /**
   * \brief Internal function used for composing the Sturm chain
   * \param[in] p1 First polynomial.
   * \param[in] p2 Second polynomial.
   * \param[out] r The negated remainder of the polynomial division p1/p2.
   */
  void computeNegatedRemainder(
      const Eigen::MatrixXd & p1,
      const Eigen::MatrixXd & p2,
      Eigen::MatrixXd & r );

  /** A matrix containing the coefficients of the Sturm-chain of the polynomial */
  Eigen::MatrixXd _C;
  /** The dimension _C, which corresponds to (polynomial order+1) */
  size_t _dimension;
};

}
}

#endif /* OPENGV_STURM_HPP_ */


// Local private PANACEA includes
#include "vector_eigen.hpp"

// Third party includes
#include <Eigen/Dense>

// Standard includes
#include <cassert>
#include <iostream>
#include <memory>

namespace panacea {

VectorEigen::VectorEigen() { vector_ = std::make_unique<Eigen::VectorXd>(); }

const VectorType VectorEigen::type() const { return VectorType::Eigen; }

void VectorEigen::setZero() { vector_->setZero(); }

void VectorEigen::resize(const int size) {
  assert(vector_->rows() >= 0);
  vector_->resize(size);
}

VectorEigen &VectorEigen::operator=(const Vector &vec) {
  this->resize(vec.rows());
  for (int row = 0; row < vec.rows(); ++row) {
    this->operator()(row) = vec(row);
  }
  return *this;
}

double &VectorEigen::operator()(const int index) {
  assert(index >= 0);
  assert(index < vector_->rows());
  return (*vector_)(index);
}

double VectorEigen::operator()(const int index) const {
  assert(index >= 0);
  assert(index < vector_->rows());
  return (*vector_)(index);
}

int VectorEigen::rows() const {
  if (direction_ == Direction::AlongRows) {
    return vector_->rows();
  } else {
    return 1;
  }
}

Direction VectorEigen::direction() const { return direction_; }

int VectorEigen::cols() const {
  if (direction_ == Direction::AlongRows) {
    return 1;
  } else {
    return vector_->rows();
  }
}

void VectorEigen::print() const { std::cout << (*vector_) << std::endl; }
} // namespace panacea


/**
 * @file 	ex5.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jun 5, 2017
 */

#include <iostream>
#include <fstream>
#include <boost/timer/timer.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/algorithm/string.hpp>

using namespace std;
using namespace boost;

/**
 * main function which reads a graph from a .gph file, then calculates shortest
 * paths from all vertices to the first vertex with the dijsktra algorithm
 * and prints the furthest vertex together with its distance
 * to the standard output
 * @param numargs number of inputs on command line
 * @param args array of inputs on command line
 * @return whether the function operated successfully
 */
int main(int numargs, char *args[]) {

	timer::cpu_timer t;

	if (numargs != 2) {
		cout << "Usage: " << args[0] << " filename" << endl;
		exit(EXIT_FAILURE);
	}

	ifstream inputFile;
	inputFile.open(args[1]);							//trying to read file
	if (inputFile.fail()) {
		cerr << "file could not be read" << endl;
		exit(EXIT_FAILURE);
	}

	typedef adjacency_list<vecS, vecS, undirectedS,
			no_property, property<edge_weight_t, double>> Graph;
	typedef pair<int,int> Edge;

	int numVertices;
	int numEdges;

	string line;
	getline(inputFile, line);					//first line is read
	vector<string> parts;
	split(parts, line, is_any_of(" "));

	if (parts.size() != 2) {
		cerr << "error in file: first line should consist of two integers!" << endl;
		exit(EXIT_FAILURE);
	}

	try {
		numVertices = stoi(parts[0]);		//information from the first line
		numEdges = stoi(parts[1]);			//are stored
	} catch (...) {
		cerr << "error in file: first line should consist of two integers!" << endl;
		exit(EXIT_FAILURE);
	}

	Edge *edges =  new Edge[numEdges];			//in these arrays all information about
	double *weights = new double[numEdges];		//the edges are stored

	int i = 0;

	//read line by line
	while (getline(inputFile, line)) {
		split(parts, line, is_any_of(" "));
		if (parts.size() != 3) {
			cerr << "error in line " << (i+2) << ": line should consists of "
					"two integers and a double!" << endl;
			exit(EXIT_FAILURE);
		}
		try {
			edges[i] = Edge(stoi(parts[0])-1, stoi(parts[1])-1);
			weights[i] = stod(parts[2]);
		} catch(...) {
			cerr << "error in line " << (i+2) << ": line should consists of "
					"two integers and a double!" << endl;
			exit(EXIT_FAILURE);
		}
		++i;
	}

	//undirected graph is constructed with all edges and their weights
	Graph g(edges, edges + numEdges , weights, numVertices);

	vector<double> distances(numVertices);

	//call of dijsktra
	dijkstra_shortest_paths(g, *(vertices(g).first), distance_map(&distances[0]));

	property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);

	double maxDist = 0;
	int maxIdx = 0;
	graph_traits<Graph>::vertex_iterator it;

	//search for furthest vertex
	for (it = vertices(g).first+1; it != vertices(g).second; ++it) {
		double tmp = distances[*it];
		if (tmp > maxDist) {
			maxDist = tmp;
			maxIdx = index(*it);
		}
	}

	cout << "RESULT VERTEX " << (maxIdx+1) << endl;
	cout << "RESULT DIST " << maxDist << endl;
	cout << endl << t.format() <<  endl;

	exit(EXIT_SUCCESS);
}


/*
********************************************************************
版本声明：
								   ╭════════════════════════╮
								  ║         〖课题设计：〗         ║
		 ╭══════════════════════┤设计时间：2020.4.17 ├══════════════════╮
		║                        ║   设计人：  2016级工业工程专业 邝伟杜Tridu³³     ║                            ║
		║                        ╰═════════════════════════╯                            ║
	  　║                                        ★    关于    Code     ★                                         ║
		║   @计算机配置：windows 10 教育版 64位操作系统 内存：4G CPU ：i3--6100		                            ║
		║   @运行环境：Microsoft Visual C++ 2010				                                                    ║
		║   @设计思路：建立多个子函数，分别用作缓冲区无限的串行生产线建模.				                            ║
  　  　║--------------------------------------------------------------------------------------------------------- ║
	  　║                                        ★      @主要函数       ★                                        ║
  　    ║                              |----------------------------------------------|                            ║
		║				1 极大代数法的乘法MP_RealNumberTimes、2 极大代数系统的矩阵乘法MP_MaTimesMb、				║
		║			    3 极大代数法的加法MP_RealNumberPlus、4 矩阵A的极大代数法k次幂MP_AKpower、			    	║
		║				5 极大代数矩阵的星运算MP_Mastar、6 判断矩阵的可简约性并给出不可简约矩阵的特征值MP_MaValue、 ║
		║				7 计算串行生产线开环系统的参数矩阵ABC:T2ABCmatrix、	                            			║
		║				8 给出反馈矩阵K计算闭环线性模型M,N矩阵MP_KT2MN			                                	║
		║			    主函数控制管理了各个函数结构，其实需要外部使用，			                            	║
		║			    只需要把功能函数extern，写成dll提供给其他函数调用就行在主函数t_main(GUI应用本质就是通过各种 ║
		║  组件调用函数传递参数，其实逻辑类似，这里目标是说明功能GUI应用懒得整了)中，可以以1、2、3、4、5、6、7、8、║
		║  9、10数字键分别可以执行某个功能模块。                                                                   ║
		║				限教学使用交流！请勿用于商业目的。                                                          ║
　      ║----------------------------------------------------------------------------------------------------------║
		║                          欢迎提出您的建议或意见，请发邮件:2055969978@qq.com                              ║
		║----------------------------------------------------------------------------------------------------------║
	　　║                                                                                ║
		║                     ╭──────────────────────────────╮                     ║
		╰══════════┤   ★★★★★★  Tridu³³，tridu33@qq.com★★★★★★★     ├══════════╯
							  ╰───────────────────────────────╯

******************************************************************
*/

// MaxPlus_ABC.cpp : 定义控制台应用程序的入口点。
//
#define _CRT_SECURE_NO_WARNINGS ;
// 包括 SDKDDKVer.h 将定义可用的最高版本的 Windows 平台。

// 如果要为以前的 Windows 平台生成应用程序，请包括 WinSDKVer.h，并将
// WIN32_WINNT 宏设置为要支持的平台，然后再包括 SDKDDKVer.h。
//在此处引用程序需要的其他头文件
//编译程序会先从当前目录中找文件，生成预编译头文件！预编译，是为了提高编译速度！
#include <SDKDDKVer.h>
#include <stdio.h>
#include <tchar.h>//一般是用双引号来引用自己编写的文件，而用尖括号引用系统标准的文件。
#include<stdlib.h>//用于数值转换、内存分配以及具有其他相似任务的函数。free()
#include<conio.h>//Console Input/Output（控制台输入输出）的简写，其中定义了通过控制台进行数据输入和数据输出的函数，主要是一些用户通过按键盘产生的对应操作，比如getch()函数等等。
#include<string.h>//字符串处理
#include<dos.h>//包含了很多BIOS和DOS调用函数
#include<windows.h>//windows
#include <ctype.h>//
#include <iostream>
//Eigen3
#include <regex>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include<fstream>// Save to local file.
#include <sstream> // stringstream, getline

#include <limits.h>//整型无穷大小，INT_MAX表示正无穷，-DBL_MAX表示负无穷
#include <float.h>//double 型的无穷大小，用DBL_MAX表示正无穷，-DBL_MAX表示负无穷(注意不是DBL_MIN)

using namespace Eigen;
using namespace std;

//声明函数
/*
1 极大代数法的乘法MP_RealNumberTimes();
2 极大代数系统的矩阵乘法MP_MaTimesMb();
3 极大代数法的加法MP_RealNumberPlus();
4 矩阵A的极大代数法k次幂MP_AKpower();
5 极大代数矩阵的星运算MP_Mastar();
6 判断矩阵的可简约性并给出不可简约矩阵的特征值MP_MaValue();
7 计算串行生产线开环系统的参数矩阵ABC:T2ABCmatrix();
8 给出反馈矩阵K计算闭环线性模型M,N矩阵MP_KT2MN();

*/

//-------------------------------------------------------------------------
//Octave编程语言省略了繁琐的变量声明和函数调用，在把Octave代码编程实现为C++的过程中，需要增加代码中的Octave库的结构体和函数，Octave代码不能生成exe或者dll,老师建议我用c++造一遍轮子，我觉得也许c++大型数据结构精度计算优化可能会好一些吧。
//根据code，需要定义的结构体和函数(轮子)：
//col collist(k)列向量结构体=一维数组=特殊的二维矩阵
//row rowlist(k)行向量结构体=一维数组=特殊的二维矩阵
//matrix矩阵结构体=二维数据
//泛型函数max
//int result = max(collist)取行向量大值
//max(rowlist)取列向量大值
//max(matrix)取二维数组matrix最大值
//collist diag(matrix H)
//返回值[int m,int n]=函数名size(arg*参数matrix matrix_mn)取大小
//[matrix matrix_ones]=ones(int m,int n);
//......太麻烦了，所以：
//人生苦短，我选Eigen!
MatrixXd readMatrixFromTXT(string dir);
int colT = 0;//T每列数初始0
int rowT = 0;//T每行数量初始值0,变量作用域
MatrixXd readMatrixFromTXT(string dir) {
	string line;
	ifstream in(dir);  //读入文件
	regex pat_regex("[[:digit:]]+");  //匹配原则，这里代表一个或多个数字
	//获取矩阵rowT,colT
	while (getline(in, line)) {  //按行读取
		rowT++;
		colT = 0;//每行列数设置初始0,变量作用域
		for (sregex_iterator it(line.begin(), line.end(), pat_regex), end_it; it != end_it; ++it) {  //表达式匹配，匹配一行中所有满足条件的字符
			colT++;//colT++
		}
	};
	MatrixXd T = MatrixXd::Ones(rowT, colT);//注意行列数从0开始
	//赋值
	int i = 0;//每列 index初始0
	int j = 0;//每行 index初始值0
	ifstream in2(dir);
	while (getline(in2, line)) {  //按行读取
		i++;
		j = 0;
		for (sregex_iterator it(line.begin(), line.end(), pat_regex), end_it; it != end_it; ++it) {  //表达式匹配，匹配一行中所有满足条件的字符
			//cout << it->str() << " ";  //输出匹配成功的数据
			j++;
			T(i - 1, j - 1) = stoi(it->str());  //将数据转化为int型并存入
		}
	};
	in.close(); in2.close();
	return T;
}

/*非常不好用的Octave混合编程
#include <oct.h>
int mystandlone (void)
{
  std::cout << "Hello Octave world!\n";

  int n = 2;
  Matrix a_matrix = Matrix (n, n);

  for (octave_idx_type i = 0; i < n; i++)
    for (octave_idx_type j = 0; j < n; j++)
      a_matrix(i,j) = (i + 1) * 10 + (j + 1);

  std::cout << a_matrix;

  return 0;
}
*/

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

void menu();

//菜单MatrixXd

int MP_RealNumberTimes(int a,int b);//1
MatrixXd  MP_MaTimesMb(MatrixXd Ma, MatrixXd Mb);//2
int MP_RealNumberPlus(int a, int b);//3
MatrixXd  MP_AKpower(MatrixXd A, int k);	//4
MatrixXd MP_Mastar(MatrixXd A);//5
double MP_MaValue(MatrixXd A);//6
void T2ABCmatrix(MatrixXd T);//7
MatrixXd T2Amatrix(MatrixXd T);
MatrixXd T2Bmatrix(MatrixXd T);
MatrixXd T2Cmatrix(MatrixXd T);
void MP_KT2MN( MatrixXd K, MatrixXd T);//8

void exit();//9
void showtxt();//10

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

void menu() {

	system("cls");    //清屏
	system("mode con cols=150 lines=45");
	system("color f0");//改变控制台前景，背景颜色
	system("title 基于极大代数法的串行生产线计算工具包");
	//system("\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t date /T");//显示系统当前日期
	//system("\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t time /T");//显示系统当前时间，\t是没有反应的
	printf("\n\n\n\n\n");
	printf("\t\t\t\t ╭════════════════════════════════════════════════╮\n");
	printf("\t\t\t\t            〖课题设计：MaxPlus_ABC〗          \n");
	printf("\t ╭══════════════════════════════════┤  设计时间：2020.4.20 ├════════════════════════════════════════════════╮\n");
	printf("\t\t\t\t               设计人：Tridu33                                                                              \n");
	printf("\t\t\t\t╰ ════════════════════════════════════════════════╯                             \n");
	printf("\t║---------------------------------------------------------------------------------------------------------- ║\n");
	printf("\t║                                        ★    Menu菜单     ★                                              ║\n");
	printf("\t║---------------------------------------------------------------------------------------------------------- ║\n");
	printf("\t║                                    1.极大代数法的乘法MP_RealNumberTimes();                                ║\n");
	printf("\t║                                    2.极大代数系统的矩阵乘法MP_MaTimesMb();                                ║\n");
	printf("\t║                                    3.极大代数法的加法MP_RealNumberPlus();                                 ║\n");
	printf("\t║                                    4.矩阵A的极大代数法k次幂MP_AKpower();                                  ║\n");
	printf("\t║                                    5.极大代数矩阵的星运算MP_Mastar();                                     ║\n");
	printf("\t║                                    6.判断矩阵的可简约性并给出不可简约矩阵的特征值MP_MaValue();            ║\n");
	printf("\t║                                    7.计算串行生产线开环系统的参数矩阵ABC:T2ABCmatrix();                   ║\n");
	printf("\t║                                    8.给出反馈矩阵K计算闭环线性模型M,N矩阵MP_KT2MN();                      ║\n");
	printf("\t║                                    9.exit退出                                                             ║\n");
	printf("\t║                                    10.关于                                                                ║\n");
	printf("\t║\t     ╭───────────────────────────────────────────────────────────────────────────────╮              ║\n");
	printf("\t║═══════════┤ ★★★★★★  欢迎提出您的建议或意见，请发邮件:tridu33@qq.com    ★★★★★★★├══════════════║\n");
	printf("\t║\t     ╰───────────────────────────────────────────────────────────────────────────────╯\n\n\n\n");

	printf("\t\t\t请您选择(1-10):\t");
}

//-------------------------------------------------------------------------MatrixXd
//1 极大代数法的乘法MP_RealNumberTimes();

int MP_RealNumberTimes(int a, int b) {
	printf("\n极大代数法的乘法MP_RealNumberTimes\n");
	return a + b;
};
/*

function c=MP_RealNumberTimes(a,b)
  c=a+b;
endfunction

*/

//-------------------------------------------------------------------------
//2 极大代数系统的矩阵乘法MP_MaTimesMb();
MatrixXd MP_MaTimesMb(MatrixXd Ma, MatrixXd Mb) {
	//printf("极大代数系统的矩阵乘法MP_MaTimesMb\n ");
	MatrixXd MaTimesMb = MatrixXd::Ones(Ma.rows(), Mb.cols());
	int m = Ma.rows(); int n = Mb.cols();
	MatrixXd tmplist = MatrixXd::Ones(1,Ma.cols());
	if(Ma.cols()!=Mb.rows())
		cout<<"Horizontal dimension of a should equal to vertical dimension of b."<<endl;
	else
	{
		for (int i = 0; i <  m; i++) {
			for (int j = 0; j <  n; j++) {
				for (int k = 0; k<Ma.cols();k++) {
					if (Ma(i, k) != -DBL_MAX && Mb(k, j) != -DBL_MAX)
						tmplist(0, k) = Ma(i, k) + Mb(k, j);
					//-DBL_MAX会溢出导致结果0，而不会出现保持-DBL_MAX的结果，所以需要自定义，-DBL_MAX遇到什么数字相加都==-DBL_MAX
					else //其中有一个加数是-DBL_MAX,结果不用算数计算,溢出，而应该直接令结果为-DBL_MAX
						tmplist(0, k) = -DBL_MAX;
				}
				double result = -DBL_MAX;
				int length = tmplist.cols();
				for (int i = 0; i < length; i++) {
					if (result < tmplist(0,i)) result = tmplist(0,i);//double 2 int ,lose message
				}
				MaTimesMb(i, j) = result;
				//cout <<"MaTimesMb(i, j):" <<MaTimesMb(i, j) << endl;
			}
		}
	}
	//cout<<MaTimesMb<<endl;
	return MaTimesMb;
};//

/*

%%矩阵A(m*r)极大代数乘矩阵B(r*n)=MaTimesMb(m*n)
function MaTimesMb=MP_MaTimesMb(a,b)
  [m,c1]=size(a);
  [r2,n]=size(b);
  if (c1==r2)
	for i=1:m
	  for j=1:n
		for k=1:c1
		tmplist(k)=a(i,k)+b(k,j);
		endfor
		MaTimesMb(i,j)=max(tmplist);
	  endfor
	endfor
  elseif
	disp('Horizontal dimension of a should equal to vertical dimension of b.');
  endif
endfunction

*/

//-------------------------------------------------------------------------
//3 极大代数法的加法MP_RealNumberPlus();
int MP_RealNumberPlus(int a,int b) {
	//printf("\n极大代数法的加法MP_RealNumberPlus\n");
	return max(a,b);
};//

/*
function c=MP_RealNumberPlus(a,b)
  c=max(a,b);
endfunction

矩阵加法其实就是取大操作，后续懒得写出来，直接写了在需要调用的地方
function MaPlusMb=MP_MaPlusMb(a,b)
  [m,r1]=size(a);
  [r2,n]=size(b);
  if ((r1==r2) ^(m==n) ^(m=r1))
				for i=1:m
	for j=1:m
	if(a(i,j)<b(i,j))
	a(i,j)=b(i,j);
	endif
	endfor
	endfor
  elseif
	disp('dimension of a should equal to dimension of b,and both square matrix.');
  endif
endfunction
*/

//-------------------------------------------------------------------------
//4 矩阵A的极大代数法k次幂MP_AKpower();
	MatrixXd  MP_AKpower(MatrixXd A, int k) {
		//printf("\n矩阵A的极大代数法k次幂MP_AKpower\n ");
		int m = A.rows(); k = k - 1;
		MatrixXd AK= -DBL_MAX*MatrixXd::Ones(m, m);
		for (int i = 0; i < m; i++) {
			AK(i, i) = 0;
		}//AK初始值E矩阵
		//cout << "初始值AK=E\n"<<AK << endl;
		if (A.rows() == A.cols()) {
			for (int j = 0; j <= k ; j++) {
			//cout << "the" << j << "turns:\n" << A << endl;
			AK = MP_MaTimesMb(AK, A);
			//cout << AK << endl;
			}
		}
		else
			printf("Horizontal dimension of A should equal to vertical dimension");
		return AK;
	}//
/*

%矩阵A的极大代数法k次幂
function AK=MP_AKpower(A,k)
  [m,n]=size(A);
if(m==n)
		  E=-inf*ones(m,m);
		  for i=1:m
			E(i,i)=0;
		  endfor
		  AK=E;
		  for j=1:k
		  AK=MP_MaTimesMb(AK,A);
		  endfor
elseif
  disp('Horizontal dimension of A should equal to vertical dimension');
endif
endfunction

*/

//-------------------------------------------------------------------------
//5 极大代数矩阵的星运算MP_Mastar();

MatrixXd MP_Mastar(MatrixXd A) {
	//printf("\n极大代数矩阵的星运算MP_Mastar\n ");
	int m = A.rows(); int n = A.cols();
	MatrixXd AK = MatrixXd::Ones(m, m);//初始化矩阵大小,最后不取all 1
	MatrixXd E = -DBL_MAX * MatrixXd::Ones(m, m);
	for (int i = 0; i < m; i++) {
		E(i, i) = 0;
	}
	MatrixXd Astr = E;
	if (m == n) {
		for (int k = 1; k < m; k++) {//k不是下标，不许-1
		//cout<<"k="<<k<<endl;
		AK = MP_AKpower(A, k);// A的k次幂，k = 0; < dim(A) - 1,Astr是累计的矩阵结果,AK是每次极大代数加法新进去的矩阵
	//	cout <<"AK="<<AK<< endl;//MaPlusMb矩阵的极大代数加法Astronauts，Astr
				for (int i = 0; i < m; i++) {
					for (int j = 0; j < m; j++) {
						if (Astr(i, j) < AK(i, j))
							Astr(i, j) = AK(i, j);
					}
				}
				//cout<<"Astr="<<Astr<<endl;
		}
	}
	else
		printf("A should be square matrix.");
return Astr;
}

/*

%%极大代数的方阵星运算实现
function Astr=MP_Mastar(A)
[m,n]=size(A);
if m==n
	E=-inf*ones(m,m);
	for i=1:m
	  E(i,i)=0;
	endfor
 Astr=E;
 for k=1:m-1
 AK=MP_AKpower(A,k);%%A的k次幂，k=1:dim(A)-1
	for i=1: m
	  for j=1:m
		if(Astr(i,j)<AK(i,j))
		Astr(i,j)=AK(i,j);
		endif
	  endfor
	endfor
 endfor
elseif
  disp('A should be square matrix.');
endif
endfunction

*/
//------------------------------------------------------------------------
//6 判断矩阵的可简约性并给出不可简约矩阵的特征值MP_MaValue();

double MP_MaValue(MatrixXd A) {
	printf("\n判断矩阵的可简约性并给出不可简约矩阵的特征值MP_MaValue\n "); double Alambda = 0;
	MatrixXd H = A;
	MatrixXd Ak = A;
	int m = A.rows(); int n = A.cols();
	if (m == n) {
		for (int k = 2; k < m; k++) {//不是下标不许-1
			Ak = MP_MaTimesMb(Ak, A);
			for (int i = 0; i < m; i++) {
				for (int j = 0; j < m; j++) {
					if ((H(i, j) < Ak(i, j) / k)) H(i, j) = Ak(i, j) / k;
				}
			}
		}
		int x = 0;
		for (int i = 0; i < m; i++) {
			for (int j = 0; j < m; j++) {
				if (H(i, j) == (-DBL_MAX)) x = 1; break;
			}
		}

		if (x == 0) {
			double maxValue=0;
			for (int j = 0; j < m; j++) {
				if (H(j, j)> maxValue) maxValue = H(j,j) ;//double 2 int lose messgae
				//int j = 0; j < m; j++循环变量-1就不需要下标变动，但是比较麻烦的下标推理的情况下：还是用的下标-1，循环条件不变比较好。
			};
			Alambda = maxValue;
		}
		else {
			cout << "Matrix is a Reducible matrix." << endl;;
			Alambda = -DBL_MAX;
		}
	}
	else{
		cout<<"A should be square matrix."<<endl;
	}
	return Alambda;

}

/*

%判断矩阵的可简约性，并给出不可简约矩阵的特征值
function Alambda=MP_MaValue(A)
H=A;
Ak=A;
[m,n]=size(A);
if m==n
	for k=2:m
	  Ak=MP_MaTimesMb(Ak,A);
	  for i=1:m
		for j=1:m
		  if (H(i,j)<Ak(i,j)/k)
			 H(i,j)=Ak(i,j)/k;
		end
	  end
	end
  end
  x=0;
  for i=1:m
	for j=1:m
	  if (H(i,j)==(-inf))
		  x=1;break;
	  end
	end
  end
	if(x==0)
	  Alambda=max(diag(H));
	else
	  disp('Matrix is a Reducible matrix.');
	  Alambda=-inf;
	end
else
   disp('A should be square matrix.');
end
##
##>> M
##M =
##
##     7  -Inf  -Inf     7     7     4
##    10     6  -Inf    10    10     7
##    10     9     8    10    10     7
##     6     6     5     6  -Inf  -Inf
##     9     9     8     9     8  -Inf
##    10     9     8    10    10     7
##
##>> N
##N =
##
##  -Inf  -Inf     5  -Inf  -Inf  -Inf     4  -Inf  -Inf
##  -Inf  -Inf     5  -Inf  -Inf  -Inf     4     3  -Inf
##  -Inf  -Inf     6  -Inf  -Inf  -Inf     5     4     0
##  -Inf  -Inf     5  -Inf  -Inf     3     4  -Inf  -Inf
##  -Inf  -Inf     8  -Inf  -Inf     6     7     4  -Inf
##  -Inf  -Inf    11  -Inf  -Inf     8    10     9     5
##  -Inf  -Inf     8  -Inf  -Inf     6     7  -Inf     0
##  -Inf  -Inf    12  -Inf  -Inf    10    11     6     4
##  -Inf  -Inf    15  -Inf  -Inf    13    14    12     8
##
##>> Nlambda=MP_MaValue(N)
##Matrix is a Reducible matrix.
##Nlambda = -Inf
##>> Mlambda=MP_MaValue(M)
##Mlambda =  9.5000
##
##

*/

//-------------------------------------------------------------------------
//7 计算串行生产线开环系统的参数矩阵ABC:T2ABCmatrix();

void T2ABCmatrix(MatrixXd T) {
	int m = T.rows(); int n = T.cols();
	cout << "A is:\n" << T2Amatrix(T) << endl;
	cout << "\nB is:\n" << T2Bmatrix(T) << endl;
	cout << "\nC is:\n" << T2Cmatrix(T) << endl;

}

MatrixXd T2Amatrix(MatrixXd T) {
	int m = T.rows(); int n = T.cols();

	//构造矩阵A:
	MatrixXd A = -DBL_MAX * MatrixXd::Ones(m*n,m*n);
	for (int k = 0; k<=(m - 1);k++) {
		for (int j = 0; j <= (m - 1);j++) {
			if (j == k) {
				for (int i = 1; i <=( n - 1);i++) {
					A(k * n + i + 1 - 1, j * n + i - 1) = T((k + 1 - 1), i - 1);//最快的方法是只改下标-1，循环条件不变，改前A(k*n+i+1,j*n+i)=T((k+1),i);
				};
			}
			else if(1 + j == k) {
				for (int i = 1; i <= n; i++) {
					A(k * n + i - 1, j * n + i - 1) = T(k - 1, i - 1);//最快的方法是只改下标-1，循环条件不变，改前A(k*n+i,j*n+i)=T(k,i);
				}
			}
			else
				;
		}
	}
	return A;
}

MatrixXd T2Bmatrix(MatrixXd T) {
	int m = T.rows(); int n = T.cols();
	MatrixXd B = -DBL_MAX * MatrixXd::Ones(m * n, m + n); //构造矩阵B:
	for (int i = 1; i <= n; i++) {
		B(i - 1, i - 1) = 0;//-1
	}
	for (int i = 1; i <= m; i++) {
		B(n * (i - 1) + 1 - 1, n + i - 1) = 0;//-1
	}
	return B;
}

MatrixXd T2Cmatrix(MatrixXd T) {
	int m = T.rows(); int n = T.cols();
	MatrixXd C = -DBL_MAX * MatrixXd::Ones(m + n, m * n);//构造矩阵C:
	for (int i = 1; i <= n; i++) {
		C(i - 1, (m - 1) * n + i - 1) = T(m - 1, i - 1);//-1
	}
	for (int i = 1; i <= m; i++) {
		C(n + i - 1, i * n - 1) = T(i - 1, n - 1);
	}
	return C;
}

/*

%% 串行生产线开环系统的参数矩阵实现
function [A,B,C]=T2ABCmatrix(T)
  [m,n]=size(T);%%构造矩阵A:
  A=-inf*ones(m*n);
  for k=0:m-1
	for j=0:m-1
	  if(j==k)%%k*n+i=j*n+i对角元素下方元素下标k*n+i+1,j*n+i
			for i=1:n-1%%对角分块t_x1-t_x(n-1)
			A(k*n+i+1,j*n+i)=T((k+1),i);
			endfor;
	  elseif(1+j==k)
			for i=1:n %%对焦分块的左下分块t_x1-t_xn
			A(k*n+i,j*n+i)=T(k,i);
			endfor
	  %endif%如果第11行写成else if理解为两个if else语句
	  endif%if
	endfor
  endfor

  B=-inf*ones(m*n,m+n); %%构造矩阵B:
	for i=1:n
	  B(i,i)=0;
	end
	for i=1:m
	  B(n*(i-1)+1,n+i)=0;
	end

  C=-inf*ones(m+n,m*n);%%构造矩阵C:
	for i=1:n
	  C(i,(m-1)*n+i)=T(m,i);
	end
	for i=1:m
	  C(n+i,i*n)=T(i,n);
	end

endfunction

##
##>> T=[25 60 40;35 45 45;45 70 70;40 65 60]
##T =
##
##   25   60   40
##   35   45   45
##   45   70   70
##   40   65   60
##
##>> [A,B,C]=openmatrix(T)
##A =
##
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##    25  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf    60  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##    25  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf    60  -Inf    35  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf    40  -Inf    45  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf    35  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf    45  -Inf    45  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf    45  -Inf    70  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    45  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    70  -Inf    40  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    70  -Inf    65  -Inf
##
##B =
##
##     0  -Inf  -Inf     0  -Inf  -Inf  -Inf
##  -Inf     0  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf     0  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf     0  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf     0  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf     0
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##
##C =
##
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    40  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    65  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    60
##  -Inf  -Inf    40  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf    45  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    70  -Inf  -Inf  -Inf
##  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf  -Inf    60

*/
//-------------------------------------------------------------------------
//8 给出反馈矩阵K计算闭环线性模型M,N矩阵MP_KT2MN();

void MP_KT2MN(MatrixXd K, MatrixXd T){
	printf("\n给出反馈矩阵K计算闭环线性模型M,N矩阵MP_KT2MN\n");
	int m = T.rows(); int n = T.cols();
	MatrixXd A = T2Amatrix(T);
	MatrixXd B = T2Bmatrix(T) ;
	MatrixXd C = T2Cmatrix(T) ;
	MatrixXd M = MP_MaTimesMb(C, MP_MaTimesMb(MP_Mastar(A), MP_MaTimesMb(B, K))); cout << "M:\n"<<M << endl;
	MatrixXd N = MP_MaTimesMb(MP_Mastar(A), MP_MaTimesMb(B, MP_MaTimesMb(K, C))); cout << "N:\n" << N << endl;

};//

/*

%%极大代数，给出反馈矩阵K计算M,N矩阵
function [M,N]=MP_KT2MN(K,T)
  [A,B,C]=T2ABCmatrix(T);
  M=MP_MaTimesMb(C,MP_MaTimesMb(MP_Mastar(A),MP_MaTimesMb(B,K)));
  N=MP_MaTimesMb(MP_Mastar(A),MP_MaTimesMb(B,MP_MaTimesMb(K,C)));

##>> T=[0 1 5;3 2 3;4 3 0]
##T =
##
##   0   1   5
##   3   2   3
##   4   3   0
##
##>> K=-inf*ones(6,6);
##>> for i=1:6
##K(i,i)=0;
##endfor
##>> [M,N]=MP_KT2MN(K,T)
##M =
##
##     7  -Inf  -Inf     7     7     4
##    10     6  -Inf    10    10     7
##    10     9     8    10    10     7
##     6     6     5     6  -Inf  -Inf
##     9     9     8     9     8  -Inf
##    10     9     8    10    10     7
##
##N =
##
##  -Inf  -Inf     5  -Inf  -Inf  -Inf     4  -Inf  -Inf
##  -Inf  -Inf     5  -Inf  -Inf  -Inf     4     3  -Inf
##  -Inf  -Inf     6  -Inf  -Inf  -Inf     5     4     0
##  -Inf  -Inf     5  -Inf  -Inf     3     4  -Inf  -Inf
##  -Inf  -Inf     8  -Inf  -Inf     6     7     4  -Inf
##  -Inf  -Inf    11  -Inf  -Inf     8    10     9     5
##  -Inf  -Inf     8  -Inf  -Inf     6     7  -Inf     0
##  -Inf  -Inf    12  -Inf  -Inf    10    11     6     4
##  -Inf  -Inf    15  -Inf  -Inf    13    14    12     8

*/
//
void exit() {
	printf("\n9.exit \n");
	system("exit");
}

//-------------------------------------------------------------------------
void showtxt() {
	system("cls");
	//system("title 输入0返回主菜单");
	char c;
	//文件输入
	freopen("readme.txt", "r", stdin);
	while (scanf("%c", &c) != EOF) {
		printf("%c", c);
	}
	freopen("CON", "r", stdin); //https://blog.csdn.net/pqleo/article/details/23031337 卡了三天的问题原来在这儿！！！！让流回到控制台?使用freopen后如何将stdout输出流还原回屏幕？
	//break;//步进没问题，但是不知道为什么不能只执行一次，死循环
	printf("\n\n");
	system("pause");
	//exit(1);
}

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

/*本人精通面向佛系的编程语言，有空交流

					   _oo0oo_
					  o8888888o
					  88" . "88
					  (| -_- |)
					  0\  =  /0
					___/`---'\___
				  .' \\|     |// '.
				 / \\|||  :  |||// \
				/ _||||| -:- |||||- \
			   |   | \\\  -  /// |   |
			   | \_|  ''\---/''  |_/ |
			   \  .-\__  '-'  ___/-. /
			 ___'. .'  /--.--\  `. .'___
		  ."" '<  `.___\_<|>_/___.' >' "".
		 | | :  `- \`.;`\ _ /`;.`/ - ` : | |
		 \  \ `_.   \_ __\ /__ _/   .-` /  /
	 =====`-.____`.___ \_____/___.-`___.-'=====
					   `=---='

	 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

			   佛祖保佑         永无BUG
*/
///////////////////////////////////////////////////////////////////////////////////////////////
void welcomevoice();
void welcomevoice() {
	char uerInputData[2][100] = { "欢迎使用" };
	int welcome = 0;
	while (1) {
		FILE* pFile = fopen("voice.vbs", "w");//msgbox"自"//CreateObject("SAPI.SpVoice").Speak"能"//CreatObject("SAPI.SpVoice").Speak+"User_Input";
		fprintf(pFile, "CreateObject(\"SAPI.SpVoice\").Speak\"%s\"", uerInputData[welcome++]);//fprintf(pFile,"msgbox(\"%s\")",uerInputData[i++]);//main(1)函数外只能定义全局变量或者对象，而不能执行语句及调用函数。//cmd system
		fclose(pFile);
		system("voice.vbs");//run vbs
		system("del voice.vbs");//del vbs
		if (welcome < 2)
			continue;
		else
			break;
	}
};

int _tmain(int argc, _TCHAR* argv[]) {
	MatrixXd Tread = readMatrixFromTXT("T.txt");
	cout << "T is :\n" << Tread << endl;
	cout << "T矩阵有" << rowT << "行" << colT << "列" << endl;

	MatrixXd Kread = -DBL_MAX*MatrixXd::Ones(6,6);
	for (int i = 0; i < Kread.rows(); i++) {
		Kread(i, i) = 0;
	}
	cout << "K is :\n" << Kread << endl;
	cout << "K矩阵有" << Kread.rows() << "行" << Kread.cols() << "列" << endl;
//-------------------------------读取矩阵T,获得变量rowT,colT,矩阵K,获得变量rowK,colK---------------------------------------//
	int c;
	char ch[7];
	int i, n;
	system("color 0f");//改变控制台前景，背景颜色
	system("title MaxPlus_ABC");
	welcomevoice();//欢迎使用
	printf("\t\t\t password(111):");
	//---------

	for (i = 0; i < 3; i++) {
		//获取ch
		for (n = 0; n < 3; n++) {
			ch[n] = _getch();
			printf("*");
		}
		printf("\n");
		ch[n] = '\0';
		//匹配ch和password
		//if (0 == 0) {
		if (0 == strcmp(ch, "111")) {
			menu();
			//mystandlone();//octave.h call Octave Function
			scanf("%d", &c);
			while (c) {
				switch (c) {
				case 1: cout << MP_RealNumberTimes(1, 2)<< endl; printf("\nPress any key for continue...");  break;
				case 2: {
					int Marow = 2; int Macol = 3;
					MatrixXd Ma = MatrixXd::Random(Marow, Macol); Ma(0, 0) = -DBL_MAX; Ma(0, 1) = -DBL_MAX; Ma(0, 2) = -DBL_MAX;
					int Mbrow = 3; int Mbcol = 2;
					MatrixXd Mb = MatrixXd::Ones(Mbrow,Mbcol); Mb(0, 0) = -DBL_MAX; Mb(1, 0) = -DBL_MAX; Mb(2, 0) = -DBL_MAX;
					cout << Ma << endl;
					cout << Mb << endl;
					cout << MP_MaTimesMb(Ma, Mb) << endl;
					printf("\nPress any key for continue...");
				} break;
				case 3: cout<<MP_RealNumberPlus(1,2)<<endl; printf("\nPress any key for continue..."); break;
				case 4: {
					int tmprow = 3; int tmpcol = 3;
					MatrixXd tmpE = -DBL_MAX * MatrixXd::Ones(tmprow, tmpcol);
					for (int i = 0; i < tmprow;i++) { tmpE(i, i) = 0; }
					MatrixXd testA = -DBL_MAX*MatrixXd::Ones(tmprow, tmpcol);
					testA << 1, 2, 2,
						-DBL_MAX, 2, 3,
						1, -DBL_MAX, 8;
					cout << "矩阵：\n"<< testA <<endl;
					cout << "矩阵自身矩阵相乘k=0次：\n" << tmpE << endl;
					cout << "矩阵自身矩阵相乘k=1次：\n" << MP_AKpower(testA, 1) << endl;
					cout << "矩阵自身矩阵相乘k=2次：\n" << MP_AKpower(testA, 2) << endl;
					cout << "矩阵自身矩阵相乘k=3次：\n" << MP_AKpower(testA, 3) << endl;
					printf("\nPress any key for continue...");
					break;
				}
				case 5: {
					cout << Tread << endl;
					MatrixXd tmp = T2Amatrix(Tread);
					cout << "T的A矩阵：\n" << tmp << endl;
					cout << "矩阵星运算：\n" << MP_Mastar(tmp) << endl;
					printf("\nPress any key for continue...");
					break;
				}
				case 6: {
					int tmprow = 3; int tmpcol = 3;
					MatrixXd tmp = MatrixXd::Ones(tmprow, tmpcol);
					tmp << 1, 2, 2,
						-DBL_MAX, 2, 3,
						1, -DBL_MAX, 8;
					cout << "矩阵：\n" << tmp << endl;
					cout << "A矩阵特征值(8默认值)：" << MP_MaValue(tmp) << endl; printf("\nPress any key for continue..."); break;
				}
				case 7: T2ABCmatrix(Tread); printf("\nPress any key for continue..."); break;
				case 8: MP_KT2MN(Kread, Tread); printf("\nPress any key for continue..."); break;
				case 10: showtxt(); break;
				default:  printf("该编号的新功能有待开发中..."); break;
				case 9: return 0;
				};
				_getch();//会等待你按下任意键，再继续执行下面的语句
				menu();
				scanf("%d", &c);//可以换成sscanf_("%d",&n);'scanf': This function or variable may be unsafe.
			};
		}

		else {
			printf("密码错误,请重新输入\n");
		}

	}
	if (i == 3) {
		printf("你输入的错误的密码次数达到上限，系统自动退出！请联系管理员！");
		system("pause");
		exit(1);
	}
	return 0;
}

///////////   //                                       //    //
   //        //                                       //   //
  //        ////////     ////////      /////////     //  //        //////
 //        //     //    //     //     //      //    /////        //
//        //     //    //     //     //      //    //  //          ////
//        //     //    //     //     //      //    //     //            //
//        //     //     ///////////  //      //    //        //   //////
/////////////////////////////////////////////////////////////////////////////////////////////