/*
 * @file pppBayesTree.cpp
 * @brief Iterative GPS Range/Phase Estimator with collected data
 * @author Ryan Watson & Jason Gross
 */

// GTSAM related includes.
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/gnssNavigation/GnssData.h>
#include <gtsam/gnssNavigation/GnssTools.h>
#include <gtsam/gnssNavigation/PhaseFactor.h>
#include <gtsam/gnssNavigation/nonBiasStates.h>
#include <gtsam/configReader/ConfDataReader.hpp>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/gnssNavigation/PseudorangeFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

// GPSTK
#include <gtsam/gpstk/MJD.hpp>
#include <gtsam/gpstk/PowerSum.hpp>
#include <gtsam/gpstk/Decimate.hpp>
#include <gtsam/gpstk/SolidTides.hpp>
#include <gtsam/gpstk/PoleTides.hpp>
#include <gtsam/gpstk/TropModel.hpp>
#include <gtsam/gpstk/BasicModel.hpp>
#include <gtsam/gpstk/CommonTime.hpp>
#include <gtsam/gpstk/PCSmoother.hpp>
#include <gtsam/gpstk/OceanLoading.hpp>
#include <gtsam/gpstk/CodeSmoother.hpp>
#include <gtsam/gpstk/SimpleFilter.hpp>
#include <gtsam/gpstk/MWCSDetector.hpp>
#include <gtsam/gpstk/SatArcMarker.hpp>
#include <gtsam/gpstk/DCBDataReader.hpp>
#include <gtsam/gpstk/ComputeWindUp.hpp>
#include <gtsam/gpstk/Rinex3NavData.hpp>
#include <gtsam/gpstk/GNSSconstants.hpp>
#include <gtsam/gpstk/ComputeLinear.hpp>
#include <gtsam/gpstk/GPSWeekSecond.hpp>
#include <gtsam/gpstk/LICSDetector2.hpp>
#include <gtsam/gpstk/DataStructures.hpp>
#include <gtsam/gpstk/RinexObsStream.hpp>
#include <gtsam/gpstk/Rinex3ObsStream.hpp>
#include <gtsam/gpstk/Rinex3NavStream.hpp>
#include <gtsam/gpstk/ComputeTropModel.hpp>
#include <gtsam/gpstk/SP3EphemerisStore.hpp>
#include <gtsam/gpstk/ComputeSatPCenter.hpp>
#include <gtsam/gpstk/EclipsedSatFilter.hpp>
#include <gtsam/gpstk/GPSEphemerisStore.hpp>
#include <gtsam/gpstk/CorrectCodeBiases.hpp>
#include <gtsam/gpstk/ComputeSatPCenter.hpp>
#include <gtsam/gpstk/RequireObservables.hpp>
#include <gtsam/gpstk/CorrectObservables.hpp>
#include <gtsam/gpstk/LinearCombinations.hpp>
#include <gtsam/gpstk/GravitationalDelay.hpp>
#include <gtsam/gpstk/PhaseCodeAlignment.hpp>

// BOOST
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

// STD
#include <chrono>
#include <iomanip>
#include <fstream>
#include <iostream>
#include <unistd.h>
#include <algorithm>

using namespace std;
using namespace gtsam;
using namespace gpstk;
using namespace boost;
using namespace std::chrono;
namespace NM = gtsam::noiseModel;
namespace po = boost::program_options;
typedef noiseModel::Diagonal diagNoise;

// Intel Threading Building Block
#ifdef GTSAM_USE_TBB
  #include <tbb/tbb.h>
  #undef max // TBB seems to include windows.h and we don't want these macros
  #undef min
#endif

using symbol_shorthand::X; // nonBiasStates ( dx, dy, dz, trop, cb )
using symbol_shorthand::G;   // bias states ( Phase Biases )

