#ifndef __VEHICLE_VAR_HH__
#define __VEHICLE_VAR_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Vehicle variable definition)
LIBRARY DEPENDENCY:
      ((../src/Vehicle_var.cpp))
PROGRAMMERS:
      (((Chun-Hsu Lai) () () () ))
*******************************************************************************/

#include <armadillo>
#include <vector>
#include "aux.hh"
#include "body.hh"
#include "signal_process.hh"

enum STAGE { STAGE_1 = 0,
             STAGE_2 = 1,
             STAGE_3 = 2,
             FARING_SEP = 3 };
enum THRUST_TYPE { NO_THRUST = 0,
                   INPUT_THRUST = 3 };
enum TVC_TYPE {
    NO_TVC = 0,
    NO_DYNAMIC_TVC,
    SECON_ORDER_TVC,          // TVC Second order dynamics with rate limiting
    ONLINE_SECOND_ORDER_TVC,  // same as 2nd order but with on-line TVC gain
    S2_TVC,
    S3_TVC,
    S1_TVC
};

struct ENG_VAR {
    double ENG_mass;
    double ENG_mass0;
    double ENG_mass1;
    double ENG_isp;

    MATRIX(ENG_I, 3, 3);
    MATRIX(ENG_I_0, 3, 3);
    MATRIX(ENG_I_1, 3, 3);

    VECTOR(ENG_XCG, 3);
    VECTOR(ENG_XCG_0, 3);
    VECTOR(ENG_XCG_1, 3);
};

/* Stage variable */
class STAGE_VAR
{
public:
    STAGE_VAR();
    ~STAGE_VAR(){};
    double isp;            /* *o (--) Rocket engine's specific impluse */
    double fmass0;         /* ** (kg) Init fuel mass */
    double StageMass0;     /* ** (kg) Stage init mass */
    MATRIX(IBBB0, 3, 3);   /* *o (--) Initial vehicle's MOI */
    MATRIX(IBBB1, 3, 3);   /* *o (kg*m2) Final vehicle's MOI */
    VECTOR(XCG_0, 3);      /* *o (m) Initial vehicle's cg */
    VECTOR(XCG_1, 3);      /* *o (m) Final vehicle's cg */
    double fmasse;         /* *o (kg) Expended fuel mass */
    double fmassd;         /* ** (kg/s) Fuel mass expended derivative */
    double fuel_flow_rate; /* *o (kg/s) Fuel flow rate of engine */
    double aexit;          /* *o (m2) Engine nozzle area */
    double timer;          /* ** (s) Stage timer */
};

class Aerodynamics_var
{
public:
    Aerodynamics_var();
    ~Aerodynamics_var(){};
    double refa;  /* *o (m2)     Reference area for aero coefficients - m^2*/
    double refd;  /* *o (m)      Reference length for aero coefficients - m*/
    double cy;    /* *o (--)     Side force coefficient - ND*/
    double cll;   /* *o (--)     Rolling moment coefficient - ND*/
    double clm;   /* *o (--)     Pitching moment coefficient - ND*/
    double cln;   /* *o (--)     Yawing moment coefficient - ND*/
    double cx;    /* *o (--)     X-force coefficient - ND*/
    double cz;    /* *o (--)     Z-force coefficient - ND*/
    double cn;    /* *o (--)     Normal force coefficient */
    double ca;    /* *o (--)     Axial force coefficient */
    double ca_on; /* *o (--)     Axial force coefficient when engine is running */
    double cnq;   /* *o (--)     Normal damping coefficient */
    double cmq;   /* *o (--)     Pitch moment derivative coefficient */
    double ca0;
    double caa;
    double ca0b;
    double cn0;
    double clm0;
    double clmq;
    double cna;
    double clma;

    VECTOR(XCP,
           3); /* *o (m)      Non-dimensional center of pressure (from nose)*/
};

class EarthEnvironment_var
{
public:
    EarthEnvironment_var();
    ~EarthEnvironment_var(){};
    VECTOR(GRAVG, 3);  /* *o (m/s2) Earth gravity acceleration in inertia frame */
    VECTOR(VBAB, 3);   /* *o (m/s)  Vehicle speed wrt air speed in body frame */
    MATRIX(TEI, 3, 3); /* *o (--)  T.M. from ECI to ECEF */
    VECTOR(VAED, 3);

    double vmach;  /* *o (--)  vehicle mach number */
    double pdynmc; /* *o (pa)  Dynamic pressure */
    double dvba;   /* *o (m/s) Vehicle air speed magnitude */
    double gravg;  /* *o (m/s2)  Earth gravity acceleration magnitude */
    double tempc;  /* *o (c)  Atmosphere temperature in Celsius */
    double press;
    double tempk;
    double rho;
};

class DM_var
{
public:
    DM_var();
    ~DM_var(){};
    /* State variables */
    VECTOR(SBIIP, 3); /* *o  (m)      Vehicle position in inertia coord */

    VECTOR(VBIIP, 3); /* *o  (m/s)    Vehicle inertia velocity */

    VECTOR(WBIB, 3); /* *o (r/s)        Augular velocity of vehicle wrt inertia in
                     body coord */

    VECTOR(TBI_Q, 4); /* *o (--)    Transformation Matrix of body coord wrt
                      inertia coord (Quaternion) */

    VECTOR(NEXT_ACC, 3); /* *o (m/s2)   New Inertial acceleration */

    VECTOR(FAPB,
           3); /* *o (N)      Aerodynamic and propulsion forces in body axes */

    VECTOR(FAP, 3); /* *o (N)      Aerodynamic force in body axes */

    VECTOR(FMB,
           3); /* *o (N*m)    Aerodynamic and propulsion moment in body axes */

    VECTOR(FMAB,
           3); /* *o (N*m)    Aerodynamic and propulsion moment in body axes */

    VECTOR(Q_G, 6); /* *o (--)     External force generated by gravity */

    VECTOR(Q_Aero, 6); /* *o (--)  External force generated by aerodynamics */

    VECTOR(rhoC_1, 3); /* *o  (m)  Level arm from reference ponit to CG */

    MATRIX(I1, 3, 3); /* *o (kg*m2)  MOI of vehicle */

    VECTOR(ddrP_1, 3); /* *o  (m/s2)  Vehicle acceleration */

    VECTOR(ddang_1, 3); /* *o  (r/s2)  Vehicle angular acceleration */

    VECTOR(dang_1, 3); /* *o  (r/s)   Vehicle angular rate */

    VECTOR(ddrhoC_1, 3); /* *o  (m/s2)  Centrifugal acceleration and tangential
                          acceleration term */

    VECTOR(p_b1_ga,
           3); /* *o (--)   General dynamics equations 1st DoF to 3rd DoF */

    VECTOR(p_b1_be,
           3); /* *o (--)   General dynamics equations 4th DoF to 6th DoF */

    VECTOR(f, 6); /* *o  (--)  Summation of external force & internal force */

    VECTOR(gamma_b1_q1, 3); /* *o (--)  Vehicle's 1st DoF velocity coefficient */

    VECTOR(gamma_b1_q2, 3); /* *o (--)  Vehicle's 2nd DoF velocity coefficient */

    VECTOR(gamma_b1_q3, 3); /* *o (--)  Vehicle's 3rd DoF velocity coefficient */

    VECTOR(beta_b1_q4,
           3); /* *o (--)  Vehicle's 4th DoF angular velocity coefficient */

    VECTOR(beta_b1_q5,
           3); /* *o (--)  Vehicle's 5th DoF angular velocity coefficient */

    VECTOR(beta_b1_q6,
           3); /* *o (--)  Vehicle's 6th DoF angular velocity coefficient */

    /* Auxiliary variables */
    MATRIX(TBI, 3, 3); /* *o (--)    Transformation Matrix of body coord wrt
                    inertia coord */

    MATRIX(TBID, 3, 3); /* *o (--)    Transformation Matrix of body coord wrt
                     inertia coord derivative */

    VECTOR(TBID_Q, 4); /* *o (--)    Transformation Matrix of body coord wrt
                       inertia coord derivative (Quaternion) */

    MATRIX(TBD, 3, 3); /* *o (--)    Transformation Matrix of body coord wrt
                    geodetic coord */
    VECTOR(TBDQ, 4);   /* *o  (--)    Quaternion from geodetic to body */

    VECTOR(VBAB, 3); /* *o  (m/s)    Air speed in body frame */

    MATRIX(WEII_skew, 3,
           3); /* *o  (r/s)    Earth's angular velocity (skew-sym) */

    VECTOR(SBII, 3); /* *o  (m)      Vehicle position in inertia coord */

    VECTOR(VBII, 3); /* *o  (m/s)    Vehicle inertia velocity */

    VECTOR(ABII, 3); /* *o  (m/s2)   Vehicle inertia acceleration */

    VECTOR(ABIB,
           3); /* *o  (m/s2)   Vehicle inertia acceleration on body coordinate */

    VECTOR(SBEE, 3); /* *o  (m)     Vehicle position in earth coord  */

    VECTOR(VBEE, 3); /* *o  (m/s)     Vehicle speed in earth coord  */

    VECTOR(ABEE, 3); /* *o  (m/s2)   Vehicle acceleration in ECEF */

    VECTOR(SBEE_old, 3); /* *o  (m)     Vehicle position in earth coord  */

    VECTOR(VBEE_old, 3); /* *o  (m/s)     Vehicle speed in earth coord  */

    VECTOR(ABEE_old, 3); /* *o  (m/s2)   Vehicle acceleration in ECEF */

    VECTOR(JBII, 3); /* *o (m/s3)    Vehicle Jerk in ECI*/

    VECTOR(JBEE, 3); /* *o (m/s3)    Vehicle Jerk in ECEF */

    MATRIX(TDI, 3, 3); /* **  (--)     Transformation Matrix of geodetic wrt
                    inertial coordinates */

    MATRIX(
        TGI, 3,
        3); /* **  (--)     Transformation Matrix geocentric wrt inertia coord */

    VECTOR(VBED, 3); /* *o (m/s)   NED velocity */

    VECTOR(FSPB, 3); /* *o  (m/s2)   Specific force in body coord */

    MATRIX(TDE, 3, 3); /* ** (--)  T.M. from ECEF to geodetic */

    VECTOR(VBII_old, 3); /* ** (m/s)  Prior body inertia velocity */

    VECTOR(WEII, 3); /* ** (--)  Earth rate in inertia coordinate */