int main(int argc, char* argv[])
{
        // define std out print color
        vector<int> prn_vec;
        vector<rnxData> data;
        const string red("\033[0;31m");
        const string green("\033[0;32m");
        string confFile, gnssFile, station, sp3File, p1p2File, p1c1File, antennaModel;
        string rnx_file, nav_file, sp3_file, out_file, antexFile;
        double xn, yn, zn, xp, yp, range, phase, rho, minElev, weightFactor;
        double antennaOffSetH, antennaOffSetE, antennaOffSetN;
        int startKey(0), currKey, startEpoch(0), svn, doy;
        int nThreads(-1), phase_break, break_count(0), nextKey, dec_int, itsBelowThree=0, count=0;
        bool printECEF, printENU, printAmb, printUpdateRate, first_ob(true), usingP1(false);

        cout.precision(12);

        po::options_description desc("Available options");
        desc.add_options()
                ("help,h", "Print help message")
                ("confFile,c", po::value<string>(&confFile)->default_value(""),
                "Input config file" )
                ("out", po::value<string>(&out_file)->default_value(""),
                "output file.")
                ("usingP1", "Are you using P1 instead of C1?");
        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
        po::notify(vm);

        ConfDataReader confReader;
        confReader.open(confFile);

        if (confFile.empty() ) {
                cout << red << "\n\n Currently, you need to provide a conf file \n"
                     << "\n\n"  << green << desc << endl;
        }

        while ( (station = confReader.getEachSection()) != "" )
        {
                // Fetch nominal station location [m]
                xn = confReader.fetchListValueAsDouble("nominalECEF",station);
                yn = confReader.fetchListValueAsDouble("nominalECEF",station);
                zn = confReader.fetchListValueAsDouble("nominalECEF",station);
                // day of year ( used for Niell Trop model)
                doy = confReader.getValueAsInt("DOY", station);
                // Elevation cut-off
                minElev = confReader.getValueAsDouble("minElev", station);
                // Code/carrier ratio
                weightFactor = confReader.getValueAsDouble("weightFactor", station);
                // Data file names
                gnssFile = confReader("rnxFile", station);
                sp3File = confReader("sp3File", station);
                p1p2File = confReader("p1p2", station);
                p1c1File = confReader("p1c1", station);
                // Print statements
                printENU = confReader.getValueAsBoolean("printENU", station);
                printAmb = confReader.getValueAsBoolean("printAmb", station);
                printECEF = confReader.getValueAsBoolean("printECEF", station);
                printUpdateRate = confReader.getValueAsBoolean("printUpdateRate", station);
        }

        usingP1 = (vm.count("usingP1")>0);

        Point3 nomXYZ(xn, yn, zn);
        Point3 prop_xyz = nomXYZ;

        #ifdef GTSAM_USE_TBB
        std::auto_ptr<tbb::task_scheduler_init> init;
        if(nThreads > 0) {
                init.reset(new tbb::task_scheduler_init(nThreads));
        }
        else
                cout << green << " \n\n Using threads for all processors" << endl;
        #else
        if(nThreads > 0) {
                cout << red <<" \n\n GTSAM is not compiled with TBB, so threading is"
                     << " disabled and the --threads option cannot be used."
                     << endl;
                exit(1);
        }
        #endif

        ISAM2DoglegParams doglegParams;
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 100;
        ISAM2 isam(parameters);

        double output_time = 0.0;
        double rw = 2.5;
        double rangeWeight = pow(rw,2);
        double phaseWeight = pow(rw*1/weightFactor,2);

        string value;

        nonBiasStates prior_nonBias = (gtsam::Vector(5) << 0.0, 0.0, 0.0, 0.0, 0.0).finished();

        phaseBias bias_state(Z_1x1);
        gnssStateVector phase_arc(Z_34x1);
        gnssStateVector bias_counter(Z_34x1);
        for (int i=1; i<34; i++) {bias_counter(i) = bias_counter(i-1) + 10000; }

        nonBiasStates initEst(Z_5x1);
        nonBiasStates between_nonBias_State(Z_5x1);

        Values initial_values;
        Values result;

        noiseModel::Diagonal::shared_ptr nonBias_InitNoise = noiseModel::Diagonal::Variances((gtsam::Vector(5) << 10.0, 10.0, 10.0, 3e8, 1e-1).finished());

        noiseModel::Diagonal::shared_ptr nonBias_ProcessNoise = noiseModel::Diagonal::Variances((gtsam::Vector(5) << 0.1, 0.1, 0.1, 3e6, 3e-5).finished());

        noiseModel::Diagonal::shared_ptr initNoise = noiseModel::Diagonal::Variances((gtsam::Vector(1) << 100).finished());

        string obs_path = findExampleDataFile(gnssFile);
        string p1p2_path = findExampleDataFile(p1p2File);
        string p1c1_path = findExampleDataFile(p1c1File);

        if ( obs_path.empty() )
        {
                cout << " Must pass in obs file !!! " << desc << endl;
                exit(1);
        }
        if ( sp3File.empty() )
        {
                cout << " Must pass in ephemeris file !!! " << desc << endl;
                exit(1);
        }
        // Declare a "SP3EphemerisStore" object to handle precise ephemeris
        SP3EphemerisStore SP3EphList;

        size_t pos = 0;
        string path, token;
        string delimiter = " ";
        while ((pos = sp3File.find(delimiter)) != string::npos) {
                path = findExampleDataFile(sp3File.substr(0,pos));
                SP3EphList.loadFile(path);
                sp3File.erase(0, pos + delimiter.length());
        }
        path = findExampleDataFile(sp3File);
        SP3EphList.loadFile(path);

        // Set flags to reject satellites with bad or absent positional
        // values or clocks
        SP3EphList.rejectBadPositions(true);
        SP3EphList.rejectBadClocks(true);

        // Create the input observation file stream
        Rinex3ObsStream rin(obs_path);

        // station nominal position
        Position nominalPos(xn, yn, zn);

        CorrectCodeBiases corrCode;
        corrCode.setDCBFile(p1p2_path, p1c1_path);

        if (!usingP1) {
                corrCode.setUsingC1(true);
        }

        // This is the GNSS data structure that will hold all the
        // GNSS-related information
        gnssRinex gRin;

        RequireObservables requireObs;
        requireObs.addRequiredType(TypeID::L1);
        requireObs.addRequiredType(TypeID::L2);

        SimpleFilter pObsFilter;
        pObsFilter.setFilteredType(TypeID::C1);

        if ( usingP1 )
        {
                requireObs.addRequiredType(TypeID::P1);
                pObsFilter.addFilteredType(TypeID::P1);
                requireObs.addRequiredType(TypeID::P2);
                pObsFilter.addFilteredType(TypeID::P2);
        }
        else
        {
                requireObs.addRequiredType(TypeID::C1);
                pObsFilter.addFilteredType(TypeID::C1);
                requireObs.addRequiredType(TypeID::P2);
                pObsFilter.addFilteredType(TypeID::P2);
        }

        // Declare a couple of basic modelers
        BasicModel basic(nominalPos, SP3EphList);
        basic.setMinElev(minElev);

        // Object to correct for SP3 Sat Phase-center offset
        ComputeSatPCenter svPcenter(SP3EphList, nominalPos);

        // Objects to mark cycle slips
        MWCSDetector markCSMW;  // Checks Merbourne-Wubbena cycle slip

        // object def several linear combinations
        LinearCombinations comb;

        // Object to compute linear combinations for cycle slip detection
        ComputeLinear linear1;
        if ( usingP1 )
        {
                linear1.addLinear(comb.pdeltaCombination);
                linear1.addLinear(comb.mwubbenaCombination);
        }
        else
        {
                linear1.addLinear(comb.pdeltaCombWithC1);
                linear1.addLinear(comb.mwubbenaCombWithC1);
        }
        linear1.addLinear(comb.ldeltaCombination);
        linear1.addLinear(comb.liCombination);

        ComputeLinear linear2;

        // Read if we should use C1 instead of P1
        if ( usingP1 ) { linear2.addLinear(comb.pcCombination); }
        else {  linear2.addLinear(comb.pcCombWithC1); }
        linear2.addLinear(comb.lcCombination);

        LICSDetector2 markCSLI2;       // Checks LI cycle slips

        // Object to keep track of satellite arcs
        SatArcMarker markArc;
        markArc.setDeleteUnstableSats(true);

        // Objects to compute gravitational delay effects
        GravitationalDelay grDelay(nominalPos);

        // Object to remove eclipsed satellites
        EclipsedSatFilter eclipsedSV;

        //Object to compute wind-up effect
        ComputeWindUp windup( SP3EphList, nominalPos );

        // Object to compute prefit-residuals
        ComputeLinear linear3(comb.pcPrefit);
        linear3.addLinear(comb.lcPrefit);

        TypeIDSet tset;
        tset.insert(TypeID::prefitC);
        tset.insert(TypeID::prefitL);

        // Declare a NeillTropModel object, setting the defaults
        NeillTropModel neillTM( nominalPos.getAltitude(),
                                nominalPos.getGeodeticLatitude(),
                                doy);

        // Objects to compute the tropospheric data
        ComputeTropModel computeTropo(neillTM);

        // initialize factor graph
        NonlinearFactorGraph *graph = new NonlinearFactorGraph();

        auto start = std::chrono::steady_clock::now();
        auto end = std::chrono::steady_clock::now();
        // Loop over all data epochs
        while(rin >> gRin)
        {
                TimeSystem sys;
                sys.fromString("GPS");
                CommonTime time(gRin.header.epoch);
                time.setTimeSystem(sys);
                GPSWeekSecond gpstime( time );

                // update nominal ECEF with propogated pos.
                NeillTropModel neillTM( nominalPos.getAltitude(),
                                        nominalPos.getGeodeticLatitude(),
                                        doy);
                try
                {
                        gRin >> requireObs // Check if required observations are present
                        >> pObsFilter // Filter out spurious data
                        >> linear1 // Compute linear combinations to detect CS
                        >> markCSLI2 // Mark cycle slips
                        >> markArc // Keep track of satellite arcs
                        >> basic // Compute the basic components of model
                        >> eclipsedSV // Remove satellites in eclipse
                        >> grDelay // Compute gravitational delay
                        >> svPcenter // Computer delta for sat. phase center
                        >> corrCode // Correct for differential code biases
                        >> windup // phase windup correction
                        >> computeTropo // neill trop function
                        >> linear2  // Compute ionosphere-free combinations
                        >> linear3;   // Compute prefit residuals
                }
                catch(Exception& e) {continue; }
                catch(...)
                {
                        cerr << "Unknown exception at epoch: " << time << endl;
                        continue;
                }

                // Iterate through the GNSS Data Structure
                satTypeValueMap::const_iterator it;
                typeValueMap::const_iterator itObs;
                if ( itsBelowThree > 0 )
                {
                        itsBelowThree = 0;
                        continue;
                }

                if (gRin.body.size() == 0) { continue; }

                // Loop over all observed sats at current epoch
                for (it = gRin.body.begin(); it!= gRin.body.end(); it++)
                {

                        start = std::chrono::steady_clock::now();
                        svn = ((*it).first).id;
                        double satX, satY, satZ;
                        satX = (*it).second.getValue(TypeID::satX);
                        satY = (*it).second.getValue(TypeID::satY);
                        satZ = (*it).second.getValue(TypeID::satZ);
                        Point3 satXYZ = Point3(satX,satY,satZ);
                        double range, rangeRes;
                        range = (*it).second.getValue(TypeID::PC);
                        rangeRes = (*it).second.getValue(TypeID::prefitC);
                        double phase, phaseRes;
                        phase = (*it).second.getValue(TypeID::LC);
                        phaseRes = (*it).second.getValue(TypeID::prefitL);
                        int phase_break;
                        phase_break = (*it).second.getValue(TypeID::satArc);

                        if (first_ob) {
                                startKey = count; first_ob=false;
                                graph->add(PriorFactor<nonBiasStates>(X(count), initEst,  nonBias_InitNoise));
                                initial_values.insert(X(count), initEst);
                        }

                        if (phase_arc[svn]!=phase_break)
                        {
                                bias_state[0] = phase - range;
                                if (count > startKey) { bias_counter[svn] = bias_counter[svn] +1; }
                                graph->add(PriorFactor<phaseBias>(G(bias_counter[svn]), bias_state,  initNoise));
                                initial_values.insert(G(bias_counter[svn]), bias_state);
                                phase_arc[svn] = phase_break;
                        }
                        // Generate pseudorange factor
                        PseudorangeFactor gpsRangeFactor(X(count), rangeRes, satXYZ, nomXYZ, diagNoise::Variances( (gtsam::Vector(1) << elDepWeight(satXYZ, nomXYZ, rangeWeight)).finished()));

                        graph->add(gpsRangeFactor);

                        // Generate phase factor
                        PhaseFactor gpsPhaseFactor(X(count), G(bias_counter[svn]), phaseRes, satXYZ, nomXYZ, diagNoise::Variances( (gtsam::Vector(1) << elDepWeight(satXYZ, nomXYZ, phaseWeight)).finished() ));

                        graph->add(gpsPhaseFactor);
                        prn_vec.push_back(svn);
                }
                if (count > startKey ) {
                        graph->add(BetweenFactor<nonBiasStates>(X(count), X(count-1), between_nonBias_State, nonBias_ProcessNoise));
                }
                isam.update(*graph, initial_values);
                result = isam.calculateEstimate();

                end = std::chrono::steady_clock::now();

                prior_nonBias = result.at<nonBiasStates>(X(count));
                Point3 delta_xyz = (gtsam::Vector(3) << prior_nonBias.x(), prior_nonBias.y(), prior_nonBias.z()).finished();
                Position deltaPos(prior_nonBias.x(), prior_nonBias.y(), prior_nonBias.z());
                prop_xyz = nomXYZ - delta_xyz;
                nominalPos -= deltaPos;

                if (printECEF) {
                        cout << "xyz " << gpstime.week << " " << gpstime.sow << " " << prop_xyz.x() << " " << prop_xyz.y() << " " << prop_xyz.z() << endl;
                }

                if (printENU) {
                        Point3 enu = xyz2enu(prop_xyz, nomXYZ);
                        cout << "enu " << gpstime.week << " " << gpstime.sow << " " << enu.x() << " " << enu.y() << " " << enu.z() << endl;
                }

                if (printAmb) {
                        for (int k=0; k<prn_vec.size(); k++) {
                                cout << "amb. " << gpstime.week << " " << gpstime.sow << " ";
                                cout << prn_vec[k] << " ";
                                cout << result.at<phaseBias>(G(bias_counter[prn_vec[k]])) << endl;
                        }
                }

                if (printUpdateRate) {
                        cout << "Elapsed time "
                             << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count()
                             << " µs" << endl;
                }

                output_time = output_time +1;
                graph->resize(0);
                initial_values.clear();
                prn_vec.clear();
                count++;
                initial_values.insert(X(count), prior_nonBias);
        }
        isam.saveGraph("gnss.tree");
        return 0;
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/types.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>

static int ret;
static int fd;

ros::Publisher pubImu;
#define BAUD 115200 // 115200 for JY61 ,9600 for others

template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3>

ypr2R(const Eigen::MatrixBase<Derived> &ypr) {
  typedef typename Derived::Scalar Scalar_t;

  Scalar_t y = ypr(0) / 180.0 * M_PI;
  Scalar_t p = ypr(1) / 180.0 * M_PI;
  Scalar_t r = ypr(2) / 180.0 * M_PI;

  Eigen::Matrix<Scalar_t, 3, 3> Rz;
  Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1;

  Eigen::Matrix<Scalar_t, 3, 3> Ry;
  Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p);

  Eigen::Matrix<Scalar_t, 3, 3> Rx;
  Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r);

  return Rz * Ry * Rx;
}