    VECTOR(WBII, 3); /* *o (r/s)        Vehicle's inertia angular velocity in
                     inertia coord */

    VECTOR(WBEB, 3); /* *o (r/s)        Angular velocity of vehicle wrt earth in
                     body coord */

    VECTOR(WBIBD, 3); /* *o (r/s2)       Angular velocity of vehicle wrt inertia
                      in body coord - derivative */

    MATRIX(TVD, 3, 3); /* **  (--)     Transformation Matrix of geographic
                    velocity wrt geodetic coord */

    VECTOR(SBEE_test, 3); /* *o  (m)     Vehicle position in earth coord  */

    VECTOR(VBEE_test, 3); /* *o  (m/s)     Vehicle speed in earth coord  */

    VECTOR(ABEE_test, 3); /* *o  (m/s2)   Vehicle acceleration in ECEF */

    MATRIX(TLI, 3, 3); /* **  (--)  T.M. from inertia to launch site */

    VECTOR(LT_euler, 3); /* ** (rad)  Launch site coordinate Euler angle */

    VECTOR(TBLQ, 4); /* ** (--) Quaternion from launch site to body */

    VECTOR(ABID, 3); /* ** (--) Vehicle inertia body acceleration in geodetic */

    VECTOR(Fg, 3);

    VECTOR(F_Aero, 3);

    VECTOR(M_Aero, 3);
    /* Generating Outputs */
    double
        ortho_error; /* *o (--)    Direction cosine matrix orthogonality error*/
    double alphax;   /* *o (d)     Angle of attack */
    double betax;    /* *o (d)     Sideslip angle */
    double alppx;    /* *o (d)     Total angle of attack */
    double phipx;    /* *o (d)     Aerodynamic roll angle*/
    double alphaix;  /* *o (d)     Angle of attack, inertia velocity*/
    double betaix;   /* *o (d)     Sideslip angle, inertia velocity*/
    double psibdx;   /* *o (d)     Yaw angle of Vehicle wrt geodetic coord - deg */
    double
        thtbdx;          /* *o (d)     Pitch angle of Vehicle wrt geodetic coord - deg */
    double phibdx;       /* *o (d)     Roll angle of Vehicle wrt geodetic coord - deg */
    double psibd;        /* *o (r)     Yaw angle of Vehicle wrt geodetic coord - rad */
    double thtbd;        /* *o (r)     Pitch angle of Vehicle wrt geodetic coord - rad */
    double phibd;        /* *o (r)     Roll angle of Vehicle wrt geodetic coord - rad */
    double alt;          /* *o  (m)      Vehicle altitude */
    double lonx;         /* *o  (d)      Vehicle longitude */
    double latx;         /* *o  (d)      Vehicle latitude */
    double _aero_loss;   /* *o  (m/s)    Velocity loss caused by aerodynamic drag */
    double gravity_loss; /* *o  (m/s)    Velocity loss caused by gravity */
    // double t;            /* *o (s)       timer */
    double _grndtrck;    /* *o  (m)     [DIAG] Vehicle ground track on earth */
    double _gndtrkmx;    /* *o  (km)    [DIAG] Ground track - km */
    double _gndtrnmx;    /* **  (nm)    [DIAG] Ground track - nm */
    double _ayx;         /* *o  (m/s2)  [DIAG] Achieved side acceleration */
    double _anx;         /* *o  (m/s2)  [DIAG] Achieved normal acceleration */
    double _dbi;         /* *o  (m)     [DIAG] Vehicle distance from center of earth */
    double _dvbi;        /* *o  (m/s)   [DIAG] Vehicle inertia speed */
    double _dvbe;        /* *o  (m/s)   [DIAG] Vehicle geographic speed */
    double _thtvdx;      /* *o  (d)     [DIAG] Vehicle's flight path angle */
    double _psivdx;      /* *o  (d)     [DIAG] Vehicle's heading angle */
    int liftoff;         /* *i  (--)     To check wether the rocket liftoff or
                              not: liftoff = 1, not liftoff = 0 */
    int cadorbin_flag;   /* Orbit calculation status flag */
    double ppx;          /* *o (d/s)        Body roll angular velocity wrt earth in body
                 axes */
    double qqx;          /* *o (d/s)        Body pitch angular velocity wrt earth in body
                 axes */
    double rrx;          /* *o (d/s)        Body yaw angular velocity wrt earth in body
                 axes */
    double control_loss; /* *o (--) Velocity loss due to control effect */

    /* Orbital Logging */
    double _inclination;    /* *o  (deg)   [DIAG] Orbital inclination is the minimun
                        angle between reference plane and the orbital plane or
                        direction of an object in orbit around another object */
    double _eccentricity;   /* *o  (--)    [DIAG] Determines the amount by which its
                           orbit around another body deviates from a perfect
                           circle */
    double _semi_major;     /* *o  (m)     [DIAG] the major axis of an ellipse is its
                         longest diameter */
    double _ha;             /* *o  (m)     [DIAG] Orbital Apogee */
    double _hp;             /* *o  (m)     [DIAG] Orbital Perigee */
    double _lon_anodex;     /* *o  (deg)   [DIAG] The longitude of the ascending node
                         (☊ or Ω) is one of the orbital elements used to specify
                         the orbit of an object in space. It is the angle from a
                         reference direction, called the origin of longitude, to
                         the direction of the ascending node, measured in a
                         reference plane */
    double _arg_perix;      /* *o  (deg)   [DIAG] The argument of periapsis (also
                         called argument of perifocus or argument of pericenter),
                         symbolized as ω, is one of the orbital elements of an
                         orbiting body. Parametrically, ω is the angle from the
                         body's ascending node to its periapsis, measured in the
                         direction of motion */
    double _true_anomx;     /* *o  (deg)   [DIAG] In celestial mechanics, true anomaly
                      is an angular parameter that defines the position of a
                      body moving along a Keplerian orbit. It is the angle
                      between the direction of periapsis and the current
                      position of the body, as seen from the main focus of the
                      ellipse (the point around which the object orbits) */
    double _ref_alt;        /* *o  (m)     [DIAG] */
    double reference_point; /* *o (m)    Multibody dynamics reference point */
    double Roll;
    double Pitch;
    double Yaw;
    unsigned int Interpolation_Extrapolation_flag;
    // double xp; /* *o (m) Reference point  */
    int its;                  /* *o (--) Number of iterations */
    int DOF;                  /* *o (--)  Number of Degree of Freedom */
    int reference_point_flag; /* *o (--)  check if reference point equal to xcg */
    unsigned int Aero_flag;   /* *o (-)  Aerodynamics flag */

    Body *Body_ptr;
};

class Prop_var
{
public:
    Prop_var();
    ~Prop_var(){};

    VECTOR(XCG, 3);      /* *o (m) Current vehicle's cg */
    MATRIX(IBBB, 3, 3);  /* *o (kg*m2) Current cheicle's MOI */
    double vmass;        /* *o (kg) Current vehicle mass */
    double fmassr;       /* *o (kg) Remaining fuel mass */
    double mass_ratio;   /* *o (--) fmasse / fmass0 */
    double payload_mass; /* *o (kg) Payload mass */
    double faring_mass;  /* *o (kg) Vehicle faring mass */
    double thrust;       /* *o (N) Thrust generated by vehicle's engine */
    double delta_v;      /* *o (m/s)  Vehicle delta-V generated by engine */

    /* State */
    enum THRUST_TYPE
        thrust_state; /* *o (--)     Propulsion mode, See THRUST TYPE*/
    enum STAGE stage;
};

class ACT_var
{
public:
    ACT_var();
    ~ACT_var(){};

    VECTOR(Q_RCS, 6);   /* ** (--)  External force generated by RCS thruster */
    double gtvc;        /* *o  (--)    TVC nozzle deflection gain n*/
    double theta_a_cmd; /* *o (r)  Store first actuator command */
    double theta_b_cmd; /* *o (r)  Store second actuator command */
    double theta_c_cmd; /* *o (r)  Store third actuator command */
    double theta_d_cmd; /* *o (r)  Store fourth actuator command */
    enum TVC_TYPE mtvc; /* *o  (--)      see TVC_TYPE */
    VECTOR(Q_TVC, 6);   /* *o (--) External force generated by the rocket engines */
    VECTOR(F_TVC, 3);
    VECTOR(M_TVC, 3);
};

class Sensor_var
{
public:
    Sensor_var();
    ~Sensor_var(){};

    //Accelerometer var
    VECTOR(FSPCB, 3); /* *o  (N/kg)  Computed specific force on body */

    VECTOR(EFSPB, 3);  /* *o   (N/kg)  Error in specific force on body in body
                      coordinate */
    VECTOR(EWALKA, 3); /* *o   (m/s2)  Acceleration random noise */

    VECTOR(EMISA, 3); /* *o   (r)     Acceleration misalignment */

    VECTOR(ESCALA, 3); /* *o   (--)    Acceleration scale factor */

    VECTOR(EBIASA, 3); /* *o   (m/s2)  Acceleration bias */

    VECTOR(ITA1_A, 3); /* *o   (--)  Random noise calculation variable */

    VECTOR(ITA2_A, 3); /* *o   (--)  Random noise calculation variable */

    VECTOR(BETA_A, 3); /* *o   (--)  Random noise calculation variable */

    // Gyro var
    VECTOR(WBICB, 3); /* *o  (r/s)   Computed inertial body rate in body coordinate */

    VECTOR(EWBIB, 3); /* *o  (r/s)   Error in angular vel of body wrt earth */

    VECTOR(EUG, 3); /* *o   (r/s)    Gyro spin axis accel sensitivity */

    VECTOR(EWG, 3); /* *o   (r/s)    Gyro random walk errors */

    VECTOR(EWALKG, 3); /* *o   (r/s0.5) Random walk */

    VECTOR(EUNBG, 3); /* *o   (r)      Gyro cluster misalignment */

    VECTOR(EMISG, 3); /* *o   (r)      Gyro misalignmt */

    VECTOR(ESCALG, 3); /* *o   (--)     Gyro scale factor */

    VECTOR(EBIASG, 3); /* *o   (r/s)    Gyro bias */

    VECTOR(ITA1_G, 3); /* *o   (--)  Coefficient of gyro noise calculation */

    VECTOR(ITA2_G, 3); /* *o   (--)  Coefficient of gyro noise calculation */

    VECTOR(BETA_G, 3); /* *o   (--)  Coefficient of gyro noise calculation */

    //SDT var
    VECTOR(PHI, 3); /* *o (r)       PHI = DELTA_ALPHA + 0.5 * DELTA_BETA */

    VECTOR(DELTA_VEL, 3); /* *o (m/s)     Delta V */

    unsigned int k; /* *o (--)       calculate index */

    VECTOR(WBISB, 3); /* *o  (r/s)    Angular rate of body frame relative inertial
                       frame as described in body frame sensed by gyro */

    VECTOR(WBISB_old,
           3); /* *o  (r/s)    Angular rate of body frame relative inertial frame
                as described in body frame (previous time step) */

    VECTOR(DELTA_ALPHA, 3); /* *o  (r)      Delta theta */

    VECTOR(DELTA_ALPHA_old, 3); /* *o (r)   Delta theta (previous time step) */

    VECTOR(ALPHA, 3); /* *o (r)       Alpha */

    VECTOR(FSPSB, 3); /* *o (m/s2)    Specific force of body frame sensed by
                       accelerometer */

    VECTOR(FSPSB_old, 3); /* *o  (m/s2)    Previous Specific force of body frame
                           sensed by accelerometer */

    VECTOR(cross2_old,
           3); /* *o (--)      temporal store the cross product result */

    VECTOR(VEL, 3); /* *o (m/s)     Delta velocity generated by Accelerometer
                     sensing value */
};
#endif  // __VEHICLE_VAR_HH__


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file fdm2dblackscholesop.cpp
*/

#include <ql/processes/blackscholesprocess.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmesher.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/operators/fdm2dblackscholesop.hpp>
#include <ql/methods/finitedifferences/operators/secondordermixedderivativeop.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace QuantLib {

    Fdm2dBlackScholesOp::Fdm2dBlackScholesOp(
            const ext::shared_ptr<FdmMesher>& mesher,
            const ext::shared_ptr<GeneralizedBlackScholesProcess>& p1,
            const ext::shared_ptr<GeneralizedBlackScholesProcess>& p2,
            Real correlation,
            Time /*maturity*/,
            bool localVol,
            Real illegalLocalVolOverwrite)
    : mesher_(mesher),
      p1_(p1),
      p2_(p2),

      localVol1_((localVol) ? p1->localVolatility().currentLink()
                            : ext::shared_ptr<LocalVolTermStructure>()),
      localVol2_((localVol) ? p2->localVolatility().currentLink()
                            : ext::shared_ptr<LocalVolTermStructure>()),

      x_((localVol) ? Array(Exp(mesher->locations(0))) : Array()),
      y_((localVol) ? Array(Exp(mesher->locations(1))) : Array()),

      opX_(mesher, p1, p1->x0(), localVol, illegalLocalVolOverwrite, 0),
      opY_(mesher, p2, p2->x0(), localVol, illegalLocalVolOverwrite, 1),

      corrMapT_(0, 1, mesher),
      corrMapTemplate_(SecondOrderMixedDerivativeOp(0, 1, mesher)
                      .mult(Array(mesher->layout()->size(), correlation))),

      illegalLocalVolOverwrite_(illegalLocalVolOverwrite) {
    }

    Size Fdm2dBlackScholesOp::size() const {
        return 2;
    }

    void Fdm2dBlackScholesOp::setTime(Time t1, Time t2) {
        opX_.setTime(t1, t2);
        opY_.setTime(t1, t2);

        if (localVol1_ != nullptr) {
            const ext::shared_ptr<FdmLinearOpLayout> layout=mesher_->layout();
            const FdmLinearOpIterator endIter = layout->end();

            Array vol1(layout->size()), vol2(layout->size());
            for (FdmLinearOpIterator iter = layout->begin();
                 iter!=endIter; ++iter) {
                const Size i = iter.index();

                if (illegalLocalVolOverwrite_ < 0.0) {
                    vol1[i] = localVol1_->localVol(0.5*(t1+t2), x_[i], true);
                    vol2[i] = localVol2_->localVol(0.5*(t1+t2), y_[i], true);
                }
                else {
                    try {
                        vol1[i] = localVol1_->localVol(0.5*(t1+t2), x_[i],true);
                    } catch (Error&) {
                        vol1[i] = illegalLocalVolOverwrite_;
                    }
                    try {
                        vol2[i] = localVol2_->localVol(0.5*(t1+t2), y_[i],true);
                    } catch (Error&) {
                        vol2[i] = illegalLocalVolOverwrite_;
                    }

                }
            }
            corrMapT_ = corrMapTemplate_.mult(vol1*vol2);
        } else {
            const Real vol1 = p1_
                    ->blackVolatility()->blackForwardVol(t1, t2, p1_->x0());

            const Real vol2 = p2_
                    ->blackVolatility()->blackForwardVol(t1, t2, p2_->x0());

            corrMapT_ = corrMapTemplate_
                      .mult(Array(mesher_->layout()->size(), vol1*vol2));
        }

        currentForwardRate_ = p1_->riskFreeRate()
                                 ->forwardRate(t1, t2, Continuous).rate();
    }

    Array Fdm2dBlackScholesOp::apply(const Array& x) const {
        return opX_.apply(x) + opY_.apply(x) + apply_mixed(x);
    }

    Array Fdm2dBlackScholesOp::apply_mixed(const Array& x) const {
        return corrMapT_.apply(x) + currentForwardRate_*x;
    }

    Array Fdm2dBlackScholesOp::apply_direction(
                                       Size direction, const Array& x) const {
        if (direction == 0) {
            return opX_.apply(x);
        }
        else if (direction == 1) {
            return opY_.apply(x);
        }
        else {
            QL_FAIL("direction is too large");
        }
    }

    Array Fdm2dBlackScholesOp::solve_splitting(Size direction,
                                               const Array& x, Real s) const {
        if (direction == 0) {
            return opX_.solve_splitting(direction, x, s);
        }
        else if (direction == 1) {
            return opY_.solve_splitting(direction, x, s);
        }
        else
            QL_FAIL("direction is too large");
    }

    Array Fdm2dBlackScholesOp::preconditioner(const Array& r,
                                              Real dt) const {
        return solve_splitting(0, r, dt);
    }

    std::vector<SparseMatrix> Fdm2dBlackScholesOp::toMatrixDecomp() const {
        return {
            opX_.toMatrix(),
            opY_.toMatrix(),
            corrMapT_.toMatrix() +
            currentForwardRate_*boost::numeric::ublas::identity_matrix<Real>(
                    mesher_->layout()->size())
        };
    }

}


#ifndef PYTHONIC_SCIPY_SPECIAL_YVP_HPP
#define PYTHONIC_SCIPY_SPECIAL_YVP_HPP

#include "pythonic/include/scipy/special/yvp.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/bessel_prime.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      double yvp(T0 x, T1 y)
      {
        using namespace boost::math::policies;
        return boost::math::cyl_neumann_prime(
            x, y, make_policy(promote_double<true>()));
      }
    }

#define NUMPY_NARY_FUNC_NAME yvp
#define NUMPY_NARY_FUNC_SYM details::yvp
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/expr.h"
#include "dynet/lstm.h"
#include "dynet/mp.h"
#include "rnnlm.h"
#include <boost/algorithm/string.hpp>

#include <iostream>
#include <fstream>
#include <vector>
/*
TODO:
- The shadow params in the trainers need to be shared.
*/

using namespace std;
using namespace dynet;
using namespace dynet::mp;
using namespace boost::interprocess;

typedef vector<int> Datum;

vector<Datum> ReadData(string filename) {
  vector<Datum> data;
  ifstream fs(filename);
  if (!fs.is_open()) {
    cerr << "ERROR: Unable to open " << filename << endl;
    exit(1);
  }
  string line;
  while (getline(fs, line)) {
    data.push_back(read_sentence(line, d));
  }
  return data;
}

template<class T, class D>
class Learner : public ILearner<D, dynet::real> {
public:
  explicit Learner(RNNLanguageModel<T>& rnnlm, unsigned data_size) : rnnlm(rnnlm) {}
  ~Learner() {}

  dynet::real LearnFromDatum(const D& datum, bool learn) {
    ComputationGraph cg;
    Expression loss_expr = rnnlm.BuildLMGraph(datum, cg);
    dynet::real loss = as_scalar(cg.forward(loss_expr));
    if (learn) {
      cg.backward(loss_expr);
    }
    return loss;
  }

  void SaveModel() {}

private:
  RNNLanguageModel<T>& rnnlm;
};

int main(int argc, char** argv) {
  if (argc < 4) {
    cerr << "Usage: " << argv[0] << " cores corpus.txt dev.txt [iterations]" << endl;
    return 1;
  }
  srand(time(NULL));
  unsigned num_children = atoi(argv[1]);
  assert (num_children <= 64);
  vector<Datum> data = ReadData(argv[2]);
  vector<Datum> dev_data = ReadData(argv[3]);
  unsigned num_iterations = (argc >= 5) ? atoi(argv[4]) : UINT_MAX;
  unsigned dev_frequency = 5000;
  unsigned report_frequency = 10;

  dynet::initialize(argc, argv, true);

  Model model;
  SimpleSGDTrainer sgd(model, 0.2);
  //AdagradTrainer sgd(model, 0.0);
  //AdamTrainer sgd(model, 0.0);

  RNNLanguageModel<LSTMBuilder> rnnlm(model);

  Learner<LSTMBuilder, Datum> learner(rnnlm, data.size());
  run_multi_process<Datum>(num_children, &learner, &sgd, data, dev_data, num_iterations, dev_frequency, report_frequency);
}


#include <OpenEXR/ImathVec.h>
#include <actions/io.h>
#include <possumwood_sdk/datatypes/filename.h>
#include <possumwood_sdk/node_implementation.h>

#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>
#include <cctype>
#include <fstream>
#include <iostream>

#include "datatypes/attributes.h"
#include "datatypes/skeleton.h"
#include "datatypes/transform.h"
#include "tokenizer.h"

using std::cout;
using std::endl;

namespace {

class AsfTokenizer : public anim::Tokenizer {
  private:
	State start, keyword, comment;