int uart_open(int fd, const char *pathname) {
  fd = open(pathname, O_RDWR | O_NOCTTY);
  if (-1 == fd) {
    perror("Can't Open Serial Port");
    return (-1);
  } else
    printf("open %s success!\n", pathname);
  if (isatty(STDIN_FILENO) == 0)
    printf("standard input is not a terminal device\n");
  else
    printf("isatty success!\n");
  return fd;
}

int uart_set(int fd, int nSpeed, int nBits, char nEvent, int nStop) {
  struct termios newtio, oldtio;
  if (tcgetattr(fd, &oldtio) != 0) {
    perror("SetupSerial 1");
    printf("tcgetattr( fd,&oldtio) -> %d\n", tcgetattr(fd, &oldtio));
    return -1;
  }
  bzero(&newtio, sizeof(newtio));
  newtio.c_cflag |= CLOCAL | CREAD;
  newtio.c_cflag &= ~CSIZE;
  switch (nBits) {
  case 7:
    newtio.c_cflag |= CS7;
    break;
  case 8:
    newtio.c_cflag |= CS8;
    break;
  }
  switch (nEvent) {
  case 'o':
  case 'O':
    newtio.c_cflag |= PARENB;
    newtio.c_cflag |= PARODD;
    newtio.c_iflag |= (INPCK | ISTRIP);
    break;
  case 'e':
  case 'E':
    newtio.c_iflag |= (INPCK | ISTRIP);
    newtio.c_cflag |= PARENB;
    newtio.c_cflag &= ~PARODD;
    break;
  case 'n':
  case 'N':
    newtio.c_cflag &= ~PARENB;
    break;
  default:
    break;
  }

  /*设置波特率*/

  switch (nSpeed) {
  case 2400:
    cfsetispeed(&newtio, B2400);
    cfsetospeed(&newtio, B2400);
    break;
  case 4800:
    cfsetispeed(&newtio, B4800);
    cfsetospeed(&newtio, B4800);
    break;
  case 9600:
    cfsetispeed(&newtio, B9600);
    cfsetospeed(&newtio, B9600);
    break;
  case 115200:
    cfsetispeed(&newtio, B115200);
    cfsetospeed(&newtio, B115200);
    break;
  case 460800:
    cfsetispeed(&newtio, B460800);
    cfsetospeed(&newtio, B460800);
    break;
  default:
    cfsetispeed(&newtio, B9600);
    cfsetospeed(&newtio, B9600);
    break;
  }
  if (nStop == 1)
    newtio.c_cflag &= ~CSTOPB;
  else if (nStop == 2)
    newtio.c_cflag |= CSTOPB;
  newtio.c_cc[VTIME] = 0;
  newtio.c_cc[VMIN] = 0;
  tcflush(fd, TCIFLUSH);

  if ((tcsetattr(fd, TCSANOW, &newtio)) != 0) {
    perror("com set error");
    return -1;
  }
  printf("set done!\n");
  return 0;
}

int uart_close(int fd) {
  assert(fd);
  close(fd);

  return 0;
}
int send_data(int fd, char *send_buffer, int length) {
  length = write(fd, send_buffer, length * sizeof(unsigned char));
  return length;
}

int recv_data(int fd, char *recv_buffer, int length) {
  int has_receive_length = 0;
  while (length - has_receive_length > 0) {

    has_receive_length +=
        read(fd, recv_buffer + has_receive_length, length - has_receive_length);
    usleep(1000);
    //        std::cout << "has_receive_length: " << has_receive_length <<
    //        std::endl;
  }
  return length;
}

float a[3], w[3], Angle[3], h[3];
void ParseData(char *buffer) {
  char chrBuf[100];
  signed short sData[4];

  for (int i = 0; i < 4; i++) {
    memcpy(&chrBuf[0], buffer + 11 * i, 11);

    //        std::cout << "i: " << i << std::endl;
    //        for(int m =0; m< 11; m++)
    //        {
    //            printf("%x  ", chrBuf[m]);
    //        }
    memcpy(&sData[0], &chrBuf[2], 8);
    char cTemp = 0;
    time_t now;
    for (int j = 0; j < 10; j++) {
      cTemp += chrBuf[j];
    }

    if ((chrBuf[0] != 0x55) || ((chrBuf[1] & 0x50) != 0x50) ||
        (cTemp != chrBuf[10])) {
      printf("Error:%x  %x\r\n", chrBuf[0], chrBuf[1]);
      return;
    }
    switch (chrBuf[1]) {
    case 0x51:
      for (int k = 0; k < 3; k++) {
        a[k] = (float)sData[k] / 32768.0 * 16.0 * 9.81;
      }
      time(&now);
      printf("\r\nT:%s a:%6.3f %6.3f %6.3f ", asctime(localtime(&now)), a[0],
             a[1], a[2]);

      break;
    case 0x52:
      for (int k = 0; k < 3; k++) {
        w[k] = (float)sData[k] / 32768.0 * 2000.0/180*M_PI;
      }
      printf("w:%7.3f %7.3f %7.3f ", w[0], w[1], w[2]);
      break;
    case 0x53:
      for (int k = 0; k < 3; k++) {
        Angle[k] = (float)sData[k] / 32768.0 * 180.0;
      }
      printf("A:%7.3f %7.3f %7.3f ", Angle[0], Angle[1], Angle[2]);

      break;

    case 0x54:
      for (int k = 0; k < 3; k++) {
        h[k] = (float)sData[k];
      }
      printf("h:%4.0f %4.0f %4.0f ", h[0], h[1], h[2]);

      break;
    }
  }

  sensor_msgs::Imu imu_msg;

  imu_msg.header.stamp = ros::Time::now();
  imu_msg.header.frame_id = "/imu";
  imu_msg.angular_velocity.x = w[0];
  imu_msg.angular_velocity.y = w[1];
  imu_msg.angular_velocity.z = w[2];

  Eigen::Vector3d ypr;
  ypr << Angle[2], Angle[1], Angle[0];
  Eigen::Matrix R = ypr2R(ypr);
  std::cout << "R:\n " << R << std::endl;
  Eigen::Quaterniond q(R);
  q.normalize();

  imu_msg.orientation.w = q.w();
  imu_msg.orientation.x = q.x();
  imu_msg.orientation.y = q.y();
  imu_msg.orientation.z = q.z();

  imu_msg.linear_acceleration.x = a[0];
  imu_msg.linear_acceleration.y = a[1];
  imu_msg.linear_acceleration.z = a[2];

  pubImu.publish(imu_msg);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "xense_driver");
  ros::NodeHandle n_private("~");

  char r_buf[1024];
  bzero(r_buf, 1024);

  fd = uart_open(fd, "/dev/ttyUSB0"); /*串口号/dev/ttySn,USB口号/dev/ttyUSBn */
  if (fd == -1) {
    fprintf(stderr, "uart_open error\n");
    exit(EXIT_FAILURE);
  }

  if (uart_set(fd, BAUD, 8, 'N', 1) == -1) {
    fprintf(stderr, "uart set failed!\n");
    exit(EXIT_FAILURE);
  }

  pubImu = n_private.advertise<sensor_msgs::Imu>("/imu/data", 3000);

  while (ros::ok()) {
    ret = recv_data(fd, r_buf, 1);

    if (r_buf[0] != 0x55)
      continue;
    ret = recv_data(fd, r_buf + 1, 1);
    if (r_buf[1] != 0x51) {
      continue;
    }

    ret = recv_data(fd, r_buf + 2, 44 - 2);

    //        for(int i =0; i< 44; i++)
    //        {
    //            printf("%x  ", r_buf[i]);
    //        }
    std::cout << std::endl;
    ParseData(r_buf);
    usleep(1000);
  }

  ret = uart_close(fd);
  if (ret == -1) {
    fprintf(stderr, "uart_close error\n");
    exit(EXIT_FAILURE);
  }

  exit(EXIT_SUCCESS);
}


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <fstream>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/dipole_pseudo_pathway.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_mask.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/circular_dichroism_config.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/sites_to_states.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using num::int_t;
using num::real_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::circular_dichroism_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		int_t steps_per_iteration = heom_config.program_observe_steps();
		int_t total_steps = iterations * heom_config.program_observe_steps();

		//output data structure for traces
		const size_t num_dipole_matrices = heom_config.dipole_tensor_prefactors().size();
		const int_t num_observations = iterations + 1;
		std::vector<heom::matrix_trace_observation_view> observations(num_observations);

		// circular dichroism pseudo pathway with ground state
		const auto& pathway_spec = heom::dipole_pseudo_pathway_wgs_spec;
		const auto sts_mode = pathway_spec.sites_to_states_mode();

		// for progress estimate
		const size_t solver_runs = num_dipole_matrices;
		size_t solver_run = 0;

		// iterate over all dipoles and compute population dynamics
		for (size_t tensor_index = 0; tensor_index < heom_config.dipole_tensor_prefactors().size(); ++tensor_index) {
			heom::instance heom_instance(heom_config, sts_mode, complete_graph);
			heom_instance.set_hierarchy_top(); // NOTE: hierarchy top is initialised to zero with first element being complex_t(1.0,0.0) here

			// OpenCL range
			ocl::nd_range range {
				{}, // offset
				{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
				{1, 1} // local size
			};

			// create a solver from configuration and instance
			solver_t solver(ocl_config, range, heom_config, heom_instance);
			std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
			solver.write_ocl_runtime_config(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;
			++solver_run; // for progress estimate

			// compute dipole matrices
			auto dipole_matrix_minus = heom::get_dipole_matrix_for_pathway_circular(pathway_spec, 0, heom_config.system_sites(), heom_config, tensor_index);
			auto dipole_matrix_plus  = heom::get_dipole_matrix_for_pathway(pathway_spec, 1, heom_config.system_sites(), heom_config, tensor_index);

			// pre-scale D- for observer
			dipole_matrix_minus.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));

			// multiply D+ from left
			solver.hierarchy_mmult_left(dipole_matrix_plus.data());

			// setup observer with pre-scaled D-
			heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix_minus.data()); // D- from left via observer

			// generate first observation with initial value
			observations[0] += trace_observer.observe_trace(0.0);

			// write header once
			if (tensor_index == 0)
				trace_observer.write_header(observation_file, true);

			for (int_t i = 0; i < iterations; ++i) {
				// propagate
				solver.step_forward(steps_per_iteration);

				// after propagation, we are at:
				const auto current_step = (i + 1) * steps_per_iteration;
				const real_t current_time = current_step * heom_config.solver_step_size();

				// observe
				observations[i + 1] += trace_observer.observe_trace(current_time);

				// update status
				heom::write_progress(current_step - 1, total_steps, solver_run, solver_runs, "Calculation circular dichroism: ", std::cout);
			}

			// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
			std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
			solver.write_runtime_summary(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;

		} // for tensor_index

		// write averaged traces
		for (auto& obs : observations) {
			obs.avg(num_dipole_matrices);
			observation_file << obs << '\n';
		}
		observation_file << std::flush; // tell the OS to write to disk

		std::cout   << "-------------------- Memory Summary ------------------------" << std::endl;
		std::cout   << "max. heap via aligned::allocate(..):    "
		            << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		            << " ("
		            << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		            << ')' << std::endl;
//		std::cout   << "max. OpenCL buffer allocations:         "
//		            << boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		            << " ("
//		            << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
//		            << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "time in main():\t" << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h"

namespace tudat
{

namespace propagators
{

//! Function to reset the state transition and sensitivity matrix interpolators
void SingleArcCombinedStateTransitionAndSensitivityMatrixInterface::updateMatrixInterpolators(
        const std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >
        stateTransitionMatrixInterpolator,
        const std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >
<<<<<<< HEAD
        sensitivityMatrixInterpolator )
=======
        sensitivityMatrixInterpolator,
        const std::vector< std::pair< int, int > >& statePartialAdditionIndices )
>>>>>>> origin/master
{
    stateTransitionMatrixInterpolator_ = stateTransitionMatrixInterpolator;
    sensitivityMatrixInterpolator_ = sensitivityMatrixInterpolator;
    statePartialAdditionIndices_ = statePartialAdditionIndices;
}

//! Function to get the concatenated state transition and sensitivity matrix at a given time.
Eigen::MatrixXd SingleArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    combinedStateTransitionMatrix_.setZero( );

    // Set Phi and S matrices.
    combinedStateTransitionMatrix_.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
            stateTransitionMatrixInterpolator_->interpolate( evaluationTime );

    if( sensitivityMatrixSize_ > 0 )
    {
        combinedStateTransitionMatrix_.block( 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
                sensitivityMatrixInterpolator_->interpolate( evaluationTime );
    }

    for( unsigned int i = 0; i < statePartialAdditionIndices_.size( ); i++ )
    {
        combinedStateTransitionMatrix_.block( statePartialAdditionIndices_.at( i ).first, 0, 6, stateTransitionMatrixSize_ + sensitivityMatrixSize_ ) +=
                combinedStateTransitionMatrix_.block( statePartialAdditionIndices_.at( i ).second, 0, 6, stateTransitionMatrixSize_ + sensitivityMatrixSize_ );
    }

    return combinedStateTransitionMatrix_;
}

//! Constructor
MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::MultiArcCombinedStateTransitionAndSensitivityMatrixInterface(
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        stateTransitionMatrixInterpolators,
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        sensitivityMatrixInterpolators,
        const std::vector< double >& arcStartTimes,
        const std::vector< double >& arcEndTimes,
        const int numberOfInitialDynamicalParameters,
        const int numberOfParameters,
        const std::vector< std::vector< std::pair< int, int > > >& statePartialAdditionIndices ):
    CombinedStateTransitionAndSensitivityMatrixInterface( numberOfInitialDynamicalParameters, numberOfParameters ),
    stateTransitionMatrixInterpolators_( stateTransitionMatrixInterpolators ),
    sensitivityMatrixInterpolators_( sensitivityMatrixInterpolators ),
    arcStartTimes_( arcStartTimes ),
    arcEndTimes_( arcEndTimes ),
    statePartialAdditionIndices_( statePartialAdditionIndices )
{
    if( arcStartTimes_.size( ) != arcEndTimes_.size( ) )
    {
        throw std::runtime_error( "Error when making MultiArcCombinedStateTransitionAndSensitivityMatrixInterface, incompatible time lists" );
    }
    numberOfStateArcs_ = arcStartTimes_.size( );

    sensitivityMatrixSize_ = numberOfParameters - numberOfStateArcs_ * stateTransitionMatrixSize_;

    if( stateTransitionMatrixInterpolators_.size( ) != sensitivityMatrixInterpolators_.size( ) ||
            stateTransitionMatrixInterpolators_.size( ) != static_cast< unsigned int >( numberOfStateArcs_ ) )
    {
        throw std::runtime_error(
                    "Error when making multi arc state transition and sensitivity interface, vector sizes are inconsistent" );
    }

    std::vector< double > arcSplitTimes = arcStartTimes_;
    arcSplitTimes.push_back(  std::numeric_limits< double >::max( ));
    lookUpscheme_ = std::make_shared< interpolators::HuntingAlgorithmLookupScheme< double > >(
                arcSplitTimes );
}

//! Function to reset the state transition and sensitivity matrix interpolators
void MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::updateMatrixInterpolators(
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        stateTransitionMatrixInterpolators,
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        sensitivityMatrixInterpolators,
        const std::vector< double >& arcStartTimes,
        const std::vector< double >& arcEndTimes,
        const std::vector< std::vector< std::pair< int, int > > >& statePartialAdditionIndices )
{
    stateTransitionMatrixInterpolators_ = stateTransitionMatrixInterpolators;
    sensitivityMatrixInterpolators_ = sensitivityMatrixInterpolators;
    arcStartTimes_ =  arcStartTimes;
    arcEndTimes_ = arcEndTimes;
    statePartialAdditionIndices_ = statePartialAdditionIndices;

    if( stateTransitionMatrixInterpolators_.size( ) != sensitivityMatrixInterpolators_.size( ) ||
            stateTransitionMatrixInterpolators_.size( ) != static_cast< unsigned int >( numberOfStateArcs_ ) )
    {
        throw std::runtime_error(
                    "Error when resetting multi arc state transition and sensitivity interface, vector sizes are inconsistent." );
    }

    std::vector< double > arcSplitTimes = arcStartTimes_;
    arcSplitTimes.push_back( std::numeric_limits< double >::max( ) );

    lookUpscheme_ = std::make_shared< interpolators::HuntingAlgorithmLookupScheme< double > >(
                arcSplitTimes );
}

//! Function to get the concatenated single-arc state transition and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime,
        const bool addCentralBodySensitivity )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    int currentArc = getCurrentArc( evaluationTime ).first;

    // Set Phi and S matrices.
    if( currentArc >= 0 )
    {
        combinedStateTransitionMatrix.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
                stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
        combinedStateTransitionMatrix.block( 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
                sensitivityMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );

        for( unsigned int i = 0; i < statePartialAdditionIndices_.at( currentArc ).size( ); i++ )
        {
            int indicesToAdd = addCentralBodySensitivity ?
                        ( stateTransitionMatrixSize_ + sensitivityMatrixSize_ ) : stateTransitionMatrixSize_;
            combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices_.at( currentArc ).at( i ).first, 0, 6, indicesToAdd ) +=
                    combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices_.at( currentArc ).at( i ).second, 0, 6, indicesToAdd );
        }
    }
    return combinedStateTransitionMatrix;
}