  public:
	AsfTokenizer(std::istream& in) : Tokenizer(in), start(this), keyword(this), comment(this) {
		// start parsing in "start" state
		start.setActive();

		start = [this](char c) {
			// don't read any space-like characters
			if(std::isspace(c))
				reject();

			// comments start with #
			else if(c == '#')
				comment.setActive();

			// brackets are handled separately (no need for a state)
			else if(c == '(' || c == ')') {
				accept(c);
				emit();
			}

			// everything else is a "keyword"
			else
				keyword.setActive();
		};

		comment = [this](char c) {
			// endline character ends comment
			if(c == '\n')
				start.setActive();

			// reject everything
			reject();
		};

		keyword = [this](char c) {
			// space or bracket characters end a keyword
			if(std::isspace(c) || c == '(' || c == ')') {
				emit();
				start.setActive();
			}
			// otherwise just parse another character of the keyword
			else
				accept(c);
		};

		// read the first keyword into current
		next();
	}
};

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

struct Joint {
	std::string name;
	Imath::V3f position;
	anim::Transform axis;
	int parent = -1;
	anim::Attributes attrs;
};

void readUnits(anim::Tokenizer& tokenizer) {
	tokenizer.next();

	// for now just check that the file is ok, but don't try to do
	// anything with the unit values (this might change)
	while(true) {
		if(tokenizer.current().value == "mass")
			tokenizer.next();
		else if(tokenizer.current().value == "length")
			tokenizer.next();
		else if(tokenizer.current().value == "angle")
			tokenizer.next();
		else
			break;

		// read next token
		tokenizer.next();
	}
}

void readDocumentation(anim::Tokenizer& tokenizer) {
	tokenizer.next();

	// skip until next keyword
	while(!tokenizer.current().value.empty() && tokenizer.current().value[0] != ':')
		tokenizer.next();
}

Imath::V3f readVec3(anim::Tokenizer& tokenizer) {
	// has to be written on separate lines to prevent the compiler from reordering the next() calls
	const float x = boost::lexical_cast<float>(tokenizer.next().value);
	const float y = boost::lexical_cast<float>(tokenizer.next().value);
	const float z = boost::lexical_cast<float>(tokenizer.next().value);

	return Imath::V3f(x, y, z);
}

// makes an XYZ rotation transform
anim::Transform makeAxisTransform(const Imath::V3f& v) {
	Imath::Quatf qx;
	qx.setAxisAngle(Imath::V3f(1, 0, 0), v.x / 180.0f * M_PI);

	Imath::Quatf qy;
	qy.setAxisAngle(Imath::V3f(0, 1, 0), v.y / 180.0f * M_PI);

	Imath::Quatf qz;
	qz.setAxisAngle(Imath::V3f(0, 0, 1), v.z / 180.0f * M_PI);

	return anim::Transform(qz * qy * qx);
}

void readRoot(anim::Tokenizer& tokenizer, std::vector<Joint>& bones) {
	if(!bones.empty())
		throw std::runtime_error(":root tag is not placed before bonedata tag");

	// for now, just read whatever is required and skip it
	tokenizer.next();

	// the resulting joint value
	Joint root;
	root.name = "root";

	// initialise the DOF field in joint attributes (might end up empty)
	std::vector<std::string>& dofs = root.attrs["dof"].as<std::vector<std::string>>();

	while(!tokenizer.current().value.empty() && tokenizer.current().value[0] != ':') {
		if(tokenizer.current().value == "order") {
			static const boost::regex value("(T|R)(X|Y|Z)");
			while(boost::regex_match(tokenizer.next().value, value)) {
				std::string dof = tokenizer.current().value;
				boost::algorithm::to_lower(dof);

				dofs.push_back(dof);
			}
		}

		else if(tokenizer.current().value == "axis") {
			if(tokenizer.next().value != "XYZ")
				throw("unknown axis value in :root - " + tokenizer.current().value);
			tokenizer.next();
		}

		else if(tokenizer.current().value == "position") {
			root.position = readVec3(tokenizer);
			tokenizer.next();
		}

		else if(tokenizer.current().value == "orientation") {
			root.axis = readVec3(tokenizer);
			tokenizer.next();
		}

		else
			throw std::runtime_error("unknown value in :root section - " + tokenizer.current().value);
	}

	// assuming we've read the root bone data - make the bone
	//   ignoring orientation for now
	bones.push_back(root);
}

std::pair<Joint, unsigned> readJoint(anim::Tokenizer& tokenizer) {
	// the result - Joint and its ID
	std::pair<Joint, unsigned> result;
	// initialise the DOF field in joint attributes (might end up empty)
	std::vector<std::string>& dofs = result.first.attrs["dof"].as<std::vector<std::string>>();

	// skip the begin token
	tokenizer.next();

	while(true) {
		if(tokenizer.current().value == "end")
			break;

		else if(tokenizer.current().value == "id") {
			result.second = boost::lexical_cast<unsigned>(tokenizer.next().value);
			tokenizer.next();
		}

		else if(tokenizer.current().value == "name") {
			result.first.name = tokenizer.next().value;
			tokenizer.next();
		}

		else if(tokenizer.current().value == "direction") {
			result.first.position = readVec3(tokenizer);
			tokenizer.next();
		}

		else if(tokenizer.current().value == "length") {
			result.first.position *= boost::lexical_cast<float>(tokenizer.next().value);
			tokenizer.next();
		}

		else if(tokenizer.current().value == "axis") {
			result.first.axis = makeAxisTransform(readVec3(tokenizer));
			if(tokenizer.next().value != "XYZ")
				throw std::runtime_error("invalid axis rotation order " + tokenizer.next().value);
			tokenizer.next();
		}

		else if(tokenizer.current().value == "dof") {
			static const boost::regex value("r(x|y|z)");
			while(boost::regex_match(tokenizer.next().value, value))
				dofs.push_back(tokenizer.current().value);
		}

		else if(tokenizer.current().value == "limits") {
			tokenizer.next();

			// read all expressions in brackets
			while(tokenizer.current().value == "(") {
				tokenizer.next();

				// two numbers
				tokenizer.next();
				tokenizer.next();

				// closing bracked
				tokenizer.next();
			}
		}

		else
			throw std::runtime_error("unknown value in joint section - " + tokenizer.current().value);
	}

	// skip the end token
	tokenizer.next();

	return result;
}

void readBoneData(anim::Tokenizer& tokenizer, std::vector<Joint>& bones) {
	tokenizer.next();

	while(tokenizer.current().value == "begin") {
		std::pair<Joint, unsigned> j = readJoint(tokenizer);
		if(j.second != bones.size())
			throw std::runtime_error("invalid index for bone " + j.first.name);
		bones.push_back(j.first);
	}
}

unsigned findIndex(const std::vector<Joint>& bones, const std::string& name) {
	int result = -1;
	for(unsigned a = 0; a < bones.size(); ++a)
		if(bones[a].name == name)
			result = a;

	if(result < 0)
		throw std::runtime_error(":hierarchy refers to bone " + name + " which was not defined in :bonedata section");

	return result;
}

void readHierarchy(anim::Tokenizer& tokenizer, std::vector<Joint>& bones) {
	if(tokenizer.next().value != "begin")
		throw std::runtime_error(":hierarchy section does not have begin tag");
	tokenizer.next();

	while(tokenizer.current().value != "end") {
		const unsigned line = tokenizer.current().line;

		const unsigned parent = findIndex(bones, tokenizer.current().value);
		tokenizer.next();

		while(tokenizer.current().line == line) {
			bones[findIndex(bones, tokenizer.current().value)].parent = parent;
			tokenizer.next();
		}
	}

	tokenizer.next();
}

void readKeyword(anim::Tokenizer& tokenizer, std::vector<Joint>& bones) {
	// get next token
	auto token = tokenizer.current();

	// check the version
	if(token.value == ":version") {
		if(tokenizer.next().value != "1.10")
			throw std::runtime_error("only version 1.10 is supported (" + tokenizer.next().value + " found)");
		tokenizer.next();
	}

	// ignore the name
	else if(token.value == ":name") {
		tokenizer.next();
		tokenizer.next();
	}

	// reading the units
	else if(token.value == ":units")
		readUnits(tokenizer);

	// reading the documentation
	else if(token.value == ":documentation")
		readDocumentation(tokenizer);

	else if(token.value == ":root")
		readRoot(tokenizer, bones);

	else if(token.value == ":bonedata")
		readBoneData(tokenizer, bones);

	else if(token.value == ":hierarchy")
		readHierarchy(tokenizer, bones);

	else
		throw std::runtime_error("unknown token '" + token.value + "' on line " +
		                         boost::lexical_cast<std::string>(token.line));
}
}  // namespace

std::unique_ptr<anim::Skeleton> doLoad(const boost::filesystem::path& filename) {
	// open the file
	std::ifstream in(filename.string());
	AsfTokenizer tokenizer(in);

	// read the joints from the input file
	std::vector<Joint> joints;
	while(!tokenizer.eof())
		readKeyword(tokenizer, joints);

	// and, finally, transfer the information from joints to a Skeleton instance
	std::unique_ptr<anim::Skeleton> result(new anim::Skeleton());
	result->attributes()["type"] = "asf";

	for(const auto& j : joints) {
		std::size_t index;

		// root is simple
		if(j.parent < 0) {
			result->addRoot(j.name, j.position);
			index = 0;
		}

		// non-roots require a search
		else {
			int parentJoint = -1;
			for(unsigned ji = 0; ji < result->size(); ++ji)
				if((*result)[ji].name() == joints[j.parent].name)
					parentJoint = ji;
			assert(parentJoint >= 0);

			// both the position and rotation are at this stage in *world space*
			index = result->addChild((*result)[parentJoint],
			                         anim::Transform(j.axis.rotation, joints[j.parent].position), j.name);
		}

		(*result)[index].attributes() = j.attrs;
	}

	// change the positions to be in world space
	for(auto& j : *result)
		if(j.hasParent())
			j.tr().translation += j.parent().tr().translation;

	// and convert to local space
	for(int a = result->size() - 1; a > 0; --a)
		(*result)[a].tr() = (*result)[a].parent().tr().inverse() * (*result)[a].tr();

	return result;
}

//////////

dependency_graph::InAttr<possumwood::Filename> a_filename;
dependency_graph::OutAttr<anim::Skeleton> a_skel;

dependency_graph::State compute(dependency_graph::Values& data) {
	dependency_graph::State out;

	const possumwood::Filename filename = data.get(a_filename);

	if(!filename.filename().empty() && boost::filesystem::exists(filename.filename()))
		data.set(a_skel, *doLoad(filename.filename()));
	else {
		data.set(a_skel, anim::Skeleton());
		out.addError("Cannot load filename " + filename.filename().string());
	}

	return out;
}