//! Function to get the concatenated single-arc state transition and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    return getCombinedStateTransitionAndSensitivityMatrix( evaluationTime, true );
}

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime,
        const bool addCentralBodySensitivity )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = getCombinedStateTransitionAndSensitivityMatrix(
                evaluationTime, addCentralBodySensitivity );
    Eigen::MatrixXd fullCombinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, numberOfStateArcs_ * stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    int currentArc = getCurrentArc( evaluationTime ).first;

    // Set Phi and S matrices of current arc.
<<<<<<< HEAD
    combinedStateTransitionMatrix.block( 0, currentArc * stateTransitionMatrixSize_, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
            stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );

    combinedStateTransitionMatrix.block(
                0, numberOfStateArcs_ * stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
            sensitivityMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
=======
    if( currentArc >= 0 )
    {
        fullCombinedStateTransitionMatrix.block(
                    0, currentArc * stateTransitionMatrixSize_, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
                stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
        fullCombinedStateTransitionMatrix.block(
                    0, numberOfStateArcs_ * stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
                combinedStateTransitionMatrix.block( 0, 0, stateTransitionMatrixSize_, sensitivityMatrixSize_ );
    }
    return fullCombinedStateTransitionMatrix;
}
>>>>>>> origin/master

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    return getFullCombinedStateTransitionAndSensitivityMatrix( evaluationTime, true );
}

//! Function to retrieve the current arc for a given time
std::pair< int, double > MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getCurrentArc( const double evaluationTime )
{

    int currentArc =  lookUpscheme_->findNearestLowerNeighbour( evaluationTime );
    if( evaluationTime < arcEndTimes_.at( currentArc ) && evaluationTime > arcStartTimes_.at( currentArc ) )
    {
        return std::make_pair( currentArc, arcStartTimes_.at( currentArc ) );
    }
    else
    {
        return std::make_pair( -1, TUDAT_NAN );
    }
}

//! Function to get the concatenated state transition and sensitivity matrix at a given time.
Eigen::MatrixXd HybridArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    // Get single-arc matrices
    Eigen::MatrixXd singleArcStateTransition = singleArcInterface_->getCombinedStateTransitionAndSensitivityMatrix(
                evaluationTime );

    // Get multi-arc matrices
    Eigen::MatrixXd multiArcStateTransition = multiArcInterface_->getCombinedStateTransitionAndSensitivityMatrix(
                evaluationTime, false );
    std::pair< int, double >  currentArc = multiArcInterface_->getCurrentArc( evaluationTime );

    // Set single-arc block
    combinedStateTransitionMatrix.block( 0, 0, singleArcStateSize_, singleArcStateSize_ ) =
            singleArcStateTransition.block( 0, 0, singleArcStateSize_, singleArcStateSize_ );

    // Set single-arc sensitivity block
    combinedStateTransitionMatrix.block( 0, multiArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ ) =
            singleArcStateTransition.block( 0, singleArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ );

    if( !( currentArc.first < 0 ) )
    {
        // Set multi-arc block
        combinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_, originalMultiArcStateSize_, originalMultiArcStateSize_ ) =
                multiArcStateTransition.block(
                    singleArcStateSize_, singleArcStateSize_, originalMultiArcStateSize_, originalMultiArcStateSize_ );

        // Get single-arc matrices at current arc start
        Eigen::MatrixXd singleArcStateTransitionAtArcStart = singleArcInterface_->getCombinedStateTransitionAndSensitivityMatrix(
                    currentArc.second );

        // Set coupled block
        combinedStateTransitionMatrix.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ ) =
                multiArcStateTransition.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ ) *
                singleArcStateTransitionAtArcStart.block(
                    0, 0, singleArcStateSize_, singleArcStateSize_ );

        // Set multi-arc sensitivity block
        combinedStateTransitionMatrix.block(
                    singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ ) =
                multiArcStateTransition.block(
                    singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ );

        std::vector< std::pair< int, int > > statePartialAdditionIndices =
                multiArcInterface_->getStatePartialAdditionIndices( currentArc.first );

        for( unsigned int i = 0; i < statePartialAdditionIndices.size( ); i++ )
        {
            combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices.at( i ).first, multiArcStateSize_,
                        6, sensitivityMatrixSize_ ) +=
                    combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices.at( i ).second, multiArcStateSize_,
                        6, sensitivityMatrixSize_ );

        }
    }

    return combinedStateTransitionMatrix;

}

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd HybridArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = getCombinedStateTransitionAndSensitivityMatrix( evaluationTime );
    Eigen::MatrixXd fullCombinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_ + sensitivityMatrixSize_ );

    fullCombinedStateTransitionMatrix.block(
                0, 0, singleArcStateSize_, singleArcStateSize_ ) =
            combinedStateTransitionMatrix.block(
                0, 0, singleArcStateSize_, singleArcStateSize_ );

    fullCombinedStateTransitionMatrix.block(
                0, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ ) =
            combinedStateTransitionMatrix.block(
                0, multiArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ );

    int currentArc = multiArcInterface_->getCurrentArc( evaluationTime ).first;

    // Set Phi and S matrices of current arc.
    if( currentArc >= 0 )
    {

        // Set multi-arc block
        fullCombinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_ + currentArc * originalMultiArcStateSize_,
                    originalMultiArcStateSize_, originalMultiArcStateSize_ ) =
                combinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_, originalMultiArcStateSize_, originalMultiArcStateSize_ );

<<<<<<< HEAD
    // Set sensitivity block
    combinedStateTransitionMatrix.block(
                0, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_,
                singleArcStateSize_, sensitivityMatrixSize_ ) =
            singleArcStateTransition.block( 0, singleArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ );
    combinedStateTransitionMatrix.block(
                singleArcStateSize_, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_,
                originalMultiArcStateSize_, sensitivityMatrixSize_ ) =
            multiArcStateTransition.block(
                singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ );
=======
        // Set coupled block
        fullCombinedStateTransitionMatrix.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ ) =
                combinedStateTransitionMatrix.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ );
>>>>>>> origin/master

        // Set multi-arc sensitivity block
        fullCombinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_,
                    originalMultiArcStateSize_, sensitivityMatrixSize_ ) =
                combinedStateTransitionMatrix.block(
                    singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ );

    }

    return fullCombinedStateTransitionMatrix;
}

}

}


/*
 qe2alm.cpp

 Copyright (c) 2015 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/version.hpp>
#include <boost/lexical_cast.hpp>
#include "memory.h"
#include "constants.h"
#include "mathfunctions.h"
#include "qe2alm.h"

using namespace std;

int main()
{
    cout << " QE2ALM -- a converter of harmonic FCs" << endl;
    cout << " Input file (QE fc file generated by q2r.x) : ";
    cin >> file_fc2qe;
    cout << " Output file (xml file for ALAMODE) : ";
    cin >> file_xmlout;
    cout << " Impose acoustic sum rule ? [y/n] : ";
    cin >> flag_asr;

    ifs_fc2qe.open(file_fc2qe.c_str(), std::ios::in);
    if (!ifs_fc2qe) {
        cout << " ERROR: Cannot open file " << file_fc2qe << endl;
        exit(EXIT_FAILURE);
    }

    transform(flag_asr.begin(), flag_asr.end(), flag_asr.begin(), ::tolower);
    if (flag_asr[0] == 'y') {
        consider_asr = true;
    } else if (flag_asr[0] == 'n') {
        consider_asr = false;
    } else {
        cout << " ERROR: Please specify 'y' or 'n' for the acoustic sum rule." << endl;
        exit(EXIT_FAILURE);
    }

    // Read structural parameters

    ifs_fc2qe >> nkd >> natmin >> ibrav
              >> celldm[0] >> celldm[1] >> celldm[2]
              >> celldm[3] >> celldm[4] >> celldm[5];

    if (ibrav == 0) {
        for (i = 0; i < 3; ++i) {
            ifs_fc2qe >> lavec[0][i] >> lavec[1][i] >> lavec[2][i];
        }

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                lavec[i][j] *= celldm[0];
            }
        }
    } else {
        calc_lattice_vector(ibrav, celldm, lavec);
    }

    recips(lavec, rlavec);

    int dummy;
    string str_dummy;
    double tmp;

    allocate(kd_symbol, nkd);

    for (i = 0; i < nkd; ++i) {
        ifs_fc2qe >> dummy >> kd_symbol[i] >> str_dummy >> tmp;
        kd_symbol[i] = kd_symbol[i].substr(1);
    }

    allocate(kd, natmin);
    allocate(xcrd, natmin, 3);

    for (i = 0; i < natmin; ++i) {
        ifs_fc2qe >> dummy >> kd[i] >> xcrd[i][0] >> xcrd[i][1] >> xcrd[i][2];
    }

    std::string str_na;
    ifs_fc2qe >> str_na;

    if (str_na[0] == 'T') {
        include_na = true;
    } else if (str_na[0] == 'F') {
        include_na = false;
    } else {
        cout << " ERROR: This cannot happen " << endl;
        exit(EXIT_FAILURE);
    }

    if (include_na) {
        cout << " WARNING: The given force constant file contains information of" << endl;
        cout << "          dielectric constants and Born effective charges." << endl;
        cout << "          The program skips these parts and converts the" << endl;
        cout << "          short-range terms only." << endl;
    }

    if (include_na) {
        allocate(born, natmin, 3, 3);
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                ifs_fc2qe >> dielec[i][j];
            }
        }
        for (i = 0; i < natmin; ++i) {
            ifs_fc2qe >> dummy;
            for (j = 0; j < 3; ++j) {
                for (k = 0; k < 3; ++k) {
                    ifs_fc2qe >> born[i][j][k];
                }
            }
        }
    }

    // Read force constant info

    ifs_fc2qe >> nq[0] >> nq[1] >> nq[2];
    nsize = nq[0] * nq[1] * nq[2];

    allocate(fc2, 3 * natmin, 3 * natmin, nsize);

    int icell = 0;
    int m1, m2, m3;
    int icrd, jcrd;
    int iat, jat;

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            for (iat = 0; iat < natmin; ++iat) {
                for (jat = 0; jat < natmin; ++jat) {
                    ifs_fc2qe >> dummy >> dummy >> dummy >> dummy;

                    icell = 0;
                    for (m3 = 0; m3 < nq[2]; ++m3) {
                        for (m2 = 0; m2 < nq[1]; ++m2) {
                            for (m1 = 0; m1 < nq[0]; ++m1) {
                                ifs_fc2qe >> dummy >> dummy >> dummy;

                                // Force constant matrix should be transposed here.
                                ifs_fc2qe >> fc2[3 * jat + jcrd][3 * iat + icrd][icell];

                                ++icell;
                            }
                        }
                    }
                }
            }
        }
    }

    if (consider_asr) {

        double sum_fc;

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jcrd = 0; jcrd < 3; ++jcrd) {
                for (iat = 0; iat < natmin; ++iat) {
                    sum_fc = 0.0;
                    for (jat = 0; jat < natmin; ++jat) {
                        for (icell = 0; icell < nsize; ++icell) {
                            sum_fc += fc2[3 * iat + icrd][3 * jat + jcrd][icell];
                        }
                    }
                    fc2[3 * iat + icrd][3 * iat + jcrd][0] -= sum_fc;
                }
            }
        }
    }

    // Convert positions from alat to crystal basis
    for (i = 0; i < natmin; ++i) {
        rotvec(xcrd[i], xcrd[i], rlavec);
        for (j = 0; j < 3; ++j) {
            xcrd[i][j] *= celldm[0] / (2.0 * pi);
        }
    }

    // Extend the system to the supercell

    nat = natmin * nsize;

    allocate(xcrd_super, nat, 3);
    allocate(kd_super, nsize, natmin);
    allocate(map_p2s, nsize, natmin);

    icell = 0;
    int icount = 0;

    for (m3 = 0; m3 < nq[2]; ++m3) {
        for (m2 = 0; m2 < nq[1]; ++m2) {
            for (m1 = 0; m1 < nq[0]; ++m1) {
                for (i = 0; i < natmin; ++i) {
                    xcrd_super[icount][0] = (xcrd[i][0] + static_cast<double>(m1)) / static_cast<double>(nq[0]);
                    xcrd_super[icount][1] = (xcrd[i][1] + static_cast<double>(m2)) / static_cast<double>(nq[1]);
                    xcrd_super[icount][2] = (xcrd[i][2] + static_cast<double>(m3)) / static_cast<double>(nq[2]);
                    kd_super[icell][i] = kd[i];

                    map_p2s[icell][i] = icount;
                    ++icount;
                }
                ++icell;
            }
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            lavec[i][j] *= static_cast<double>(nq[j]);
        }
    }

    allocate(mindist, natmin, nat);

    get_pairs_of_minimum_distance(natmin, nat, map_p2s, xcrd_super, mindist);

    // Write to XML file

    using boost::property_tree::ptree;

    ptree pt;
    string str_pos[3], str_tmp;

    str_tmp.clear();

    //    for (i = 0; i < 3; ++i) {
    //        str_tmp += " " + std::to_string(nq[i]);
    //    }

    pt.put("Data.FilenameOfQE", file_fc2qe);
    //    pt.put("Data.Qmesh", str_tmp);

    pt.put("Data.Structure.NumberOfAtoms", nat);
    pt.put("Data.Structure.NumberOfElements", nkd);

    for (i = 0; i < nkd; ++i) {
        ptree &child = pt.add("Data.Structure.AtomicElements.element", kd_symbol[i]);
        child.put("<xmlattr>.number", i + 1);
    }

    for (i = 0; i < 3; ++i) {
        str_pos[i].clear();
        for (j = 0; j < 3; ++j) {
            str_pos[i] += " " + double2string(lavec[j][i]);
        }
    }
    pt.put("Data.Structure.LatticeVector", "");
    pt.put("Data.Structure.LatticeVector.a1", str_pos[0]);
    pt.put("Data.Structure.LatticeVector.a2", str_pos[1]);
    pt.put("Data.Structure.LatticeVector.a3", str_pos[2]);

    pt.put("Data.Structure.Position", "");

    icount = 0;

    for (i = 0; i < nsize; ++i) {

        for (j = 0; j < natmin; ++j) {
            str_tmp.clear();
            for (k = 0; k < 3; ++k) str_tmp += " " + double2string(xcrd_super[map_p2s[i][j]][k]);
            ptree &child = pt.add("Data.Structure.Position.pos", str_tmp);
            child.put("<xmlattr>.index", icount + 1);
            child.put("<xmlattr>.element", kd_symbol[kd_super[i][j] - 1]);

            ++icount;
        }
    }

    pt.put("Data.Symmetry.NumberOfTranslations", nsize);
    for (i = 0; i < nsize; ++i) {
        for (j = 0; j < natmin; ++j) {
            ptree &child = pt.add("Data.Symmetry.Translations.map", map_p2s[i][j] + 1);
            child.put("<xmlattr>.tran", i + 1);
            child.put("<xmlattr>.atom", j + 1);
        }
    }

    pt.put("Data.ForceConstants", "");
    str_tmp.clear();

    int nmulti, kat;

    for (iat = 0; iat < natmin; ++iat) {
        for (icrd = 0; icrd < 3; ++icrd) {
            for (icell = 0; icell < nsize; ++icell) {
                for (jat = 0; jat < natmin; ++jat) {
                    kat = map_p2s[icell][jat];
                    nmulti = mindist[iat][kat].size();
                    for (jcrd = 0; jcrd < 3; ++jcrd) {
                        if (std::abs(fc2[3 * iat + icrd][3 * jat + jcrd][icell]) < eps15) continue;
                        for (i = 0; i < nmulti; ++i) {
                            ptree &child = pt.add("Data.ForceConstants.HARMONIC.FC2",
                                                  double2string(fc2[3 * iat + icrd][3 * jat + jcrd][icell] /
                                                                static_cast<double>(nmulti)));

                            child.put("<xmlattr>.pair1", boost::lexical_cast<std::string>(iat + 1)
                                                         + " " + boost::lexical_cast<std::string>(icrd + 1));
                            child.put("<xmlattr>.pair2", boost::lexical_cast<std::string>(kat + 1)
                                                         + " " + boost::lexical_cast<std::string>(jcrd + 1)
                                                         + " " + boost::lexical_cast<std::string>(
                                    mindist[iat][kat][i].cell + 1));
                        }
                    }
                }
            }
        }
    }

    using namespace boost::property_tree::xml_parser;
    const int indent = 2;

#if BOOST_VERSION >= 105600
    write_xml(file_xmlout, pt, std::locale(),
              xml_writer_make_settings<ptree::key_type>(' ', indent, widen<std::string>("utf-8")));
#else
    write_xml(file_xmlout, pt, std::locale(),
              xml_writer_make_settings(' ', indent, widen<char>("utf-8")));
#endif

    deallocate(kd);
    deallocate(kd_symbol);
    deallocate(xcrd);
    deallocate(fc2);
    deallocate(xcrd_super);
    deallocate(kd_super);
    deallocate(map_p2s);

    if (include_na) {
        deallocate(born);
    }
}

void calc_lattice_vector(const int ibrav, double celldm[6], double aa[3][3])
{
    double a, b, c;
    double alpha, beta, gamma;
    double tx, ty, tz;
    double a2, b2, c2;
    double cosalpha, a_prime, u, v;

    switch (ibrav) {
        case 1:
            a = celldm[0];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = a;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = a;

            break;

        case 2:
            a = celldm[0] / 2.0;
            aa[0][0] = -a;
            aa[0][1] = 0.0;
            aa[0][2] = a;
            aa[1][0] = 0.0;
            aa[1][1] = a;
            aa[1][2] = a;
            aa[2][0] = -a;
            aa[2][1] = a;
            aa[2][2] = 0.0;

            break;

        case 3:
            a = celldm[0] / 2.0;
            aa[0][0] = a;
            aa[0][1] = a;
            aa[0][2] = a;
            aa[1][0] = -a;
            aa[1][1] = a;
            aa[1][2] = a;
            aa[2][0] = -a;
            aa[2][1] = -a;
            aa[2][2] = a;

            break;

        case 4:
            a = celldm[0];
            c = celldm[0] * celldm[2];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = -0.5 * a;
            aa[1][1] = sqrt(3.0) / 2.0 * a;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case 5:
        case -5:
            a = celldm[0];
            cosalpha = celldm[3];
            tx = a * sqrt((1.0 - cosalpha) / 2.0);
            ty = a * sqrt((1.0 - cosalpha) / 6.0);
            tz = a * sqrt((1.0 + 2.0 * cosalpha) / 3.0);

            if (ibrav == 5) {
                aa[0][0] = tx;
                aa[0][1] = -ty;
                aa[0][2] = tz;
                aa[1][0] = 0.0;
                aa[1][1] = 2.0 * ty;
                aa[1][2] = tz;
                aa[2][0] = -tx;
                aa[2][1] = -ty;
                aa[2][2] = tz;
            } else {
                a_prime = a / sqrt(3.0);
                u = tz - 2.0 * sqrt(2.0) * ty;
                v = tz + sqrt(2.0) * ty;
                aa[0][0] = u;
                aa[0][1] = v;
                aa[0][2] = v;
                aa[1][0] = v;
                aa[1][1] = u;
                aa[1][2] = v;
                aa[2][0] = v;
                aa[2][1] = v;
                aa[2][2] = u;
            }

            break;

        case 6:
            a = celldm[0];
            c = celldm[0] * celldm[2];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = a;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case 7:
            a = celldm[0];
            c = celldm[0] * celldm[2];
            a2 = a * 0.5;
            c2 = c * 0.5;
            aa[0][0] = a2;
            aa[0][1] = -a2;
            aa[0][2] = c2;
            aa[1][0] = a2;
            aa[1][1] = a2;
            aa[1][2] = c2;
            aa[2][0] = -a2;
            aa[2][1] = -a2;
            aa[2][2] = c2;

            break;

        case 8:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = b;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case 9:
        case -9:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            a2 = a * 0.5;
            b2 = b * 0.5;

            if (ibrav == 9) {
                aa[0][0] = a2;
                aa[0][1] = b2;
                aa[0][2] = 0.0;
                aa[1][0] = -a2;
                aa[1][1] = b2;
                aa[1][2] = 0.0;
                aa[2][0] = 0.0;
                aa[2][1] = 0.0;
                aa[2][2] = c;
            } else {
                aa[0][0] = a2;
                aa[0][1] = -b2;
                aa[0][2] = 0.0;
                aa[1][0] = a2;
                aa[1][1] = b2;
                aa[1][2] = 0.0;
                aa[2][0] = 0.0;
                aa[2][1] = 0.0;
                aa[2][2] = c;
            }

            break;

        case 10:
            a2 = celldm[0] * 0.5;
            b2 = celldm[0] * celldm[1] * 0.5;
            c2 = celldm[0] * celldm[2] * 0.5;
            aa[0][0] = a2;
            aa[0][1] = 0.0;
            aa[0][2] = c2;
            aa[1][0] = a2;
            aa[1][1] = b2;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = b2;
            aa[2][2] = c2;

            break;

        case 11:
            a2 = celldm[0] * 0.5;
            b2 = celldm[0] * celldm[1] * 0.5;
            c2 = celldm[0] * celldm[2] * 0.5;
            aa[0][0] = a2;
            aa[0][1] = b2;
            aa[0][2] = c2;
            aa[1][0] = -a2;
            aa[1][1] = b2;
            aa[1][2] = c2;
            aa[2][0] = -a2;
            aa[2][1] = -b2;
            aa[2][2] = c2;

            break;

        case 12:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            gamma = acos(celldm[3]);
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = b * cos(gamma);
            aa[1][1] = b * sin(gamma);
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case -12:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            beta = acos(celldm[4]);
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = b;
            aa[1][2] = 0.0;
            aa[2][0] = c * cos(beta);
            aa[2][1] = 0.0;
            aa[2][2] = c * sin(beta);

            break;

        case 13:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            gamma = acos(celldm[3]);
            aa[0][0] = 0.5 * a;
            aa[0][1] = 0.0;
            aa[0][2] = -0.5 * c;
            aa[1][0] = b * cos(gamma);
            aa[1][1] = b * sin(gamma);
            aa[1][2] = 0.0;
            aa[2][0] = 0.5 * a;
            aa[2][1] = 0.0;
            aa[2][2] = 0.5 * c;

            break;

        case 14:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            alpha = acos(celldm[3]);
            beta = acos(celldm[4]);
            gamma = acos(celldm[5]);
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = b * cos(gamma);
            aa[1][1] = b * sin(gamma);
            aa[1][2] = 0.0;
            aa[2][0] = c * cos(beta);
            aa[2][1] = c * (cos(alpha) - cos(beta) * cos(gamma)) / sin(gamma);
            aa[2][2] = c * sqrt(1.0 + 2.0 * cos(alpha) * cos(beta) * cos(gamma)
                                - cos(alpha) * cos(alpha) - cos(beta) * cos(beta) - cos(gamma) * cos(gamma)) /
                       sin(gamma);

            break;

        default:

            cout << "ERROR: Invalid ibrav." << endl;
            exit(EXIT_FAILURE);
    }

    // Transpose lavec for later use

    double tmp[3][3];

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            tmp[i][j] = aa[i][j];
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            aa[i][j] = tmp[j][i];
        }
    }
}

void recips(double aa[3][3], double bb[3][3])
{
    /*
    Calculate Reciprocal Lattice Vectors

    Here, BB is just the inverse matrix of AA (multiplied by factor 2 Pi)

    BB = 2 Pi AA^{-1},
    = t(b1, b2, b3)

    = (b11 b12 b13)
      (b21 b22 b23)
      (b31 b32 b33),

    b1 = t(b11, b12, b13) etc.
    */

    double det;
    det = aa[0][0] * aa[1][1] * aa[2][2]
          + aa[1][0] * aa[2][1] * aa[0][2]
          + aa[2][0] * aa[0][1] * aa[1][2]
          - aa[0][0] * aa[2][1] * aa[1][2]
          - aa[2][0] * aa[1][1] * aa[0][2]
          - aa[1][0] * aa[0][1] * aa[2][2];

    if (std::abs(det) < eps12) {
        cout << " ERROR: Lattice vector is singular" << endl;
        exit(EXIT_FAILURE);
    }

    double factor = 2.0 * pi / det;

    bb[0][0] = (aa[1][1] * aa[2][2] - aa[1][2] * aa[2][1]) * factor;
    bb[0][1] = (aa[0][2] * aa[2][1] - aa[0][1] * aa[2][2]) * factor;
    bb[0][2] = (aa[0][1] * aa[1][2] - aa[0][2] * aa[1][1]) * factor;

    bb[1][0] = (aa[1][2] * aa[2][0] - aa[1][0] * aa[2][2]) * factor;
    bb[1][1] = (aa[0][0] * aa[2][2] - aa[0][2] * aa[2][0]) * factor;
    bb[1][2] = (aa[0][2] * aa[1][0] - aa[0][0] * aa[1][2]) * factor;

    bb[2][0] = (aa[1][0] * aa[2][1] - aa[1][1] * aa[2][0]) * factor;
    bb[2][1] = (aa[0][1] * aa[2][0] - aa[0][0] * aa[2][1]) * factor;
    bb[2][2] = (aa[0][0] * aa[1][1] - aa[0][1] * aa[1][0]) * factor;
}