void init(possumwood::Metadata& meta) {
	meta.addAttribute(a_filename, "filename",
	                  possumwood::Filename({
	                      "ASF files (*.asf)",
	                  }));
	meta.addAttribute(a_skel, "skeleton", anim::Skeleton(), possumwood::AttrFlags::kVertical);

	meta.addInfluence(a_filename, a_skel);

	meta.setCompute(compute);
}

possumwood::NodeImplementation s_impl("anim/loaders/asf", init);


//
// Created by june on 19. 12. 5..
//

// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>

#include <urdf/model.h>

// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/chainjnttojacsolver.hpp>        // jacobian
#include <kdl/chainfksolverpos_recursive.hpp> // forward kinematics
#include <kdl/chainfksolvervel_recursive.hpp> // forward kinematics

#include <boost/scoped_ptr.hpp>
#include <boost/lexical_cast.hpp>

#include <cmath>
#include <Controller.h>

#define _USE_MATH_DEFINES

#include "SerialRobot.h"
#include "Trajectory.h"

#define R2D 180/M_PI
#define D2R M_PI/180
#define num_taskspace 6
#define SaveDataMax 97

namespace  hyuspa_controller
{
    class ComputedTorque_Control_CLIK : public controller_interface::Controller<hardware_interface::EffortJointInterface>
    {
    public:
        bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
        {
            // ********* 1. Get joint name / gain from the parameter server *********
            // 1.0 Control objective & Inverse Kinematics mode
            ROS_INFO("#######Initialize start##########");
            if (!n.getParam("ctr_obj", ctr_obj_))
            {
                ROS_ERROR("Could not find control objective");
                return false;
            }

            if (!n.getParam("ik_mode", ik_mode_))
            {
                ROS_ERROR("Could not find control objective");
                return false;
            }

            // 1.1 Joint Name
            if (!n.getParam("joints", joint_names_))
            {
                ROS_ERROR("Could not find joint name");
                return false;
            }
            n_joints_ = joint_names_.size();
            ROS_INFO("#########joint info is obtained########");
            if (n_joints_ == 0)
            {
                ROS_ERROR("List of joint names is empty.");
                return false;
            }
            else
            {
                ROS_INFO("Found %d joint names", n_joints_);
                for (int i = 0; i < n_joints_; i++)
                {
                    ROS_INFO("%s", joint_names_[i].c_str());
                }
            }

            // 1.2 Gain
            // 1.2.1 Joint Controller
            Kp_.resize(n_joints_);
            Kd_.resize(n_joints_);
            Ki_.resize(n_joints_);

            std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_);

            for (size_t i = 0; i < n_joints_; i++)
            {
                std::string si = std::to_string(i + 1);
                if (n.getParam("/hyuspa/computedtorque_control_clik/gains/Arm_Joint_" + si + "/pid/p", Kp[i]))
                {
                    Kp_(i) = Kp[i];
                }
                else
                {
                    std::cout << "/hyuspa/computedtorque_control_clik/gains/Arm_Joint_" + si + "/pid/p" << std::endl;
                    ROS_ERROR("Cannot find pid/p gain");
                    return false;
                }

                if (n.getParam("/hyuspa/computedtorque_control_clik/gains/Arm_Joint_" + si + "/pid/i", Ki[i]))
                {
                    Ki_(i) = Ki[i];
                }
                else
                {
                    ROS_ERROR("Cannot find pid/i gain");
                    return false;
                }

                if (n.getParam("/hyuspa/computedtorque_control_clik/gains/Arm_Joint_" + si + "/pid/d", Kd[i]))
                {
                    Kd_(i) = Kd[i];
                }
                else
                {
                    ROS_ERROR("Cannot find pid/d gain");
                    return false;
                }
            }

            // 1.2.2 Closed-loop Inverse Kinematics Controller
            if (ctr_obj_ == 1)
            {
                if (!n.getParam("/hyuspa/computedtorque_control_clik/clik_gain/K_regulation", K_regulation_))
                {
                    ROS_ERROR("Cannot find clik regulation gain");
                    return false;
                }
            }

            else if (ctr_obj_ == 2)
            {
                if (!n.getParam("/hyuspa/computedtorque_control_clik/clik_gain/K_tracking", K_tracking_))
                {
                    ROS_ERROR("Cannot find clik tracking gain");
                    return false;
                }
            }

            // 2. ********* urdf *********
            urdf::Model urdf;
            if (!urdf.initParam("robot_description"))
            {
                ROS_ERROR("Failed to parse urdf file");
                return false;
            }
            else
            {
                ROS_INFO("Found robot_description");
            }

            // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
            for (int i = 0; i < n_joints_; i++)
            {
                try
                {
                    joints_.push_back(hw->getHandle(joint_names_[i]));
                }
                catch (const hardware_interface::HardwareInterfaceException &e)
                {
                    ROS_ERROR_STREAM("Exception thrown: " << e.what());
                    return false;
                }

                urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
                if (!joint_urdf)
                {
                    ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                    return false;
                }
                joint_urdfs_.push_back(joint_urdf);
            }

            // 4. ********* KDL *********
            // 4.1 kdl parser
            if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
            {
                ROS_ERROR("Failed to construct kdl tree");
                return false;
            }
            else
            {
                ROS_INFO("Constructed kdl tree");
            }

            // 4.2 kdl chain
            std::string root_name, tip_name1;
            if (!n.getParam("root_link", root_name))
            {
                ROS_ERROR("Could not find root link name");
                return false;
            }
            if (!n.getParam("tip_link1", tip_name1))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }
            if (!kdl_tree_.getChain(root_name, tip_name1, kdl_chain_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO("Got kdl chain");
            }
            gravity_ = KDL::Vector::Zero();
            gravity_(2) = -9.81; // 0: x-axis 1: y-axis 2: z-axis

            M_.resize(kdl_chain_.getNrOfJoints());
            C_.resize(kdl_chain_.getNrOfJoints());
            G_.resize(kdl_chain_.getNrOfJoints());

            id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_));

            jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
            fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));

            // ********* 5. 각종 변수 초기화 *********

            // 5.1 KDL Vector 초기화 (사이즈 정의 및 값 0)
            x_cmd_.data = Eigen::VectorXd::Zero(num_taskspace);

            qd_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

            q_.data = Eigen::VectorXd::Zero(n_joints_);
            qdot_.data = Eigen::VectorXd::Zero(n_joints_);

            q_chain[0].data = Eigen::VectorXd::Zero(kdl_chain_.getNrOfJoints());

            J_.resize(kdl_chain_.getNrOfJoints());

            TrajFlag_j.resize(n_joints_);
            TrajFlag_j.setZero();
            TrajFlag_t.resize(n_joints_);
            TrajFlag_t.setZero();
            TrajFlag_td.resize(num_taskspace);
            TrajFlag_td.setZero();

            // ********* 6. ROS 명령어 *********
            // 6.1 publisher
            pub_qd_ = n.advertise<std_msgs::Float64MultiArray>("qd", 1000);
            pub_q_ = n.advertise<std_msgs::Float64MultiArray>("q", 1000);
            pub_e_ = n.advertise<std_msgs::Float64MultiArray>("e", 1000);

            pub_xd_ = n.advertise<std_msgs::Float64MultiArray>("xd", 1000);
            pub_x_ = n.advertise<std_msgs::Float64MultiArray>("x", 1000);
            pub_ex_ = n.advertise<std_msgs::Float64MultiArray>("ex", 1000);

            pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000);

            // 6.2 subsriber
            sub_x_cmd_ = n.subscribe<std_msgs::Float64MultiArray>("command",
                    1, &ComputedTorque_Control_CLIK::commandCB, this);
            event = 0; // subscribe 받기 전: 0
            // subscribe 받은 후: 1

            return true;
        }

        void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
        {
            if (msg->data.size() != num_taskspace)
            {
                ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match DOF of Task Space (" << 2 << ")! Not executing!");
                return;
            }

            for (int i = 0; i < num_taskspace; i++)
            {
                x_cmd_(i) = msg->data[i];
            }

            event = 1;  // subscribe 받기 전: 0
                        // subscribe 받은 후: 1
        }

        void starting(const ros::Time &time) override
        {
            t = 0.0;
            ROS_INFO("Starting Computed Torque Controller with Closed-Loop Inverse Kinematics");
            //xd={0.048723,-0.30684,1.23653};
            cManipulator = new robot;
            Control = new HYUControl::Controller(cManipulator,n_joints_);
            traj5th_joint = new hyuCtrl::Trajectory();
            traj5th_task = new hyuCtrl::Trajectory();
            qd.setZero();
            qd_dot.setZero();
            qd_old.setZero();
            q_flag=0;
            cManipulator->robot_update_R();
            for(int i=0; i<n_joints_; i++)
            {
                Control->SetPIDGain(Kp_(i), Kd_(i), Ki_(i), i);
            }

            //ROS_INFO("Starting is done");
        }

        void update(const ros::Time &time, const ros::Duration &period) override
        {
            // ********* 0. Get states from gazebo *********
            // 0.1 sampling time
            double dt = period.toSec();
            t = t + 0.001;

            // 0.2 joint state
            for (int i = 0; i < n_joints_; i++)
            {
                q_(i) = joints_[i].getPosition();
                qdot_(i) = joints_[i].getVelocity();
                //torque_(i) = joints_[i].getEffort();
            }

            //Implement the control code
            Eigen::Map<VectorXd>(q, n_joints_) = q_.data;
            Eigen::Map<VectorXd>(qdot, n_joints_) = qdot_.data;

            //ROS_INFO("Controller start");
            cManipulator->pKin->Unflag_isInfoupdate();
            //ROS_INFO("PKIN info");
            cManipulator->pKin->HTransMatrix(q);
            //ROS_INFO("PKIN HTransMatrix");
            rpy = cManipulator->pKin->GetEulerAngle();
            quat = cManipulator->pKin->GetQuaternion();
            //cManipulator->pDyn->Prepare_Dynamics(q,qdot);////////////////////////
            //ROS_INFO("PDyn Prepare");

            b_jaco=cManipulator->pKin->BodyJacobian();
            s_jaco=cManipulator->pKin->SpaceJacobian();
            l_jaco=cManipulator->pKin->LinearJacobian();
            l_jaco_dot=cManipulator->pKin->Jacobian_l_dot();
            a_jaco=cManipulator->pKin->AnalyticJacobian();
            DPI_jaco=cManipulator->pKin->DPI(l_jaco);
            jnt_to_jac_solver_->JntToJac(q_, J_);
            fk_pos_solver_->JntToCart(q_, x_);

            //Control->InvDynController(q, qdot, dq, dqdot, dqddot, torque, dt);
            //torque[0]=-5.0;torque[1]=3.0;torque[2]=0.0;torque[3]=2.0;torque[4]=0.0;

            xdot=l_jaco*qdot_.data;
            x=cManipulator->pKin->ForwardKinematics();

            xdota=a_jaco*qdot_.data;
            ROT=cManipulator->pKin->Rot();
            xa.block<3,1>(0,0)=cManipulator->pKin->so3ToVec(ROT);
            xa.block<3,1>(3,0)=x;

            id_solver_->JntToMass(q_, M_);
            id_solver_->JntToCoriolis(q_, qdot_, C_);
            id_solver_->JntToGravity(q_, G_); // output은 머지? , id_solver는 어디에서?
            /////////////Trajectory for Joint Space//////////////
 /*           if(Motion==1 &&TrajFlag_j(0)==0)
            {
                traj_q[0]=0.01; traj_q[1]=-1.57; traj_q[2]= 0.01; traj_q[3]=1.57; traj_q[4]=0.01;
                Motion++;
                for(int i=0;i<n_joints_;i++)
                {
                    TrajFlag_j(i)=1;
                }
            }
            else if(Motion==2 &&TrajFlag_j(0)==0)
            {
                traj_q[0]=0.732177; traj_q[1]=-1.231181; traj_q[2]= -1.284267; traj_q[3] = 1.05148; traj_q[4] = -0.407245;
                Motion++;
                for(int i=0;i<n_joints_;i++)
                {
                    TrajFlag_j(i)=1;
                }
            }
            else if(Motion==3 &&TrajFlag_j(0)==0)
            {
                traj_q[0]=0.741427; traj_q[1]=-1.961923; traj_q[2]= -1.348277; traj_q[3] = 0.493244; traj_q[4] = -0.407213;
                Motion=1;
                for(int i=0;i<n_joints_;i++)
                {
                    TrajFlag_j(i)=1;
                }
            }

            for (int i = 0; i < n_joints_; i++) {

                if (TrajFlag_j(i)==2)
                {
                    traj5th_joint->Polynomial5th(i, t, &TrajFlag_j(i), res_tra);
                    qd[i] = res_tra[0];
                    qd_dot[i] = res_tra[1];
                    qd_ddot[i]=res_tra[2];
                }
                else if(TrajFlag_j(i)==1) {
                    traj5th_joint->SetPolynomial5th(i, q[i], traj_q(i), t, 2.0, res_tra);
                    qd[i] = res_tra[0];
                    qd_dot[i] = res_tra[1];
                    qd_ddot[i]=res_tra[2];
                    TrajFlag_j(i)=2;
                }

            }
            */
            ////////////////////////

           if(t<=5)
           {
               /////////////Trajectory for Joint Space//////////////
               if(TrajFlag_j[0]==0)
               {
                   traj_q[0]=0.732177; traj_q[1]=-1.431181; traj_q[2]= -1.084267; traj_q[3]=1.05148; traj_q[4]=-0.407245;
                   //traj_q[0]=0.0; traj_q[1]=-1.57; traj_q[2]= 0.0; traj_q[3]=1.57; traj_q[4]=0.0;
                   for(int i=0;i<n_joints_;i++)
                   {
                       TrajFlag_j(i)=1;
                   }
               }

                for (int i = 0; i < n_joints_; i++) {

                    if (TrajFlag_j(i)==2)
                    {
                        traj5th_joint->Polynomial5th(i, t, &TrajFlag_j(i), res_tra);
                        qd[i] = res_tra[0];
                        qd_dot[i] = res_tra[1];
                        qd_ddot[i]=res_tra[2];
                        if(TrajFlag_j[i]==0)
                        {
                            TrajFlag_j[i] = 3;
                        }
                    }
                    else if(TrajFlag_j(i)==1) {
                        traj5th_joint->SetPolynomial5th(i, q[i], traj_q(i), t, 2.0, res_tra);
                        qd[i] = res_tra[0];
                        qd_dot[i] = res_tra[1];
                        qd_ddot[i]=res_tra[2];
                        TrajFlag_j(i)=2;
                    }

                }

               ////////////////////////
            }
            else
            {
                traj_flag=1;
                /////////////Trajectory for Task Space//////////////
                if(Motion==1 &&TrajFlag_t(0)==0)
                {
                    traj_x[0]=0.308723; traj_x[1]=-0.30684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==2 &&TrajFlag_t(0)==0)
                {
                    traj_x[0]=0.308723; traj_x[1]=-0.20684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==3 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.148723; traj_x[1]=-0.30684; traj_x[2]= 1.23653;
                    traj_x[0]=0.308723; traj_x[1]=-0.10684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==4 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.148723; traj_x[1]=-0.30684; traj_x[2]= 1.33653;
                    traj_x[0]=0.308723; traj_x[1]=-0.030684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==5 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.108723; traj_x[1]=-0.70684; traj_x[2]= 1.43653;
                    traj_x[0]=0.308723; traj_x[1]=-0.20684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==6 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.508723; traj_x[1]=-0.60684; traj_x[2]= 1.567653;
                    traj_x[0]=0.308723; traj_x[1]=-0.40684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==7 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.508723; traj_x[1]=-0.60684; traj_x[2]= 1.567653;
                    traj_x[0]=0.308723; traj_x[1]=-0.40684; traj_x[2]= 1.33653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==8 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.508723; traj_x[1]=-0.60684; traj_x[2]= 1.567653;
                    traj_x[0]=0.308723; traj_x[1]=-0.40684; traj_x[2]= 1.43653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==9 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.508723; traj_x[1]=-0.60684; traj_x[2]= 1.567653;
                    traj_x[0]=0.308723; traj_x[1]=-0.40684; traj_x[2]= 1.23653;
                    Motion++;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }
                else if(Motion==10 &&TrajFlag_t(0)==0)
                {
                    //traj_x[0]=0.508723; traj_x[1]=-0.60684; traj_x[2]= 1.567653;
                    traj_x[0]=0.308723; traj_x[1]=-0.20684; traj_x[2]= 1.43653;
                    Motion=1;
                    for(int i=0;i<3;i++)
                    {
                        TrajFlag_t(i)=1;
                    }
                }

                for (int i = 0; i < 3; i++) {

                    if (TrajFlag_t(i)==2)
                    {
                        traj5th_task->Polynomial5th(i, t, &TrajFlag_t(i), res_tra);
                        xd[i] = res_tra[0];
                        xddot[i]=res_tra[1];
                        xdddot[i]=res_tra[2];
                    }
                    else {
                        traj5th_task->SetPolynomial5th(i, x[i], traj_x(i), t, 2.0, res_tra);
                        xd[i] = res_tra[0];
                        xddot[i]=res_tra[1];
                        xdddot[i]=res_tra[2];
                        TrajFlag_t(i)=2;
                    }

                }

                /////////////Trajectory for Task Space//////////////
/*              if(Motion==1 &&TrajFlag_td(0)==0)
                {
                    Matrix3d Rd;
                    Rd<<0.650,-0.43,-0.6268,0.735,0.144,0.6625,0.1941,-0.8918,0.4083;
                    ROTD=cManipulator->pKin->MatrixLog3(Rd);
                    traj_xd[3]=0.2805; traj_xd[4]=-0.2179; traj_xd[5]= 1.31607;
                    traj_xd.block<3,1>(0,0)=cManipulator->pKin->so3ToVec(ROTD);
                    Motion++;
                    for(int i=0;i<6;i++)
                    {
                        TrajFlag_td(i)=1;
                    }
                }
               if(Motion==2 &&TrajFlag_td(0)==0)
               {
                   Matrix3d Rd;
                   Rd<<0.75176,-0.446541,-0.485239,0.444412,-0.200569,0.873081,-0.48719,-0.871994,0.0476685;
                   ROTD=cManipulator->pKin->MatrixLog3(Rd);
                   traj_xd[3]=0.307105; traj_xd[4]=-0.412607; traj_xd[5]= 1.32142;
                   traj_xd.block<3,1>(0,0)=cManipulator->pKin->so3ToVec(ROTD);
                   Motion=1;
                   for(int i=0;i<6;i++)
                   {
                       TrajFlag_td(i)=1;
                   }
               }

                for (int i = 0; i < 6; i++) {

                    if (TrajFlag_td(i)==2)
                    {
                        traj5th_task->Polynomial5th(i, t, &TrajFlag_td(i), res_trad);
                        xda[i] = res_trad[0];
                    }
                    else if(TrajFlag_td(i)==1){
                        traj5th_task->SetPolynomial5th(i, xa[i], traj_xd(i), t, 2.0, res_trad);
                        xda[i] = res_trad[0];
                        TrajFlag_td(i)=2;
                    }

                }
*/
                ////////////////////////
                // CLIK Algorithm
                /////general CLIK
/*                qd_dot=DPI_jaco*(xddot + K_regulation_*(xd-x));
                //qd_dot= DPI_jaco*(K_regulation_*(xda-xa));
                qd = qd_old + qd_dot*dt;

                if(q_flag==0)
                    qd_old=Map<VectorXd>(q,5);
                else
                {
                    qd_old=qd;
                    q_flag=1;
                }
*/
                ///////////////////
                ///// qd CLIK
 /*               if(q_flag==0)
                {
                    qd_dot_old = Map<VectorXd>(qdot, 5);
                    qd_old = Map<VectorXd>(q, 5);

                    q_flag=1;
                }
                qd_ddot=DPI_jaco*(xdddot - l_jaco_dot*qdot_.data+250000*(xd-x)+1000*(xddot-xdot));
                qd_dot = qd_dot_old + qd_ddot*dt;
                qd = qd_old + qd_dot*dt - qd_ddot*pow(dt,2)/2;
                if(q_flag==1)
                {
                    qd_dot_old=qd_dot;
                    qd_old=qd;
                }*/
                ////////////////////
            }
               for(int i=0;i<n_joints_;i++)
               {
                   dq[i]=qd(i);
                   dqdot[i]=qd_dot(i);
               }

            cManipulator->pDyn->Prepare_Dynamics(q,qdot);

            if(traj_flag==0)
                Control->PD_Gravity(q, qdot, qd, qd_dot, torque);
            else
              // Control->VSD(q,qdot,traj_x,torque);
                Control->VSD(q,qdot,xd,torque);
            //Control->ComputedTorque(q, qdot, qd, qd_dot, qd_ddot, torque);
            //Control->PD_Gravity(q, qdot, qd, qd_dot, torque);
            //Control->Inverse_Dynamics_Control(q, qdot, qd, qd_dot, qd_ddot, torque);
            //Control->Gravity(q,qdot,torque);
            //cManipulator->pDyn->Mdot_Matrix(_Mdot);
            for (int i = 0; i < n_joints_; i++)
            {
                joints_[i].setCommand(torque[i]);
                //joints_[i].setCommand(0.0);
            }

            // ********* 4. data 저장 *********
            save_data();

            // ********* 5. state 출력 *********
            print_state();
        }

        void stopping(const ros::Time &time) override
        {
            delete Control;
            delete cManipulator;
            delete traj5th_joint;
            delete traj5th_task;
        }

        static void save_data()
        {

        }

        void print_state()
        {
            static int count = 0;
            if (count > 99)
            {
                printf("*********************************************************\n\n");
                printf("*** Simulation Time (unit: sec)  ***\n");
                printf("t = %f\n", t);
                printf("\n");

                printf("*** Command from Subscriber in Task Space (unit: m) ***\n");
                if (event == 0)
                {
                    printf("No Active!!!\n");
                }
                else
                {
                    printf("Active!!!\n");
                }

                printf("*** States in Joint Space (unit: deg) ***\n");
                for(int i=0; i < n_joints_; i++)
                {
                    printf("Joint ID:%d \t", i);
//                    printf("q: %0.3f, ", q_(i) * R2D);
//                    printf("dq: %0.3f, ", qd_(i) * R2D);
//                    printf("qdot: %0.3f, ", qdot_(i) * R2D);
//                    printf("dqdot: %0.3f, ", qd_dot_(i) * R2D);
                    printf("q: %0.3f, ", q_(i));
                    printf("dq: %0.3f, ", qd(i));
                    printf("qdot: %0.3f, ", qdot_(i));
                    printf("dqdot: %0.3f, ", qd_dot(i));
                    printf("torque: %0.3f", torque[i]);
                    printf("\n");
                }
                cout<<"FK"<<endl<<cManipulator->pKin->ForwardKinematics()<<endl;
                cout<<"KDL FK"<<endl<<x_.p(0)<<endl<<x_.p(1)<<endl<<x_.p(2)<<endl;
                cout<<"Euler Angle"<<endl<<rpy(0)<<", "<<rpy(1)<<", "<<rpy(2)<<endl;

                //cout<<"Linear Velocity Jacobian"<<endl<<l_jaco<<endl;
                JacobiSVD<MatrixXd> svd(l_jaco,ComputeThinU|ComputeThinV);
                cout<<"w: "<<cManipulator->pKin->Manipulability(l_jaco)<<endl;
                cout<<"k: "<<cManipulator->pKin->Condition_Number(l_jaco)<<endl;
                cout<<"Singular Value"<<endl<<svd.singularValues()<<endl;
                cout<<"Rotation Matrix"<<endl<<cManipulator->pKin->GetTMat(0,5)<<endl;
                //cout<<"Xd: "<<xda<<"  X: "<<xa<<endl;
                //cout<<"KDL Jacobian"<<endl<<J_.data.block<3,5>(0,0)<<endl;
                cout<<"Analytic Jacobian"<<endl<<l_jaco<<endl;
                //cout<<"KDL Jacobian"<<endl<<J_.data<<endl;
                //cout<<"PI"<<endl<<cManipulator->pKin->Pinv(l_jaco)<<endl;
                //cout<<"DPI"<<endl<<cManipulator->pKin->DPI(l_jaco)<<endl;
                //cout<<"xdot"<<endl<<xdot<<endl;
                //cout<<"KDL xdot"<<endl<<(J_.data*qdot_.data).block<3,1>(0,0)<<endl;
                //cout<<"Gravity"<<endl<<cManipulator->pDyn->G_Matrix()<<endl;
                //cout<<"Verify"<<endl<<_Mdot-2*cManipulator->pDyn->C_Matrix()<<endl;
                cout<<"M"<<endl<<cManipulator->pDyn->M_Matrix()<<endl;
                cout<<"KDL M"<<endl<<M_.data<<endl;
                cout<<"C"<<endl<<cManipulator->pDyn->C_out()*qdot_.data<<endl;
                cout<<"KDL C"<<endl<<C_.data<<endl;
                //cout<<"Jaco_dot"<<endl<<cManipulator->pKin->Jacobian_l_dot()<<endl;

                count = 0;
            }
            count++;
        }

    private:
        // others
        double t;
        int ctr_obj_;
        int ik_mode_;
        int event;

        //Joint handles
        unsigned int n_joints_;                               // joint 숫자
        std::vector<std::string> joint_names_;                // joint name ??
        std::vector<hardware_interface::JointHandle> joints_; // ??
        std::vector<urdf::JointConstSharedPtr> joint_urdfs_;  // ??

        // kdl
        KDL::Tree kdl_tree_;   // tree?
        KDL::Chain kdl_chain_; // chain?

        // kdl M,C,G
        KDL::JntSpaceInertiaMatrix M_; // intertia matrix
        KDL::JntArray C_;              // coriolis
        KDL::JntArray G_;              // gravity torque vector
        KDL::Vector gravity_;

        // kdl and Eigen Jacobian
        KDL::Jacobian J_;
        //KDL::Jacobian J_inv_;
        //Eigen::Matrix<double, num_taskspace, num_taskspace> J_inv_;
        Eigen::MatrixXd J_inv_;
        Eigen::Matrix<double, num_taskspace, num_taskspace> J_transpose_;

        // kdl solver
        boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_; //Solver to compute the forward kinematics (position)
        boost::scoped_ptr<KDL::ChainFkSolverVel_recursive> fk_vel_solver_; //Solver to compute the forward kinematics (velocity)
        boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_; //Solver to compute the jacobian
        boost::scoped_ptr<KDL::ChainDynParam> id_solver_;               // Solver To compute the inverse dynamics

        // Joint Space State
        KDL::JntArray qd_, qd_dot_, qd_ddot_;
        KDL::JntArray qd_old_;
        KDL::JntArray q_, qdot_;

        KDL::JntArray q_chain[2];
        KDL::JntArray qdot_chain[2];

        MatrixXd q_dot;

        double q[5], qdot[5];
        double dq[5] = {0.0,};
        double dqdot[5] = {0.0,};
        double dqddot[5] = {0.0,};
        double torque[5] = {0.0,};

        Jaco b_jaco;
        Jaco s_jaco;
        Jaco a_jaco;
        LinJaco l_jaco;
        LinJaco l_jaco_dot;
        PinvLJaco DPI_jaco;

        MatrixXd _Mdot;

        // Task Space State
        // ver. 01
        Vector3d xd, xddot, xdddot;

        Vector3d x;
        Vector3d rpy;
        Vector4d quat;
        Matrix<double,5,1> qd;
        Matrix<double,5,1> qd_old;
        Matrix<double,5,1> qd_dot;
        Matrix<double,5,1> qd_dot_old;
        Matrix<double,5,1> qd_ddot;
        KDL::Frame xd_; // x.p: frame position(3x1), x.m: frame orientation (3x3)
        KDL::Frame x_;
        KDL::Twist ex_temp_;
        int q_flag;

        // KDL::Twist xd_dot_, xd_ddot_;
        Eigen::Matrix<double, num_taskspace, 1> ex_;
        Eigen::Matrix<double, num_taskspace, 1> xd_dot_, xd_ddot_;
        Eigen::Matrix<double, num_taskspace, 1> xdot_, xdota, xa, xda;
        Eigen::Matrix<double, num_taskspace, 1> ex_dot_, ex_int_;
        Eigen::Matrix<double, 3,1> xdot;
        // Input
        KDL::JntArray x_cmd_;

        //Trajectory
        int Motion = 1;
        VectorXi TrajFlag_j;
        VectorXi TrajFlag_t;
        VectorXi TrajFlag_td;
        Matrix<double,5,1> traj_q;
        Vector3d traj_x;
        Matrix<double,6,1> traj_xd;
        double res_tra[3]={0.0,};
        double res_trad[6]={0.0,};
        Matrix3d ROT, ROTD;
        int traj_flag=0;

        // gains
        KDL::JntArray Kp_, Ki_, Kd_;
        double K_regulation_, K_tracking_;

        // save the data
        double SaveData_[SaveDataMax];

        // ros subscriber
        ros::Subscriber sub_x_cmd_;

        // ros publisher
        ros::Publisher pub_qd_, pub_q_, pub_e_;
        ros::Publisher pub_xd_, pub_x_, pub_ex_;
        ros::Publisher pub_SaveData_;

        // ros message
        std_msgs::Float64MultiArray msg_qd_, msg_q_, msg_e_;
        std_msgs::Float64MultiArray msg_xd_, msg_x_, msg_ex_;
        std_msgs::Float64MultiArray msg_SaveData_;

        robot *cManipulator;
        HYUControl::Controller *Control;
        hyuCtrl::Trajectory *traj5th_joint;
        hyuCtrl::Trajectory *traj5th_task;

    };
}