string double2string(const double d)
{
    std::string rt;
    std::stringstream ss;

    ss << std::scientific << std::setprecision(15) << d;
    ss >> rt;
    return rt;
}

void get_pairs_of_minimum_distance(const int natmin, const int nat, int **map_p2s,
                                   double **xf, std::vector<DistInfo> **mindist_pairs)
{
    int icell = 0;
    int i, j, k;
    int isize, jsize, ksize;
    int iat;
    double dist_tmp;
    double vec[3];
    std::vector<DistInfo> **distall;
    int nneib = 27;

    double ***xcrd;

    allocate(distall, natmin, nat);
    allocate(xcrd, nneib, nat, 3);

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            xcrd[0][i][j] = xf[i][j];
        }
    }

    for (isize = -1; isize <= 1; ++isize) {
        for (jsize = -1; jsize <= 1; ++jsize) {
            for (ksize = -1; ksize <= 1; ++ksize) {

                if (isize == 0 && jsize == 0 && ksize == 0) continue;

                ++icell;
                for (i = 0; i < nat; ++i) {
                    xcrd[icell][i][0] = xf[i][0] + static_cast<double>(isize);
                    xcrd[icell][i][1] = xf[i][1] + static_cast<double>(jsize);
                    xcrd[icell][i][2] = xf[i][2] + static_cast<double>(ksize);
                }
            }
        }
    }

    for (icell = 0; icell < nneib; ++icell) {
        for (i = 0; i < nat; ++i) {
            rotvec(xcrd[icell][i], xcrd[icell][i], lavec);
        }
    }

    for (i = 0; i < natmin; ++i) {
        iat = map_p2s[0][i];
        for (j = 0; j < nat; ++j) {
            for (icell = 0; icell < nneib; ++icell) {

                dist_tmp = distance(xcrd[0][iat], xcrd[icell][j]);

                for (k = 0; k < 3; ++k) vec[k] = xcrd[icell][j][k] - xcrd[0][iat][k];

                distall[i][j].push_back(DistInfo(icell, dist_tmp, vec));
            }
            std::sort(distall[i][j].begin(), distall[i][j].end());
        }
        /*
                for (j = 0; j < nat; ++j) {
                    for (k = 0; k < distall[i][j].size(); ++k) {
                        std::cout << std::setw(5) << i + 1;
                        std::cout << std::setw(5) << j + 1;
                        std::cout << std::setw(5) << k + 1;
                        std::cout << std::setw(15) << distall[i][j][k].dist << endl;
                    }
                }
        */
    }

    // Construct pairs of minimum distance.

    double dist_min;
    for (i = 0; i < natmin; ++i) {
        for (j = 0; j < nat; ++j) {
            mindist_pairs[i][j].clear();

            dist_min = distall[i][j][0].dist;
            for (std::vector<DistInfo>::const_iterator it = distall[i][j].begin(); it != distall[i][j].end(); ++it) {
                if (std::abs((*it).dist - dist_min) < 1.e-3) {
                    mindist_pairs[i][j].push_back(DistInfo(*it));
                }
            }
        }
    }

    deallocate(distall);
    deallocate(xcrd);
}