PLUGINLIB_EXPORT_CLASS(hyuspa_controller::ComputedTorque_Control_CLIK,controller_interface::ControllerBase)


/*
 * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
 * 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 "robot_localization/ukf.h"
#include "robot_localization/filter_common.h"

#include <XmlRpcException.h>

#include <sstream>
#include <iomanip>
#include <limits>

#include <Eigen/Cholesky>

#include <iostream>
#include <vector>

#include <assert.h>

namespace RobotLocalization
{
  Ukf::Ukf(std::vector<double> args) :
    FilterBase(),  // Must initialize filter base!
    uncorrected_(true)
  {
    assert(args.size() == 3);

    double alpha = args[0];
    double kappa = args[1];
    double beta = args[2];

    size_t sigmaCount = (STATE_SIZE << 1) + 1;
    sigmaPoints_.resize(sigmaCount, Eigen::VectorXd(STATE_SIZE));

    // Prepare constants
    lambda_ = alpha * alpha * (STATE_SIZE + kappa) - STATE_SIZE;

    stateWeights_.resize(sigmaCount);
    covarWeights_.resize(sigmaCount);

    stateWeights_[0] = lambda_ / (STATE_SIZE + lambda_);
    covarWeights_[0] = stateWeights_[0] + (1 - (alpha * alpha) + beta);
    sigmaPoints_[0].setZero();
    for (size_t i = 1; i < sigmaCount; ++i)
    {
      sigmaPoints_[i].setZero();
      stateWeights_[i] =  1 / (2 * (STATE_SIZE + lambda_));
      covarWeights_[i] = stateWeights_[i];
    }
  }

  Ukf::~Ukf()
  {
  }

  void Ukf::correct(const Measurement &measurement)
  {
    FB_DEBUG("---------------------- Ukf::correct ----------------------\n" <<
             "State is:\n" << state_ <<
             "\nMeasurement is:\n" << measurement.measurement_ <<
             "\nMeasurement covariance is:\n" << measurement.covariance_ << "\n");

    // In our implementation, it may be that after we call predict once, we call correct
    // several times in succession (multiple measurements with different time stamps). In
    // that event, the sigma points need to be updated to reflect the current state.
    // Throughout prediction and correction, we attempt to maximize efficiency in Eigen.
    if (!uncorrected_)
    {
      // Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition
      weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();

      // Compute sigma points

      // First sigma point is the current state
      sigmaPoints_[0] = state_;

      // Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column]
      // STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column]
      for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
      {
        sigmaPoints_[sigmaInd + 1] = state_ + weightedCovarSqrt_.col(sigmaInd);
        sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = state_ - weightedCovarSqrt_.col(sigmaInd);
      }
    }

    // We don't want to update everything, so we need to build matrices that only update
    // the measured parts of our state vector

    // First, determine how many state vector values we're updating
    std::vector<size_t> updateIndices;
    for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
    {
      if (measurement.updateVector_[i])
      {
        // Handle nan and inf values in measurements
        if (std::isnan(measurement.measurement_(i)))
        {
          FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n");
        }
        else if (std::isinf(measurement.measurement_(i)))
        {
          FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n");
        }
        else
        {
          updateIndices.push_back(i);
        }
      }
    }

    FB_DEBUG("Update indices are:\n" << updateIndices << "\n");

    size_t updateSize = updateIndices.size();

    // Now set up the relevant matrices
    Eigen::VectorXd stateSubset(updateSize);                              // x (in most literature)
    Eigen::VectorXd measurementSubset(updateSize);                        // z
    Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize);  // R
    Eigen::MatrixXd stateToMeasurementSubset(updateSize, STATE_SIZE);     // H
    Eigen::MatrixXd kalmanGainSubset(STATE_SIZE, updateSize);             // K
    Eigen::VectorXd innovationSubset(updateSize);                         // z - Hx
    Eigen::VectorXd predictedMeasurement(updateSize);
    Eigen::VectorXd sigmaDiff(updateSize);
    Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize);
    Eigen::MatrixXd crossCovar(STATE_SIZE, updateSize);

    std::vector<Eigen::VectorXd> sigmaPointMeasurements(sigmaPoints_.size(), Eigen::VectorXd(updateSize));

    stateSubset.setZero();
    measurementSubset.setZero();
    measurementCovarianceSubset.setZero();
    stateToMeasurementSubset.setZero();
    kalmanGainSubset.setZero();
    innovationSubset.setZero();
    predictedMeasurement.setZero();
    predictedMeasCovar.setZero();
    crossCovar.setZero();

    // Now build the sub-matrices from the full-sized matrices
    for (size_t i = 0; i < updateSize; ++i)
    {
      measurementSubset(i) = measurement.measurement_(updateIndices[i]);
      stateSubset(i) = state_(updateIndices[i]);

      for (size_t j = 0; j < updateSize; ++j)
      {
        measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
      }

      // Handle negative (read: bad) covariances in the measurement. Rather
      // than exclude the measurement or make up a covariance, just take
      // the absolute value.
      if (measurementCovarianceSubset(i, i) < 0)
      {
        FB_DEBUG("WARNING: Negative covariance for index " << i <<
                 " of measurement (value is" << measurementCovarianceSubset(i, i) <<
                 "). Using absolute value...\n");

        measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
      }

      // If the measurement variance for a given variable is very
      // near 0 (as in e-50 or so) and the variance for that
      // variable in the covariance matrix is also near zero, then
      // the Kalman gain computation will blow up. Really, no
      // measurement can be completely without error, so add a small
      // amount in that case.
      if (measurementCovarianceSubset(i, i) < 1e-9)
      {
        measurementCovarianceSubset(i, i) = 1e-9;

        FB_DEBUG("WARNING: measurement had very small error covariance for index " <<
                 updateIndices[i] <<
                 ". Adding some noise to maintain filter stability.\n");
      }
    }

    // The state-to-measurement function, h, will now be a measurement_size x full_state_size
    // matrix, with ones in the (i, i) locations of the values to be updated
    for (size_t i = 0; i < updateSize; ++i)
    {
      stateToMeasurementSubset(i, updateIndices[i]) = 1;
    }

    FB_DEBUG("Current state subset is:\n" << stateSubset <<
             "\nMeasurement subset is:\n" << measurementSubset <<
             "\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
             "\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n");

    // (1) Generate sigma points, use them to generate a predicted measurement
    for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
    {
      sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset * sigmaPoints_[sigmaInd];
      predictedMeasurement.noalias() += stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd];
    }

    // (2) Use the sigma point measurements and predicted measurement to compute a predicted
    // measurement covariance matrix P_zz and a state/measurement cross-covariance matrix P_xz.
    for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
    {
      sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement;
      predictedMeasCovar.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
      crossCovar.noalias() += covarWeights_[sigmaInd] * ((sigmaPoints_[sigmaInd] - state_) * sigmaDiff.transpose());
    }

    // (3) Compute the Kalman gain, making sure to use the actual measurement covariance: K = P_xz * (P_zz + R)^-1
    Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).inverse();
    kalmanGainSubset = crossCovar * invInnovCov;

    // (4) Apply the gain to the difference between the actual and predicted measurements: x = x + K(z - z_hat)
    innovationSubset = (measurementSubset - predictedMeasurement);

    // Wrap angles in the innovation
    for (size_t i = 0; i < updateSize; ++i)
    {
      if (updateIndices[i] == StateMemberRoll  ||
          updateIndices[i] == StateMemberPitch ||
          updateIndices[i] == StateMemberYaw)
      {
        while (innovationSubset(i) < -PI)
        {
          innovationSubset(i) += TAU;
        }

        while (innovationSubset(i) > PI)
        {
          innovationSubset(i) -= TAU;
        }
      }
    }

    // (5) Check Mahalanobis distance of innovation
    if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_))
    {
      state_.noalias() += kalmanGainSubset * innovationSubset;

      // (6) Compute the new estimate error covariance P = P - (K * P_zz * K')
      estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());

      wrapStateAngles();

      // Mark that we need to re-compute sigma points for successive corrections
      uncorrected_ = false;

      FB_DEBUG("Predicated measurement covariance is:\n" << predictedMeasCovar <<
               "\nCross covariance is:\n" << crossCovar <<
               "\nKalman gain subset is:\n" << kalmanGainSubset <<
               "\nInnovation:\n" << innovationSubset <<
               "\nCorrected full state is:\n" << state_ <<
               "\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ <<
               "\n\n---------------------- /Ukf::correct ----------------------\n");
    }
  }

  Eigen::VectorXd Ukf::predict(Eigen::VectorXd const& posterioriState, double delta)
  {
    double roll = posterioriState(StateMemberRoll);
    double pitch = posterioriState(StateMemberPitch);
    double yaw = posterioriState(StateMemberYaw);

    // We'll need these trig calculations a lot.
    double sp = ::sin(pitch);
    double cp = ::cos(pitch);
    double cpi = 1.0 / cp;
    double tp = sp * cpi;

    double sr = ::sin(roll);
    double cr = ::cos(roll);

    double sy = ::sin(yaw);
    double cy = ::cos(yaw);

    // Prepare the transfer function
    transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
    transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
    transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
    transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
    transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
    transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
    transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
    transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
    transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
    transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
    transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
    transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
    transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
    transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
    transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
    transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
    transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
    transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
    transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
    transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
    transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
    transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
    transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
    transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
    transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
    transferFunction_(StateMemberVx, StateMemberAx) = delta;
    transferFunction_(StateMemberVy, StateMemberAy) = delta;
    transferFunction_(StateMemberVz, StateMemberAz) = delta;

    return transferFunction_ * posterioriState;
  }

  void Ukf::predict(const double referenceTime, const double delta)
  {
    FB_DEBUG("---------------------- Ukf::predict ----------------------\n" <<
             "delta is " << delta <<
             "\nstate is " << state_ << "\n");

    prepareControl(referenceTime, delta);

    // (1) Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition
    weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();

    // (2) Compute sigma points *and* pass them through the transfer function to save
    // the extra loop

    // First sigma point is the current state
    sigmaPoints_[0] = predict(state_, delta);

    // Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column]
    // STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column]
    for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
    {
      sigmaPoints_[sigmaInd + 1] = predict(state_ + weightedCovarSqrt_.col(sigmaInd), delta);
      sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = predict(state_ - weightedCovarSqrt_.col(sigmaInd), delta);
    }

    // (3) Sum the weighted sigma points to generate a new state prediction
    state_.setZero();
    for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
    {
      state_.noalias() += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd];
    }

    // (4) Now us the sigma points and the predicted state to compute a predicted covariance
    estimateErrorCovariance_.setZero();
    Eigen::VectorXd sigmaDiff(STATE_SIZE);
    for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
    {
      sigmaDiff = (sigmaPoints_[sigmaInd] - state_);
      estimateErrorCovariance_.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
    }

    // (5) Not strictly in the theoretical UKF formulation, but necessary here
    // to ensure that we actually incorporate the processNoiseCovariance_
    Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_;

    if (useDynamicProcessNoiseCovariance_)
    {
      computeDynamicProcessNoiseCovariance(state_, delta);
      processNoiseCovariance = &dynamicProcessNoiseCovariance_;
    }

    estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);

    // Keep the angles bounded
    wrapStateAngles();

    // Mark that we can keep these sigma points
    uncorrected_ = true;

    FB_DEBUG("Predicted state is:\n" << state_ <<
             "\nPredicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
             "\n\n--------------------- /Ukf::predict ----------------------\n");
  }

}  // namespace RobotLocalization