double distance(double *x1, double *x2)
{
    double dist;
    dist = std::pow(x1[0] - x2[0], 2) + std::pow(x1[1] - x2[1], 2) + std::pow(x1[2] - x2[2], 2);
    dist = std::sqrt(dist);

    return dist;
}


#include <chrono>
#include <cmath>
#include <exception>
#include <thread>

#include <boost/log/trivial.hpp>
#include <casadi/casadi.hpp>

#include "mimir/algorithm/TestAlgorithm.hpp"
#include "mimir/StateMachineFwd.hpp"
#include "mimir/FkinDds.hpp"

// ReaderLifespan
#include "dds/core/detail/ProprietaryApi.hpp"

namespace mimir
{
  namespace algorithm
  {
    class TestAlgorithm::Impl
    {
    public:
      Impl(
          const YAML::Node& config,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber) :
        inputReader(dds::core::null),
        outputWriter(dds::core::null)
      {

        // Setup input reader
        const auto test_config = config["inputs"]["signal"];
        auto test_age = test_config["max_age_ms"].as<std::int64_t>();

        auto readerQos = subscriber.default_datareader_qos();
        //readerQos << dds::core::policy::Durability::TransientLocal()
        //          << dds::core::policy::Reliability::Reliable()
        readerQos << org::opensplice::core::policy::ReaderLifespan(
                      true,
                      dds::core::Duration::from_millisecs(test_age));

        const auto topicName = test_config["topic"].as<std::string>();
        const auto id = test_config["id"].as<std::string>(); // For filter topic
        auto topic = dds::topic::Topic<fkin::IdVec1d>(
            subscriber.participant(),
            topicName);
        auto filter = dds::topic::Filter("id = %0", {id});

        inputReader = dds::sub::DataReader<fkin::IdVec1d>(
            subscriber,
            dds::topic::ContentFilteredTopic<fkin::IdVec1d>(
                topic,
                topicName + id,
                filter),
            readerQos);

        // Setup output writer
        auto writerQos = publisher.default_datawriter_qos();
        //writerQos << dds::core::policy::Durability::TransientLocal();

        outputWriter = dds::pub::DataWriter<fkin::IdVec1d>(
            publisher,
            dds::topic::Topic<fkin::IdVec1d>(
                publisher.participant(),
                config["outputs"]["response"]["topic"].as<std::string>()),
            writerQos);

        outputId = config["outputs"]["response"]["id"].as<std::string>();

        // Setup model description
        casadi::SX x = casadi::SX::sym("x", 1);     // state
        casadi::SX x_d = casadi::SX::sym("x_d", 1); // set-point
        casadi::SX T = casadi::SX::sym("T", 1);     // time constant
        casadi::SX rhs = -(x-x_d)/T;                // prop-ctrl with time constant T
        casadi::SXDict ode = {{"x", x}, {"p", vertcat(x_d,T)}, {"ode", rhs}};

        integrator = casadi::integrator(
            "F", "cvodes", ode,
            {{"tf", config["time_step_ms"].as<double>()/1000.}});

        p_vec = casadi::DM(std::vector<double>{5.,5.});

      }
      ~Impl() {}
      dds::sub::DataReader<fkin::IdVec1d> inputReader;
      dds::pub::DataWriter<fkin::IdVec1d> outputWriter;
      std::string outputId;
      casadi::Function integrator;
      casadi::DM p_vec;
      casadi::DM x_k;
    };

    TestAlgorithm::TestAlgorithm(
        const YAML::Node& config,
        boost::statechart::fifo_scheduler<> & scheduler,
        boost::statechart::fifo_scheduler<>::processor_handle machine,
        dds::pub::Publisher publisher,
        dds::sub::Subscriber subscriber) :
      m_impl( new TestAlgorithm::Impl(config, publisher, subscriber)),
      m_scheduler(scheduler),
      m_stateMachine(machine),
      m_time_step(std::chrono::milliseconds(config["time_step_ms"].as<std::int32_t>())),
      m_next_step(std::chrono::steady_clock::now()),
      m_config(config)
    {}
    void TestAlgorithm::solve(const std::atomic<bool>& cancel_token)
    {
      try
      {
        BOOST_LOG_TRIVIAL(trace) << "Solving " << name();

        // Publish x_k(t_now) (we do not publish future samples)
        m_impl->outputWriter << fkin::IdVec1d(
            m_impl->outputId,
            fkin::Vector1d(m_impl->x_k.get_nonzeros()[0]));

        // Fetch input, sample and hold
        auto samples = m_impl->inputReader.read();
        // waitset with short timeout, allowing run in "open loop"

        // Set new desired value
        if(samples.length() > 0)
        {
          auto sample = (--samples.end());
          if(sample->info().valid())
          {
            m_impl->p_vec(0,0) = sample->data().vec().x();
            BOOST_LOG_TRIVIAL(trace) << "Got a sample: " << sample->data().vec().x();
          }
        }
        else
        {
          // TODO: In case of measurements: Disable feedback gain
          BOOST_LOG_TRIVIAL(trace) << "No valid sample for " << name() << ", using old";
        }

        if(cancel_token)
        {
          event(new mimir::EvInterrupt());
          return;
        }

        // Integrate
        auto result = m_impl->integrator(
            casadi::DMDict( {{"x0",m_impl->x_k}, {"p", m_impl->p_vec}} ));

        if(cancel_token)
        {
          event(new mimir::EvInterrupt());
          return;
        }

        // Update output (not published yet)
        m_impl->x_k = result["xf"];
        step_time();
        event(new mimir::EvReady());

      }
      catch (...)
      {
        // Need to post event in case of exception so that the state machine
        // knows that the job has finished.
        BOOST_LOG_TRIVIAL(fatal) << name() << " exception thrown";
        event(new mimir::EvError());
        throw;
      }
    }

    void TestAlgorithm::initialize(const std::atomic<bool>&)
    {
      try
      {
        BOOST_LOG_TRIVIAL(debug) << name() << " is initializing";

        // waitset to get initial measurements that meet requirements.
        // set initial state vector..
        m_impl->x_k = casadi::DM(std::vector<double>{0.});
        // set parameters as well?
        m_next_step = std::chrono::steady_clock::now();

        event(new mimir::EvReady());
      }
      catch (...)
      {
        event(new mimir::EvError());
        throw;
      }
    }

    void TestAlgorithm::timer(const std::atomic<bool>& cancel_token)
    {

      // There is no sanity checks in terms of real-timeliness of execution.
      // probably want external time point to wait until.
      typedef std::chrono::milliseconds scm;
      auto now = std::chrono::steady_clock::now();
      auto time_p = m_next_step;
      auto timeleft = std::chrono::duration_cast<scm>(time_p - now).count();
      BOOST_LOG_TRIVIAL(trace) << "Time left: " << timeleft;

      auto fraction = (m_time_step/100 > scm(0) ? m_time_step/100 :
       (m_time_step/50 > scm(0) ? m_time_step/50 :
        (m_time_step/25 > scm(0) ? m_time_step/25 :
         (m_time_step/10 > scm(0) ? m_time_step/10 :
          (m_time_step/2 > scm(0) ?  m_time_step/2  : scm(1))))));

      while(now < time_p)
      {
        auto diff = time_p - now;
        std::this_thread::sleep_for(diff < fraction ? diff : fraction);
        if(cancel_token)
          break;
        now = std::chrono::steady_clock::now();
      }

      if(cancel_token)
      {
        BOOST_LOG_TRIVIAL(trace) << "Timer Canceled";
        event(new mimir::EvInterrupt());
      }
      else
      {
        BOOST_LOG_TRIVIAL(trace) << "Timeout";
        event(new mimir::EvTimeout());
      }
    }

    void TestAlgorithm::event(boost::statechart::event_base * const event)
    {
      // who is responsible for the pointer?
      m_scheduler.queue_event(
          m_stateMachine,
          make_intrusive(event));
    }

    TestAlgorithm::~TestAlgorithm() = default;
  }
}