#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>

#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/flann.hpp>
#include <opencv2/video.hpp>

#include <cstdio>
#include <iostream>
#include <sys/stat.h>

#include <cassert>

#include <sphericalsfm/spherical_estimator.h>
#include <sphericalsfm/spherical_fast_estimator.h>
#include <sphericalsfm/preemptive_ransac.h>
//#include <sphericalsfm/msac.h>
#include <sphericalsfm/so3.h>
#include <sphericalsfm/rotation_averaging.h>
#include <sphericalsfm/spherical_utils.h>

#include "spherical_sfm_tools.h"

using namespace sphericalsfm;

namespace sphericalsfmtools {

    void drawmatches( const cv::Mat &image0, const cv::Mat &image1, const Features &features0, const Features &features1, const Matches &m01, cv::Mat &imageout )
    {
        cv::addWeighted( image0, 0.5, image1, 0.5, 0, imageout );
        for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
        {
            size_t ind0 = it->first;
            size_t ind1 = it->second;

            cv::line( imageout, features0.points[it->first], features1.points[it->second], cv::Scalar(255,255,255) );
        }
    }

    class DetectorTracker
    {
    protected:
        double min_dist; // minimum distance between existing points and detecting points
        const int xradius; // horizontal tracking radius
        const int yradius; // vertical tracking radius
        cv::Ptr<cv::xfeatures2d::SIFT> sift;
    public:
        DetectorTracker( double _min_dist=0, double _xradius=0, double _yradius=0 ) :
        min_dist(_min_dist), xradius(_xradius), yradius(_yradius), sift(cv::xfeatures2d::SIFT::create(2000)) { }

        void detect( const cv::Mat &image, Features &features )
        {
            assert( image.type() == CV_8UC1 );

            std::vector<cv::KeyPoint> keypoints;
            cv::Mat descs;

            if ( features.empty() )
            {
                sift->detectAndCompute( image, cv::noArray(), keypoints, descs );
            } else {
                cv::Mat mask(image.size(),CV_8UC1);
                mask = cv::Scalar(255);
                for ( int i = 0; i < features.points.size(); i++ ) cv::circle(mask,features.points[i],min_dist,cv::Scalar(0),-1);
                sift->detectAndCompute( image, mask, keypoints, descs );
            }

            for  ( int i = 0; i < keypoints.size(); i++ )
            {
                features.points.push_back( keypoints[i].pt );
                features.descs.push_back( descs.row(i) );
            }
        }

        void track( cv::Mat &image0, cv::Mat &image1,
                    const Features &features0, Features &features1,
                    Matches &m01 )
        {
            const size_t npts0 = features0.size();

            const std::vector<cv::Point2f> &prevPts(features0.points);
            std::vector<cv::Point2f> nextPts;
            std::vector<uchar> status;
            std::vector<float> err;

            cv::calcOpticalFlowPyrLK(image0, image1, prevPts, nextPts, status, err, cv::Size(xradius*2+1,yradius*2+1));

            for ( int i = 0; i < npts0; i++ )
            {
                if ( status[i] == 0 ) continue;

                features1.points.push_back( nextPts[i] );
                features1.descs.push_back( features0.descs.row(i) );
                m01[i] = features1.size()-1;
            }
        }

    };

    void match( const Features &features0, const Features &features1, Matches &m01, double ratio = 0.75 )
    {
        cv::BFMatcher matcher;

        std::vector< std::vector<cv::DMatch> > matches;

        // p1 = query, p0 = train
        matcher.knnMatch( features1.descs, features0.descs, matches, 2);

        for ( int i = 0; i < matches.size(); i++ )
        {
            if ( matches[i][0].distance < ratio*matches[i][1].distance )
            {
                m01[matches[i][0].trainIdx] = matches[i][0].queryIdx;
            }
        }
    }

    void build_feature_tracks( const Intrinsics &intrinsics, const std::string &videopath,
                              std::vector<Keyframe> &keyframes, std::vector<ImageMatch> &image_matches,
                              const double inlier_threshold, const double min_rot )
    {
        bool inward = false;

        Eigen::Matrix3d Kinv = intrinsics.getKinv();

        cv::VideoCapture cap(videopath);

        cv::Mat image0_in, image0;
        if ( !cap.read(image0_in) )
        {
            std::cout << "error: could not read single frame from " << videopath << "\n";
            exit(1);
        }
        if ( image0_in.channels() == 3 ) cv::cvtColor( image0_in, image0, cv::COLOR_BGR2GRAY );
        else image0 = image0_in;
        //char imagepath[1024];
        //sprintf(imagepath,videopath.c_str(),1);
        //image0 = cv::imread(imagepath,cv::IMREAD_GRAYSCALE);

        int xradius = image0.cols/48;
        int yradius = image0.rows/48;

        std::cout << "image0 has size " << image0.cols << " " << image0.rows << "\n";
        std::cout << "radii: " << xradius << " " << yradius << "\n";
        //double min_dist = 5;
        //double min_dist = 10*xradius;
        //double min_dist = 10;
        double min_dist = 20;
        //double min_dist = sqrt(xradius*xradius+yradius*yradius);
        DetectorTracker detectortracker( min_dist, xradius, yradius );

        Features features0;
        detectortracker.detect( image0, features0 );

        keyframes.push_back(Keyframe(0,features0));
        image0.copyTo(keyframes[keyframes.size()-1].image);

        std::cout << "detected " << features0.size() << " features in image 0\n";

        PreemptiveRANSAC<RayPairList,Estimator> ransac( 100 );
        //MSAC<RayPairList,Estimator> ransac;
        ransac.inlier_threshold = inlier_threshold*Kinv(0);///intrinsics.focal;

        std::vector<Estimator*> estimators( 2000 );
        for ( size_t i = 0; i < estimators.size(); i++ )
        {
            estimators[i] = new SphericalEstimator;
        }

        int kf_index = 1;
        int video_index = 1;
        cv::Mat image1_in, image1;
        while ( cap.read( image1_in ) )
        {
            if ( image1_in.channels() == 3 ) cv::cvtColor( image1_in, image1, cv::COLOR_BGR2GRAY );
            else image1 = image1_in;
            //sprintf(imagepath,videopath.c_str(),video_index+1);
            //image1 = cv::imread(imagepath,cv::IMREAD_GRAYSCALE);
            //if ( image1.rows == 0 ) break;

            Features features1;

            Matches m01;

            detectortracker.track( image0, image1, features0, features1, m01 );
            std::cout << "tracked " << features1.size() << " features in image " << video_index << "\n";

            std::vector<bool> inliers;
            int ninliers;

            // find inlier matches using relative pose
            RayPairList ray_pair_list;
            ray_pair_list.reserve( m01.size() );
            for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
            {
                size_t index0 = it->first;
                size_t index1 = it->second;

                //Eigen::Vector3d loc0 = Eigen::Vector3d( features0[index0].x, features0[index0].y, 1 );
                //Eigen::Vector3d loc1 = Eigen::Vector3d( features1[index1].x, features1[index1].y, 1 );
                Eigen::Vector3d loc0 = Eigen::Vector3d( features0.points[index0].x, features0.points[index0].y, 1 );
                Eigen::Vector3d loc1 = Eigen::Vector3d( features1.points[index1].x, features1.points[index1].y, 1 );
                loc0 = Kinv * loc0;
                loc1 = Kinv * loc1;

                Ray u;
                u.head(3) = loc0;
                Ray v;
                v.head(3) = loc1;

                ray_pair_list.push_back( std::make_pair( u, v ) );
            }

            SphericalEstimator *best_estimator = NULL;

            std::cout << "running ransac on " << ray_pair_list.size() << " matches\n";

            ninliers = ransac.compute( ray_pair_list.begin(), ray_pair_list.end(), estimators, (Estimator**)&best_estimator, inliers );

            fprintf( stdout, "%d: %lu matches and %d inliers (%0.2f%%)\n", video_index, m01.size(), ninliers, (double)ninliers/(double)m01.size()*100 );

            Eigen::Vector3d best_estimator_t;
            Eigen::Vector3d best_estimator_r;
            decompose_spherical_essential_matrix( best_estimator->E, false, best_estimator_r, best_estimator_t );
            std::cout << best_estimator_r.transpose() << "\n";

            // check for minimum rotation
            if ( best_estimator_r.norm() < min_rot * M_PI / 180. ) {
                video_index++;
                continue;
            }

            Matches m01inliers;
            size_t i = 0;
            for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
            {
                if ( inliers[i++] ) m01inliers[it->first] = it->second;
            }

            image1.copyTo(image0);

            // only propagate inlier points
            Matches new_m01;
            features0 = Features();
            for ( Matches::const_iterator it = m01inliers.begin(); it != m01inliers.end(); it++ )
            {
                new_m01[it->first] = features0.size();
                //Feature &feature1(features1[it->second]);
                //features0.push_back(Feature(feature1.x,feature1.y,feature1.descriptor));
                features0.points.push_back(features1.points[it->second]);
                features0.descs.push_back(features1.descs.row(it->second));
            }

            std::cout << "features0 now has " << features0.size() << " features\n";
            // also detect new points that are some distance away from matched points
            detectortracker.detect( image0, features0 );
            std::cout << "after sift, features0 now has " << features0.size() << " features\n";

            keyframes.push_back(Keyframe(video_index,features0));
            image0.copyTo(keyframes[keyframes.size()-1].image);
            image_matches.push_back( ImageMatch( kf_index-1, kf_index, new_m01, so3exp(best_estimator_r) ) );
            video_index++;
            kf_index++;
        }
    }

    int make_loop_closures( const Intrinsics &intrinsics, const std::vector<Keyframe> &keyframes, std::vector<ImageMatch> &image_matches,
                            const double inlier_threshold, const int min_num_inliers, const int num_frames_begin, const int num_frames_end, const bool best_only )
    {
        Eigen::Matrix3d Kinv = intrinsics.getKinv();

        PreemptiveRANSAC<RayPairList,Estimator> ransac;//( 100 );
        //MSAC<RayPairList,Estimator> ransac;
        ransac.inlier_threshold = inlier_threshold*Kinv(0);///intrinsics.focal;

        std::vector<Estimator*> estimators( 2000 );
        for ( size_t i = 0; i < estimators.size(); i++ )
        {
            estimators[i] = new SphericalEstimator;
            //estimators[i] = new SphericalFastEstimator;
        }

        ImageMatch best_image_match( -1, -1, Matches(), Eigen::Matrix3d::Identity() );
        int loop_closure_count = 0;

        DetectorTracker detectortracker( 0, 0, 0 );

        int count0 = 0;
        for ( int index0 = 0; index0 < keyframes.size(); index0++ )
        {
            if ( ++count0 > num_frames_begin ) break;

            const Features &features0 = keyframes[index0].features;
            //Features features0;
            //detectortracker.detect( keyframes[index0].image, features0 );

            int count1 = 0;
            for ( int index1 = keyframes.size()-1; index1 >= index0+1; index1-- )
            {
                if ( ++count1 > num_frames_end ) break;

                const Features &features1 = keyframes[index1].features;
                //Features features1;
                //detectortracker.detect( keyframes[index1].image, features1 );

                Matches m01;

                match( features0, features1, m01 );
                if ( m01.size() < min_num_inliers ) continue;

                //cv::Mat matchesim;
                //drawmatches( keyframes[index0].image, keyframes[index1].image, features0, features1, m01, matchesim );
                //cv::imwrite( "matches" + std::to_string(keyframes[index0].index) + "-" + std::to_string(keyframes[index1].index) + ".png", matchesim );

                std::vector<bool> inliers;
                int ninliers;

                // find inlier matches using relative pose
                RayPairList ray_pair_list;
                ray_pair_list.reserve( m01.size() );
                for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
                {
                    cv::Point2f pt0 = features0.points[it->first];
                    cv::Point2f pt1 = features1.points[it->second];

                    Eigen::Vector3d loc0 = Eigen::Vector3d( pt0.x, pt0.y, 1 );
                    Eigen::Vector3d loc1 = Eigen::Vector3d( pt1.x, pt1.y, 1 );

                    loc0 = Kinv * loc0;
                    loc1 = Kinv * loc1;

                    Ray u;
                    u.head(3) = loc0;
                    Ray v;
                    v.head(3) = loc1;

                    ray_pair_list.push_back( std::make_pair( u, v ) );
                }

                SphericalEstimator *best_estimator = NULL;
                //SphericalFastEstimator *best_estimator = NULL;

                if ( m01.empty() ) ninliers = 0;
                else ninliers = ransac.compute( ray_pair_list.begin(), ray_pair_list.end(), estimators, (Estimator**)&best_estimator, inliers );
                fprintf( stdout, "%d %d: %lu matches and %d inliers (%0.2f%%)\n",
                        keyframes[index0].index, keyframes[index1].index, m01.size(), ninliers, (double)ninliers/(double)m01.size()*100 );

                Matches m01inliers;
                size_t i = 0;
                for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
                {
                    if ( inliers[i++] ) m01inliers[it->first] = it->second;
                }

                //cv::Mat inliersim;
                //drawmatches( keyframes[index0].image, keyframes[index1].image, features0, features1, m01inliers, inliersim );
                //cv::imwrite( "inliers" + std::to_string(keyframes[index0].index) + "-" + std::to_string(keyframes[index1].index) + ".png", inliersim );

                if ( ninliers > min_num_inliers )
                {
                    Eigen::Vector3d best_estimator_t;
                    Eigen::Vector3d best_estimator_r;
                    decompose_spherical_essential_matrix( best_estimator->E, false, best_estimator_r, best_estimator_t );
                    std::cout << best_estimator_r.transpose() << "\n";

                    ImageMatch image_match(index0, index1, m01inliers, so3exp(best_estimator_r));
                    if ( ninliers > best_image_match.matches.size() ) best_image_match = image_match;
                    if ( !best_only )
                    {
                        image_matches.push_back(image_match);
                        loop_closure_count++;
                    }
                }
            }
        }
        if ( best_only )
        {
            if ( best_image_match.matches.empty() ) return 0;
            image_matches.push_back(best_image_match);
            return 1;
        }
        return loop_closure_count;
    }

    void initialize_rotations( const int num_cameras, const std::vector<ImageMatch> &image_matches, std::vector<Eigen::Matrix3d> &rotations )
    {
        rotations.resize(num_cameras);
        for ( int i = 0 ; i < num_cameras; i++ ) rotations[i] = Eigen::Matrix3d::Identity();

        Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
        rotations[0] = R;
        for ( int index = 1; index < num_cameras; index++ )
        {
            for ( int i = 0; i < image_matches.size(); i++ )
            {
                if ( image_matches[i].index0 == index-1 && image_matches[i].index1 == index )
                {
                    R = image_matches[i].R * R;
                    rotations[index] = R;
                    break;
                }
            }
        }
    }

    void refine_rotations( const int num_cameras, const std::vector<ImageMatch> &image_matches, std::vector<Eigen::Matrix3d> &rotations )
    {
        std::vector<RelativeRotation> relative_rotations;
        for ( int i = 0; i < image_matches.size(); i++ )
        {
            relative_rotations.push_back( RelativeRotation( image_matches[i].index0, image_matches[i].index1, image_matches[i].R ) );
        }

        optimize_rotations( rotations, relative_rotations );
    }

    void build_sfm( const std::vector<Keyframe> &keyframes, const std::vector<ImageMatch> &image_matches, const std::vector<Eigen::Matrix3d> &rotations,
                   sphericalsfm::SfM &sfm )
    {
        std::cout << "building tracks\n";
        std::vector< std::vector<int> > tracks(keyframes.size());
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            tracks[i].resize( keyframes[i].features.size() );
            for ( int j = 0; j < keyframes[i].features.size(); j++ )
            {
                tracks[i][j] = -1;
            }
        }

        // add cameras
        std::cout << "adding cameras\n";
        for ( int index = 0; index < keyframes.size(); index++ )
        {
            int camera = sfm.AddCamera( Pose( Eigen::Vector3d(0,0,-1), so3ln(rotations[index]) ) );
            sfm.SetRotationFixed( camera, (index==0) );
            sfm.SetTranslationFixed( camera, true );
        }

        std::cout << "adding tracks\n";
        std::cout << "number of keyframes is " << keyframes.size() << "\n";
        for ( int i = 0; i < image_matches.size(); i++ )
        {
            const Matches &m01 = image_matches[i].matches;

            const int index0 = image_matches[i].index0;
            const int index1 = image_matches[i].index1;
            //std::cout << "match between keyframes " << index0 << " and " << index1 << " has " << m01.size() << " matches\n";

            const Features &features0 = keyframes[index0].features;
            const Features &features1 = keyframes[index1].features;

            for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
            {
                //const Feature &feature0 = features0[it->first];
                //const Feature &feature1 = features1[it->second];
                const cv::Point2f pt0 = features0.points[it->first];
                const cv::Point2f pt1 = features1.points[it->second];

                int &track0 = tracks[index0][it->first];
                int &track1 = tracks[index1][it->second];

                //Observation obs0( feature0.x-sfm.GetIntrinsics().centerx, feature0.y-sfm.GetIntrinsics().centery );
                //Observation obs1( feature1.x-sfm.GetIntrinsics().centerx, feature1.y-sfm.GetIntrinsics().centery );
                Observation obs0( pt0.x-sfm.GetIntrinsics().centerx, pt0.y-sfm.GetIntrinsics().centery );
                Observation obs1( pt1.x-sfm.GetIntrinsics().centerx, pt1.y-sfm.GetIntrinsics().centery );

                if ( track0 != -1 && track1 == -1 )
                {
                    track1 = track0;
                    sfm.AddObservation( index1, track0, obs1 );
                }
                else if ( track0 == -1 && track1 != -1 )
                {
                    track0 = track1;
                    sfm.AddObservation( index0, track1, obs0 );
                }
                else if ( track0 == -1 && track1 == -1 )
                {
                    //track0 = track1 = sfm.AddPoint( Eigen::Vector3d::Zero(), feature0.descriptor );
                    track0 = track1 = sfm.AddPoint( Eigen::Vector3d::Zero(), features0.descs.row(it->first) );

                    sfm.SetPointFixed( track0, false );

                    sfm.AddObservation( index0, track0, obs0 );
                    sfm.AddObservation( index1, track1, obs1 );
                }
                else if ( track0 != -1 && track1 != -1 && ( track0 != track1 ) )
                {
                    // track1 will be removed
                    sfm.MergePoint( track0, track1 );

                    // update all features with track1 and set to track0
                    for ( int i = 0; i < tracks.size(); i++ )
                    {
                        for ( int j = 0; j < tracks[i].size(); j++ )
                        {
                            if ( tracks[i][j] == track1 ) tracks[i][j] = track0;
                        }
                    }
                }
            }
        }

        std::cout << "retriangulating...\n";
        sfm.Retriangulate();

        /*
        std::cout << "adding loop closure observations\n";
        for ( int i = 0; i < image_matches.size(); i++ )
        {
            const Matches &m01 = image_matches[i].matches;

            const int index0 = image_matches[i].index0;
            const int index1 = image_matches[i].index1;
            //std::cout << "match between keyframes " << index0 << " and " << index1 << " has " << m01.size() << " matches\n";

            const Features &features0 = keyframes[index0].features;
            const Features &features1 = keyframes[index1].features;

            for ( Matches::const_iterator it = m01.begin(); it != m01.end(); it++ )
            {
                const Feature &feature0 = features0[it->first];
                const Feature &feature1 = features1[it->second];

                int &track0 = tracks[index0][it->first];
                int &track1 = tracks[index1][it->second];

                Observation obs0( feature0.x-sfm.GetIntrinsics().centerx, feature0.y-sfm.GetIntrinsics().centery );
                Observation obs1( feature1.x-sfm.GetIntrinsics().centerx, feature1.y-sfm.GetIntrinsics().centery );

                if ( track0 != -1 && track1 != -1 && ( track0 != track1 ) )
                {
                    // add mutual observations
                    //sfm.AddObservation( index0, track1, obs0 );
                    //sfm.AddObservation( index1, track0, obs1 );
                    // track1 will be removed
                    sfm.MergePoint( track0, track1 );

                    // update all features with track1 and set to track0
                    for ( int i = 0; i < tracks.size(); i++ )
                    {
                        for ( int j = 0; j < tracks[i].size(); j++ )
                        {
                            if ( tracks[i][j] == track1 ) tracks[i][j] = track0;
                        }
                    }
                }
            }
        }
        */
    }

}


#include <boost/math/concepts/std_real_concept.hpp>


// Copyright © 2017-2020 Trust Wallet.
//
// This file is part of Trust. The full Trust copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include "Asset.h"

#include <boost/algorithm/string/trim.hpp>
#include <boost/lexical_cast.hpp>
#include <stdexcept>

using namespace TW::EOS;

static const int64_t precision = 1000;
static const uint8_t maxDecimals = 18;

Asset::Asset(int64_t amount, uint8_t decimals, const std::string& symbol) {
    if (decimals > maxDecimals) {
        throw std::invalid_argument("Too many decimals!");
    }
    this->symbol |= decimals;

    if (symbol.size() < 1 || symbol.size() > 7) {
        throw std::invalid_argument("Symbol size invalid!");
    }

    for (int i = 0; i < symbol.size(); i++) {
        uint64_t c = symbol[i];
        if (c < 'A' || c > 'Z') {
            throw std::invalid_argument("Invalid symbol " + symbol + ".\n Symbol can only have upper case alphabets!");
        }

        this->symbol |= c << (8 * (i + 1));
    }

    this->amount = amount;
}

Asset Asset::fromString(std::string assetString) {
    using namespace std;

    boost::algorithm::trim(assetString);

    // Find space in order to split amount and symbol
    auto spacePosition = assetString.find(' ');
    if (spacePosition == string::npos) {
        throw std::invalid_argument("Asset's amount and symbol should be separated with space");
    }

    auto symbolString = boost::algorithm::trim_copy(assetString.substr(spacePosition + 1));
    auto amountString = assetString.substr(0, spacePosition);

    // Ensure that if decimal point is used (.), decimal fraction is specified
    auto dotPosition = amountString.find('.');
    if (dotPosition != string::npos && dotPosition == amountString.size() - 1) {
        throw std::invalid_argument("Missing decimal fraction after decimal point");
    }

    uint8_t decimals = 0;
    if (dotPosition != string::npos) {
        decimals = static_cast<uint8_t>(amountString.size() - dotPosition - 1);
    }

    int64_t precision = static_cast<uint64_t>(pow(10, static_cast<double>(decimals)));

    // Parse amount
    int64_t intPart, fractPart = 0;
    if (dotPosition != string::npos) {
        intPart = boost::lexical_cast<int64_t>(amountString.data(), dotPosition);
        fractPart = boost::lexical_cast<int64_t>(amountString.data() + dotPosition + 1, decimals);
        if (amountString[0] == '-') {
            fractPart *= -1;
        }
    } else {
        intPart = boost::lexical_cast<int64_t>(amountString);
    }

    int64_t amount = intPart;

    // multiply and check overflow
    amount *= precision;
    if (amount / precision != intPart) {
        throw std::invalid_argument("Amount too large!");
    }

    // add and check overflow
    amount += fractPart;
    if (amountString[0] == '-') {
        if (amount > fractPart) {
            throw std::invalid_argument("Amount too large!");
        }
    } else {
        if (amount < fractPart) {
            throw std::invalid_argument("Amount too large!");
        }
    }

    return Asset(amount, decimals, symbolString);
}

void Asset::serialize(Data& os) const noexcept {
    encode64LE(amount, os);
    encode64LE(symbol, os);
}

std::string Asset::string() const {
    static const int maxBufferSize = 30;
    char buffer[maxBufferSize];

    auto decimals = getDecimals();

    int charsWritten = snprintf(buffer, maxBufferSize, "%.*f %s",
                            decimals,
                            static_cast<double>(amount) / precision,
                            getSymbol().c_str());

    if (charsWritten < 0 || charsWritten > maxBufferSize) {
        throw std::runtime_error("Failed to create string representation of asset!");
    }

    return std::string(buffer, charsWritten);
}

std::string Asset::getSymbol() const noexcept {
    uint64_t temp = symbol >> 8;
    std::string str;

    while (temp > 0) {
        str += temp & 0xFF;
        temp >>= 8;
    }

    return str;
}


#include "ros/ros.h"

#include <tf/transform_listener.h>
// #include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/Twist.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

#include <sstream>
#include <fstream>
#include <chrono>

#include <termios.h>
#include <stdio.h>
#include <iostream>

/* crazyflie_driver */
#include "crazyflie_driver/AddCrazyflie.h"
#include "crazyflie_driver/LogBlock.h"
#include "crazyflie_driver/GenericLogData.h"
#include "crazyflie_driver/UpdateParams.h"
#include "crazyflie_driver/UploadTrajectory.h"
#include "crazyflie_driver/NotifySetpointsStop.h"
#undef major
#undef minor
#include "crazyflie_driver/Hover.h"
#include "crazyflie_driver/Takeoff.h"
#include "crazyflie_driver/Land.h"
#include "crazyflie_driver/GoTo.h"
#include "crazyflie_driver/StartTrajectory.h"
#include "crazyflie_driver/SetGroupMask.h"
#include "crazyflie_driver/FullState.h"
#include "crazyflie_driver/Position.h"
#include "crazyflie_driver/VelocityWorld.h"

/* Includes form iLQR main.cpp */
#include <unsupported/Eigen/AdolcForward>
#include <Eigen/Dense>
#include <adolc/adolc.h>
#include <adolc/adouble.h>
#include <vector>
#include <time.h>
#include <math.h>
#include <fstream>
#include <thread>
#include <future>
#include <algorithm>
#include <iterator>
#include "cost.h"
#include "dynamics.h"
#include "iLQR.h"
#include "utils.h"
#include <unistd.h>

/* simple moving average filter */
#define USE_SMA_FILTER 0
#define SMA_WINDOW_LEN 10

/* first order exponential low-pass filter */
#define USE_EXP_FILTER 1
#define EXP_FILT_ALPHA 0.9f

// static stateVector prevState_cf1;
// static stateVector stateWindowBuf_cf1[SMA_WINDOW_LEN];

// static stateVector prevState_cf2;
// static stateVector stateWindowBuf_cf2[SMA_WINDOW_LEN];

static stateVector CFState_cf1;
static stateVector CFState_cf2;
static stateVector CFState_cf3;
static stateVector CFState_cf4;

static pose currentPose;
// static pose prevPose_cf1;

// static pose currentPose_cf2;
// static pose prevPose_cf2;

std::ofstream droneDatalogFile;
std::ofstream optimizerDatalogFile;

bool first = true;

void poseCallback(const tf2_msgs::TFMessage::ConstPtr& msg) {

    /* Collect pose and timestamp information */
    std::string cf_id = msg->transforms[0].child_frame_id;
    int secs = msg->transforms[0].header.stamp.sec;
    int nsecs = msg->transforms[0].header.stamp.nsec;
    float x = msg->transforms[0].transform.translation.x;
    float y = msg->transforms[0].transform.translation.y;
    float z = msg->transforms[0].transform.translation.z;
    float qx = msg->transforms[0].transform.rotation.x;
    float qy = msg->transforms[0].transform.rotation.y;
    float qz = msg->transforms[0].transform.rotation.z;
    float qw = msg->transforms[0].transform.rotation.w;
    tf2::Quaternion q(qx, qy, qz, qw);

    /* Quaternion to roll,pitch,yaw conversion */
    double roll, pitch, yaw;
    tf2::Matrix3x3 m(q);
    m.getRPY(roll, pitch, yaw);

    /* Calculate timestamp in nanoseconds*/
    unsigned long timeStamp = (secs * 1e9) + nsecs;
    double deltaT = 0.0;

    if (cf_id == "cf1") {
        CFState_cf1.x = x;
        CFState_cf1.y = y;
        CFState_cf1.z = z;
        CFState_cf1.roll = roll;
        CFState_cf1.pitch = pitch;
        CFState_cf1.yaw = yaw;
    } else if (cf_id == "cf2") {
        CFState_cf2.x = x;
        CFState_cf2.y = y;
        CFState_cf2.z = z;
        CFState_cf2.roll = roll;
        CFState_cf2.pitch = pitch;
        CFState_cf2.yaw = yaw;
    } else if (cf_id == "cf3") {
        CFState_cf3.x = x;
        CFState_cf3.y = y;
        CFState_cf3.z = z;
        CFState_cf3.roll = roll;
        CFState_cf3.pitch = pitch;
        CFState_cf3.yaw = yaw;
    } else if (cf_id == "cf4") {
        CFState_cf4.x = x;
        CFState_cf4.y = y;
        CFState_cf4.z = z;
        CFState_cf4.roll = roll;
        CFState_cf4.pitch = pitch;
        CFState_cf4.yaw = yaw;
    }

    std::vector<stateVector> states{CFState_cf1, CFState_cf2, CFState_cf3, CFState_cf4};

    logDroneStates(droneDatalogFile, states);

    // std::printf("\n");
    // std::printf("##### CF1:\n");
    // std::printf("X: %f,\t Y: %f,\t Z: %f\t\n ROLL: %f,\t PITCH: %f,\t YAW %f\n",
    //             CFState_cf1.x, CFState_cf1.y, CFState_cf1.z, CFState_cf1.roll, CFState_cf1.pitch, CFState_cf1.yaw);
    // std::printf("X_d: %f\t, Y_d: %f\t, Z_d: %f\t\nROLL_d: %f\t, PITCH_d: %f\t, YAW_d %f\n",
    //             CFState_cf1.x_d, CFState_cf1.y_d, CFState_cf1.z_d, CFState_cf1.roll_d, CFState_cf1.pitch_d, CFState_cf1.yaw_d);

    // std::printf("\n##### CF2:\n");
    // std::printf("X: %f,\t Y: %f,\t Z: %f\t\n ROLL: %f,\t PITCH: %f,\t YAW %f\n",
    //             CFState_cf2.x, CFState_cf2.y, CFState_cf2.z, CFState_cf2.roll, CFState_cf2.pitch, CFState_cf2.yaw);
    // std::printf("X_d: %f\t, Y_d: %f\t, Z_d: %f\t\nROLL_d: %f\t, PITCH_d: %f\t, YAW_d %f\n",
    //             CFState_cf2.x_d, CFState_cf2.y_d, CFState_cf2.z_d, CFState_cf2.roll_d, CFState_cf2.pitch_d, CFState_cf2.yaw_d);
}

/* Modification of getch() to be non-blocking */
int getch() {
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering
  newt.c_cc[VMIN] = 0; newt.c_cc[VTIME] = 0;
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings
  int c = getchar();                         // read character (non-blocking)
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}

int main(int argc, char **argv) {

    srand((unsigned int) time(0));

    init_datalog(droneDatalogFile,
                 "/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/",
                 "droneStates_2x");

    init_datalog(optimizerDatalogFile,
                 "/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/",
                 "optimizer_2x");

    ros::init(argc, argv, "iLQR_Node");

    ros::NodeHandle n;

    ros::Subscriber state_subscriber = n.subscribe("tf", 1000, poseCallback);

    /* Publisher to cmd_position topic (currently doesn't work) */
    // ros::Publisher pub = n.advertise<crazyflie_driver::Position>("cf1/cmd_position",1);
    // crazyflie_driver::Position position_cmd;

    /* Service clients for simple high-level commands */
    ros::ServiceClient takeoffClient = n.serviceClient<crazyflie_driver::Takeoff>("/takeoff");
    ros::ServiceClient landClient = n.serviceClient<crazyflie_driver::Land>("/land");
    ros::ServiceClient GoToClient_cf1 = n.serviceClient<crazyflie_driver::GoTo>("/cf1/go_to");
    ros::ServiceClient GoToClient_cf2 = n.serviceClient<crazyflie_driver::GoTo>("/cf2/go_to");
    ros::ServiceClient GoToClient_cf3 = n.serviceClient<crazyflie_driver::GoTo>("/cf3/go_to");
    ros::ServiceClient GoToClient_cf4 = n.serviceClient<crazyflie_driver::GoTo>("/cf4/go_to");

    /* Wait for <Enter> key press to begin mission */
    std::cout << "\t########## PRESS <Enter> TO BEGIN MISSION ##########" << std::endl;
    while (true) {
        int c = getch();
            if (c == '\n') {
                std::cout << "\t########## BEGINNING MISSION ##########" << std::endl;
                break;
            }
            ros::spinOnce();
    }

    /* Construct GoTo service once to be used in loop */
    crazyflie_driver::GoTo srvGoTo_cf1;
    srvGoTo_cf1.request.groupMask = 0;  // signal all CFs (I think?)
    crazyflie_driver::GoTo srvGoTo_cf2;
    srvGoTo_cf2.request.groupMask = 0;
    crazyflie_driver::GoTo srvGoTo_cf3;
    srvGoTo_cf3.request.groupMask = 0;
    crazyflie_driver::GoTo srvGoTo_cf4;
    srvGoTo_cf4.request.groupMask = 0;

    /* Send takeoff command through /Takeoff service */
    crazyflie_driver::Takeoff srvTakeoff;
    srvTakeoff.request.groupMask = 0;
    srvTakeoff.request.height = 1.0;
    srvTakeoff.request.duration = ros::Duration(3.0);
    takeoffClient.call(srvTakeoff);
    ros::Duration(3.0).sleep();

    // srvGoTo_cf1.request.goal.x = 0.0;
    // srvGoTo_cf1.request.goal.y = 1.25;
    // srvGoTo_cf1.request.goal.z = 1.5;
    // srvGoTo_cf1.request.yaw = 0.0;
    // srvGoTo_cf1.request.duration = ros::Duration(2.0);
    // GoToClient_cf1.call(srvGoTo_cf1);
    // srvGoTo_cf2.request.goal.x = 1.25;
    // srvGoTo_cf2.request.goal.y = 0.0;
    // srvGoTo_cf2.request.goal.z = 1.5;
    // srvGoTo_cf2.request.yaw = 0.0;
    // srvGoTo_cf2.request.duration = ros::Duration(2.0);
    // GoToClient_cf2.call(srvGoTo_cf2);
    // // ros::Duration(2.0).sleep();
    // srvGoTo_cf3.request.goal.x = 0.0;
    // srvGoTo_cf3.request.goal.y = 1.0;
    // srvGoTo_cf3.request.goal.z = 1.0;
    // srvGoTo_cf3.request.yaw = 0.0;
    // srvGoTo_cf3.request.duration = ros::Duration(2.0);
    // GoToClient_cf3.call(srvGoTo_cf3);
    // srvGoTo_cf4.request.goal.x = 1.5;
    // srvGoTo_cf4.request.goal.y = 0.0;
    // srvGoTo_cf4.request.goal.z = 1.0;
    // srvGoTo_cf4.request.yaw = 0.0;
    // srvGoTo_cf4.request.duration = ros::Duration(2.0);
    // GoToClient_cf4.call(srvGoTo_cf4);
    // ros::Duration(2.0).sleep();

//     /* iLQR solver parameters */
//     constexpr size_t horizon=5;
//     constexpr int total_steps=300;
//     unsigned int tag1(1),tag2(2),tag3(3),tag4(4),tag5(5),
//                  tag6(6),tag7(7),tag8(8),tag9(9);
//
//     double time_step=0.2;
//
//
//     /*First Order 2 Drone Simulation*/
//
// 	cost<12,12> drone2_running_cost(Drone_First_Order_Cost::running_cost2,tag7);
// 	cost<12,12> drone2_terminal_cost(Drone_First_Order_Cost::terminal_cost2,tag8);
// 	dynamics<12,12> drone2_dynamics(Drone_First_Order_Dynamics::dynamics_2,tag9,time_step);
// 	iLQR<12,12,horizon>::input_trajectory u_init_drone2;
// 	iLQR<12,12,horizon> drone2_solver(drone2_running_cost,drone2_terminal_cost,drone2_dynamics);
// 	Eigen::Matrix<double,12,1> u_drone2=Eigen::Matrix<double,12,1>::Zero();
// 	Eigen::Matrix<double,12,1> x0_drone2=Eigen::Matrix<double,12,1>::Zero();
// 	Eigen::Matrix<double,12,1> x_goal_drone2;
// 	iLQR<12,12,horizon>::state_input_trajectory soln_drone2;
// 	iLQR<12,12,total_steps>::state_input_trajectory soln_drone2_rhc;
//
// 	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// 	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//     for (auto &item : u_init_drone2)
//         item = Eigen::Matrix<double,12,1>::Random()*5;
//
// //	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
// //	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
// 	clock_t t_start, t_end;
//
//
// 	double seconds;
// 	drone2_solver.set_MPC(u_init_drone2);
// 	int execution_steps=1;
// 	soln_drone2_rhc.first[0]=x0_drone2;
// 	// std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
// 	// std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
// 	t_start = clock();
//
//     x_goal_drone2 << 1.25, 0.0, 2.5, 0,0,0,
//                      0.0, 1.25,  2.5, 0,0,0;

    while (ros::ok())
//     // while (true)
	{
//
//         ros::spinOnce();
//
//
// 		x0_drone2 << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                      CFState_cf1.roll, CFState_cf1.pitch, CFState_cf1.yaw,
//                      CFState_cf2.x, CFState_cf2.y, CFState_cf2.z,
//                      CFState_cf2.roll, CFState_cf2.pitch, CFState_cf2.yaw;
//
//
// 		soln_drone2=drone2_solver.run_MPC(x0_drone2,
// 				x_goal_drone2, 50, 2, execution_steps);
//
//         /* Optimizer output logging */
//         std::string data;
//         for (int i = 0; i < horizon; ++i) {
//             for (int q = 0; q < 12; ++q) {
//                 data += std::to_string(soln_drone2.first[i][q]) + ",";
//             }
//
//         }
//         const auto p1 = std::chrono::system_clock::now();
//     	data += std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>(p1.time_since_epoch()).count()) + "\n";
//     	optimizerDatalogFile << data;
//
//         int idx_to_use = 3;
//
//         std::printf("\n#### SENDING WAYPOINT CF1  ");
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone2.first[idx_to_use][0], soln_drone2.first[idx_to_use][1], soln_drone2.first[idx_to_use][2]);
//         std::printf("\n#### SENDING WAYPOINT CF2   ");
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone2.first[idx_to_use][6], soln_drone2.first[idx_to_use][7], soln_drone2.first[idx_to_use][8]);
//
//         srvGoTo_cf1.request.goal.x = soln_drone2.first[idx_to_use][0];
//         srvGoTo_cf1.request.goal.y = soln_drone2.first[idx_to_use][1];
//         srvGoTo_cf1.request.goal.z = soln_drone2.first[idx_to_use][2];
//         srvGoTo_cf1.request.yaw = 0.0;
//         srvGoTo_cf1.request.duration = ros::Duration(0.8);
//         GoToClient_cf1.call(srvGoTo_cf1);
//
//         srvGoTo_cf2.request.goal.x = soln_drone2.first[idx_to_use][6];
//         srvGoTo_cf2.request.goal.y = soln_drone2.first[idx_to_use][7];
//         srvGoTo_cf2.request.goal.z = soln_drone2.first[idx_to_use][8];
//         srvGoTo_cf2.request.yaw = 0.0;
//         srvGoTo_cf2.request.duration = ros::Duration(0.8);
//         GoToClient_cf2.call(srvGoTo_cf2);
//
        int c = getch();   // call your non-blocking input function
        if (c == '\n') {
            std::cout << "########## LANDING ##########" << std::endl;
            break;
        }
        if (c == 'o') {
            std::cout << "########## RETURN TO ORIGIN ##########" << std::endl;
            srvGoTo_cf1.request.goal.x = -0.2;
            srvGoTo_cf1.request.goal.y = 1.1;
            srvGoTo_cf1.request.goal.z = 1.5;
            srvGoTo_cf1.request.yaw = 0.0;
            srvGoTo_cf1.request.duration = ros::Duration(3.0);
            GoToClient_cf1.call(srvGoTo_cf1);
            srvGoTo_cf2.request.goal.x = 0.55;
            srvGoTo_cf2.request.goal.y = 0.3;
            srvGoTo_cf2.request.goal.z = 0.5;
            srvGoTo_cf2.request.yaw = 0.0;
            srvGoTo_cf2.request.duration = ros::Duration(3.0);
            GoToClient_cf2.call(srvGoTo_cf2);
            ros::Duration(3.0).sleep();
            break;
        }
//
//
	}
// 	t_end = clock();
// ////
// //////
// ////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// ////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
// 	// write_file<12,12,total_steps>(state_path,input_path, soln_drone2_rhc);
// ////
// 	std::cout<<"finished"<<std::endl;
// 	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
// 	printf("Time: %f s\n", seconds);

    /*First Order 2 Drone Simulation Ends*/

    crazyflie_driver::Land srvLand;
    srvLand.request.duration = ros::Duration(3.0);
    landClient.call(srvLand);

    ros::spin();

    return 0;
}

/*############################################################################*/
/*############################################################################*/
/*############################################################################*/

// /*First Order Drone Simulation */
//
//
//
// 	cost<6,6> drone_running_cost(Drone_First_Order_Cost::running_cost,tag7);
// 	cost<6,6> drone_terminal_cost(Drone_First_Order_Cost::terminal_cost,tag8);
// 	dynamics<6,6> drone_dynamics(Drone_First_Order_Dynamics::dynamics,tag9,time_step);
// 	iLQR<6,6,horizon>::input_trajectory u_init_drone;
// 	iLQR<6,6,horizon> drone_solver(drone_running_cost,drone_terminal_cost,drone_dynamics);
// 	Eigen::Matrix<double,6,1> u_drone=Eigen::Matrix<double,6,1>::Zero();
// 	Eigen::Matrix<double,6,1> x0_drone=Eigen::Matrix<double,6,1>::Zero();
// 	Eigen::Matrix<double,6,1> x_goal_drone;
// 	iLQR<6,6,horizon>::state_input_trajectory soln_drone;
// 	iLQR<6,6,total_steps>::state_input_trajectory soln_drone_rhc;
//
// 	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// 		std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
// 	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
// 	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
// 		clock_t t_start, t_end;
// 	//
// 	////
// 		x0_drone<<0,0,0.5, 0,0,0;
// 	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
// 		double seconds;
// 		drone_solver.set_MPC(u_init_drone);
// 		int execution_steps=1;
// 		// soln_drone_rhc.first[0]=x0_drone;
// 		// std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
// 		// std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
// 		t_start = clock();
// 		// for(int i=0;i<total_steps;++i)
//
//         x_goal_drone<<1,1,1, 0,0,0;
//
//         while (ros::ok())
// 		{
//             ros::spinOnce();
//
// 	//		if (i>total_steps/3)
// 	//			x_goal_integrator<<0,0,0;
// 			// std::cout<<"iteration "<<i<<std::endl;
// 			// double scale=0.01;
// 			// auto term1=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// 			// auto term2=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term3=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term4=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term5=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term6=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term7=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term8=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term9=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term10=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//
//             x0_drone << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                         CFState_cf1.roll, CFState_cf1.pitch,  CFState_cf1.yaw;
//
// 			soln_drone=drone_solver.run_MPC(x0_drone,
// 					x_goal_drone, 5, execution_steps);
//
//             int idx_to_use = 1;
//
//             std::printf("\n#### SENDING WAYPOINT   ");
//             std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[idx_to_use][0], soln_drone.first[idx_to_use][1], soln_drone.first[idx_to_use][2]);
//
//             srvGoTo_cf1.request.goal.x = soln_drone.first[idx_to_use][0];
//             srvGoTo_cf1.request.goal.y = soln_drone.first[idx_to_use][1];
//             srvGoTo_cf1.request.goal.z = soln_drone.first[idx_to_use][2];
//             srvGoTo_cf1.request.yaw = 0.0;
//             srvGoTo_cf1.request.duration = ros::Duration(1.0);
//             // GoToClient_cf1.call(srvGoTo_cf1);
//
//             int c = getch();   // call your non-blocking input function
//             if (c == '\n') {
//                 std::cout << "########## LANDING ##########" << std::endl;
//                 break;
//             }
//             if (c == 'o') {
//                 std::cout << "########## RETURN TO ORIGIN ##########" << std::endl;
//                 srvGoTo_cf1.request.goal.x = 0.5;
//                 srvGoTo_cf1.request.goal.y = 0.5;
//                 srvGoTo_cf1.request.goal.z = 0.5;
//                 srvGoTo_cf1.request.yaw = 0.0;
//                 srvGoTo_cf1.request.duration = ros::Duration(3.0);
//                 GoToClient_cf1.call(srvGoTo_cf1);
//                 srvGoTo_cf2.request.goal.x = -0.5;
//                 srvGoTo_cf2.request.goal.y = -0.5;
//                 srvGoTo_cf2.request.goal.z = 0.5;
//                 srvGoTo_cf2.request.yaw = 0.0;
//                 srvGoTo_cf2.request.duration = ros::Duration(3.0);
//                 GoToClient_cf2.call(srvGoTo_cf2);
//                 ros::Duration(3.0).sleep();
//                 break;
//             }
// 	////
// 	////////		unsigned int microsecond = 1000000;
// 	////////		usleep(2 * microsecond);
// 	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
// 			// soln_drone_rhc.first[i+1]=soln_drone.first[1];
// 			// soln_drone_rhc.second[i]=soln_drone.second[0];
// 		}
// 		t_end = clock();
// 	////
// 	//////
// 	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// 	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
// 		// write_file<6,6,total_steps>(state_path,input_path, soln_drone_rhc);
// 	////
// 		std::cout<<"finished"<<std::endl;
// 		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
// 		printf("Time: %f s\n", seconds);
//
//
//
//
// /*First Order Drone Simulation Ends*/

// /* This part is single drone simulation*/
//
//     cost<12,4> running_cost_dr(Drone_Cost::running_cost,tag1);
//     cost<12,4> terminal_cost_dr(Drone_Cost::terminal_cost,tag2);
//     dynamics<12,4> dynamics_dr(Drone_Dynamics::dynamics,tag3,time_step);
//
//     iLQR<12,4,horizon> drone_solver(running_cost_dr,terminal_cost_dr,dynamics_dr);
//     Eigen::Matrix<double,4,1> u_drone=Eigen::Matrix<double,4,1>::Ones();
//     Eigen::Matrix<double,12,1> X0_drone=Eigen::Matrix<double,12,1>::Zero();
// ////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//     Eigen::Matrix<double,12,1> X_goal_drone;
//
//     u_drone=u_drone*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// //
//     iLQR<12,4,horizon>::input_trajectory u_init_drone;
//     std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
//     iLQR<12,4,horizon>::state_input_trajectory soln_drone;
//     iLQR<12,4,total_steps>::state_input_trajectory soln_drone_rhc;
//
//     clock_t t_start, t_end;
//
// //
//     double seconds;
//     drone_solver.set_MPC(u_init_drone);
//     int execution_steps=1;
//     soln_drone_rhc.first[0]=X0_drone;
//     // std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//     // std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//     t_start = clock();
//     // for(int i=0;i<total_steps;++i)
//
//
//     X_goal_drone<<-1.0,1.0,1.0, 0,0,0, 0,0,0, 0,0,0;
//
//
//     while (ros::ok())
//     {
//         // if (i>total_steps/3)
//         //     X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//         // std::cout<<"iteration "<<i<<std::endl;
//
//         ros::spinOnce();
// ////
//         X0_drone << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                     CFState_cf1.roll, CFState_cf1.pitch,  CFState_cf1.yaw,
//                     CFState_cf1.x_d, CFState_cf1.y_d, CFState_cf1.z_d,
//                     CFState_cf1.roll_d, CFState_cf1.pitch_d, CFState_cf1.yaw_d;
//
//         soln_drone=drone_solver.run_MPC(X0_drone, X_goal_drone, 20, execution_steps);
//
//         // ros::Duration(0.5).sleep();
// //
// //////		unsigned int microsecond = 1000000;
// //////		usleep(2 * microsecond);
// //		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//         // soln_drone_rhc.first[i+1]=soln_drone.first[1];
//         // soln_drone_rhc.second[i]=soln_drone.second[0];
//
//         // std::printf("\n\n");
//         // std::cout << X0_drone << std::endl;
//         // std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[4][0], soln_drone.first[4][1], soln_drone.first[4][2]);
//
//         std::printf("\n#### SENDING WAYPOINT   ");
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[4][0], soln_drone.first[4][1], soln_drone.first[4][2]);
//
//         srvGoTo_cf1.request.goal.x = soln_drone.first[4][0];
//         srvGoTo_cf1.request.goal.y = soln_drone.first[4][1];
//         srvGoTo_cf1.request.goal.z = soln_drone.first[4][2];
//         srvGoTo_cf1.request.yaw = 0.0;
//         srvGoTo_cf1.request.duration = ros::Duration(3.0);
//         GoToClient_cf1.call(srvGoTo_cf1);
//
//         int c = getch();   // call your non-blocking input function
//         if (c == '\n') {
//             std::cout << "########## LANDING ##########" << std::endl;
//             break;
//         }
//
//         // ros::spinOnce();
//
//     }
//     t_end = clock();
//
//
// ////
// ////
// //////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// //////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//     // write_file<12,4,total_steps>(state_path,input_path, soln_drone_rhc);
// //
//     std::cout<<"finished"<<std::endl;
//     seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//     printf("Time: %f s\n", seconds);
//
//
// /* End of single drone simulation*/

// cost<3,3> integrator_running_cost(Single_Integrator_Cost::running_cost,tag7);
// cost<3,3> integrator_terminal_cost(Single_Integrator_Cost::terminal_cost,tag8);
// dynamics<3,3> integrator_dynamics(Single_Integrator_3D::dynamics,tag9,time_step);
// iLQR<3,3,horizon>::input_trajectory u_init_integrator;
// iLQR<3,3,horizon> integrator_solver(integrator_running_cost,integrator_terminal_cost,integrator_dynamics);
// Eigen::Matrix<double,3,1> u_integrator=Eigen::Matrix<double,3,1>::Zero();
// Eigen::Matrix<double,3,1> x0_integrator=Eigen::Matrix<double,3,1>::Zero();
// Eigen::Matrix<double,3,1> x_goal_integrator;
// iLQR<3,3,horizon>::state_input_trajectory soln_integrator;
// iLQR<3,3,total_steps>::state_input_trajectory soln_integrator_rhc;
//
// //	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// //	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
// //	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
// //	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
// clock_t t_start, t_end;
//
// double seconds;
// integrator_solver.set_MPC(u_init_integrator);
// int execution_steps=1;
// soln_integrator_rhc.first[0]=x0_integrator;
// std::string state_path="/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/states.txt";
// std::string input_path="/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/inputs.txt";
// t_start = clock();
//
// /* Goal position of trajectory */
// x_goal_integrator << -1.0,1.0,1.0;
//
// while (ros::ok())
// {
//
//     /* Spin once for ROS callback to work */
//     ros::spinOnce();
//
//     /* populate state vector with current state */
//     x0_integrator << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z;
//
//     /* run iLQR algorithm */
// 	soln_integrator = integrator_solver.run_MPC(x0_integrator,
// 			                                    x_goal_integrator,
//                                                 20,
//                                                 execution_steps);
//
//     /* Write solution to file for debugging */
//     write_file<3,3,horizon>(state_path, input_path, soln_integrator);
// 	// soln_integrator_rhc.first[i+1]=soln_integrator.first[1];
// 	// soln_integrator_rhc.second[i]=soln_integrator.second[0];
//
//     // std::printf("\nX0_integrator: \n");
//     // std::cout << x0_integrator << std::endl;
//
//     std::printf("\n#### SENDING WAYPOINT");
//     std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_integrator.first[1][0], soln_integrator.first[1][1], soln_integrator.first[1][2]);
//
//     /* Populate GoTo command with waypoint and call service */
//     srvGoTo_cf1.request.goal.x = soln_integrator.first[1][0];
//     srvGoTo_cf1.request.goal.y = soln_integrator.first[1][1];
//     srvGoTo_cf1.request.goal.z = soln_integrator.first[1][2];
//     srvGoTo_cf1.request.yaw = 0.0;
//     srvGoTo_cf1.request.duration = ros::Duration(1.0);
//     GoToClient_cf1.call(srvGoTo_cf1);
//
//     // srvGoTo_cf2.request.goal.x = soln_integrator.first[4][0];
//     // srvGoTo_cf2.request.goal.y = soln_integrator.first[4][1];
//     // srvGoTo_cf2.request.goal.z = soln_integrator.first[4][2];
//     // srvGoTo_cf2.request.yaw = 0.0;
//     // srvGoTo_cf2.request.duration = ros::Duration(3.0);
//     // GoToClient_cf2.call(srvGoTo_cf2);
//
//
//     /* Check if <Enter> key way pressed */
//     int c = getch();
//     if (c == '\n') {
//         std::cout << "\t##### LANDING #####" << std::endl;
//         break;
//     }
//
//
// }
// t_end = clock();
//
//
// // soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// // write_file<12,4,horizon>(state_path,input_path, soln_drone);
// // write_file<3,3,total_steps>(state_path,input_path, soln_integrator_rhc);
//
//
// std::cout<<"finished"<<std::endl;
// seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
// printf("Time: %f s\n", seconds);

// /* This part is single drone simulation*/
//
//     cost<12,4> running_cost_dr(Drone_Cost::running_cost,tag1);
//     cost<12,4> terminal_cost_dr(Drone_Cost::terminal_cost,tag2);
//     dynamics<12,4> dynamics_dr(Drone_Dynamics::dynamics,tag3,time_step);
//
//     iLQR<12,4,horizon> drone_solver(running_cost_dr,terminal_cost_dr,dynamics_dr);
//     Eigen::Matrix<double,4,1> u_drone=Eigen::Matrix<double,4,1>::Ones();
//     Eigen::Matrix<double,12,1> X0_drone=Eigen::Matrix<double,12,1>::Zero();
// ////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//     Eigen::Matrix<double,12,1> X_goal_drone;
//
//     u_drone=u_drone*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// //
//     iLQR<12,4,horizon>::input_trajectory u_init_drone;
//     std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
//     iLQR<12,4,horizon>::state_input_trajectory soln_drone;
//     iLQR<12,4,total_steps>::state_input_trajectory soln_drone_rhc;
//
//     std::cout << "DEBUG 1" << std::endl;
//
//     clock_t t_start, t_end;
//
// //
//     double seconds;
//     drone_solver.set_MPC(u_init_drone);
//     int execution_steps=1;
//     soln_drone_rhc.first[0]=X0_drone;
//     // std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//     // std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//     t_start = clock();
//     // for(int i=0;i<total_steps;++i)
//
//
//     X_goal_drone<<1.0,1.0,1.5, 0,0,0, 0,0,0, 0,0,0;
//
//
//     while (ros::ok())
//     {
//         // if (i>total_steps/3)
//         //     X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//         // std::cout<<"iteration "<<i<<std::endl;
//
//         ros::spinOnce();
// ////
//         X0_drone << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                     CFState_cf1.roll, CFState_cf1.pitch,  CFState_cf1.yaw,
//                     CFState_cf1.x_d, CFState_cf1.y_d, CFState_cf1.z_d,
//                     CFState_cf1.roll_d, CFState_cf1.pitch_d, CFState_cf1.yaw_d;
//
//         soln_drone=drone_solver.run_MPC(X0_drone, X_goal_drone, 100, execution_steps);
//
//         // ros::Duration(0.5).sleep();
// //
// //////		unsigned int microsecond = 1000000;
// //////		usleep(2 * microsecond);
// //		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//         // soln_drone_rhc.first[i+1]=soln_drone.first[1];
//         // soln_drone_rhc.second[i]=soln_drone.second[0];
//
//         std::printf("\n\n");
//         std::cout << X0_drone << std::endl;
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[39][0], soln_drone.first[39][1], soln_drone.first[39][2]);
//
//         srvGoTo_cf2.request.goal.x = soln_drone.first[39][0];
//         srvGoTo_cf2.request.goal.y = soln_drone.first[39][1];
//         srvGoTo_cf2.request.goal.z = soln_drone.first[39][2];
//         srvGoTo_cf2.request.yaw = 0.0;
//         srvGoTo_cf2.request.duration = ros::Duration(5.0);
//         GoToClient_cf1.call(srvGoTo_cf2);
//
//         int c = getch();   // call your non-blocking input function
//         if (c == '\n') {
//             std::cout << "LANDING" << std::endl;
//             break;
//         }
//
//         // ros::spinOnce();
//
//     }
//     t_end = clock();
//
//
// ////
// ////
// //////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// //////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//     // write_file<12,4,total_steps>(state_path,input_path, soln_drone_rhc);
// //
//     std::cout<<"finished"<<std::endl;
//     seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//     printf("Time: %f s\n", seconds);
//
//
// /* End of single drone simulation*/

/*############################################################################*/
/*############################################################################*/
/*############################################################################*/

// /* Simple moving average filter */
// #if (USE_SMA_FILTER == 1)
//
//     /* Propogate state measurements */
//     for (int idx = 1; idx < SMA_WINDOW_LEN; ++idx) {
//         stateWindowBuf_cf1[idx] = stateWindowBuf_cf1[idx-1];
//         stateWindowBuf_cf2[idx] = stateWindowBuf_cf2[idx-1];
//     }
//
//     /* Append current state to head of buffer */
//     stateWindowBuf_cf1[0] = CFState_cf1;
//     stateWindowBuf_cf2[0] = CFState_cf2;
//
//     // /* Set all state variables to zero */
//     // memset(&CFState_cf1, 0x00, sizeof(stateVector));
//     // memset(&CFState_cf2, 0x00, sizeof(stateVector));
//
//     static stateVector tempState_cf1, tempState_cf2;
//
//     /* Accumulate state variables */
//     for (int idx = 0; idx < SMA_WINDOW_LEN; ++idx) {
//         tempState_cf1.x += stateWindowBuf_cf1[idx].x;
//     	tempState_cf1.y += stateWindowBuf_cf1[idx].y;
//     	tempState_cf1.z += stateWindowBuf_cf1[idx].z;
//         tempState_cf1.roll += stateWindowBuf_cf1[idx].roll;
//     	tempState_cf1.pitch += stateWindowBuf_cf1[idx].pitch;
//     	tempState_cf1.yaw += stateWindowBuf_cf1[idx].yaw;
//         tempState_cf1.x_d += stateWindowBuf_cf1[idx].x_d;
//     	tempState_cf1.y_d += stateWindowBuf_cf1[idx].y_d;
//     	tempState_cf1.z_d += stateWindowBuf_cf1[idx].z_d;
//     	tempState_cf1.roll_d += stateWindowBuf_cf1[idx].roll_d;
//     	tempState_cf1.pitch_d += stateWindowBuf_cf1[idx].pitch_d;
//     	tempState_cf1.yaw_d += stateWindowBuf_cf1[idx].yaw_d;
//
//         tempState_cf2.x += stateWindowBuf_cf2[idx].x;
//     	tempState_cf2.y += stateWindowBuf_cf2[idx].y;
//     	tempState_cf2.z += stateWindowBuf_cf2[idx].z;
//         tempState_cf2.roll += stateWindowBuf_cf2[idx].roll;
//     	tempState_cf2.pitch += stateWindowBuf_cf2[idx].pitch;
//     	tempState_cf2.yaw += stateWindowBuf_cf2[idx].yaw;
//         tempState_cf2.x_d += stateWindowBuf_cf2[idx].x_d;
//     	tempState_cf2.y_d += stateWindowBuf_cf2[idx].y_d;
//     	tempState_cf2.z_d += stateWindowBuf_cf2[idx].z_d;
//     	tempState_cf2.roll_d += stateWindowBuf_cf2[idx].roll_d;
//     	tempState_cf2.pitch_d += stateWindowBuf_cf2[idx].pitch_d;
//     	tempState_cf2.yaw_d += stateWindowBuf_cf2[idx].yaw_d;
//     }
//
//     CFState_cf1.x = tempState_cf1.x / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.y = tempState_cf1.y / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.z = tempState_cf1.z / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.x = tempState_cf1.roll / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.y = tempState_cf1.pitch / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.z = tempState_cf1.yaw / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.x_d = tempState_cf1.x_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.y_d = tempState_cf1.y_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.z_d = tempState_cf1.z_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.roll_d = tempState_cf1.roll_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.pitch_d = tempState_cf1.pitch_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.yaw_d = tempState_cf1.yaw_d / ((float) SMA_WINDOW_LEN);
//
//     // CFState_cf2.x = CFState_cf2.x / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.y = CFState_cf2.y / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.z = CFState_cf2.z / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.x = CFState_cf2.roll / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.y = CFState_cf2.pitch / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.z = CFState_cf2.yaw / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.x_d = CFState_cf2.x_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.y_d = CFState_cf2.y_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.z_d = CFState_cf2.z_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.roll_d = CFState_cf2.roll_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.pitch_d = CFState_cf2.pitch_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.yaw_d = CFState_cf2.yaw_d / ((float) SMA_WINDOW_LEN);
//
// #endif

// /* First order exponential low-pass filter */
// #if (USE_EXP_FILTER == 1)

    // float alpha = 0.9;
    //
    // CFState_cf1.x = (prevState_cf1.x * alpha) + (1.0 - alpha) * CFState_cf1.x;
    // CFState_cf1.y = (prevState_cf1.y * alpha) + (1.0 - alpha) * CFState_cf1.y;
    // CFState_cf1.z = (prevState_cf1.z * alpha) + (1.0 - alpha) * CFState_cf1.z;
    // CFState_cf1.roll = (prevState_cf1.roll * alpha) + (1.0 - alpha) * CFState_cf1.roll;
    // CFState_cf1.pitch = (prevState_cf1.pitch * alpha) + (1.0 - alpha) * CFState_cf1.pitch;
    // CFState_cf1.yaw = (prevState_cf1.yaw * alpha) + (1.0 - alpha) * CFState_cf1.yaw;
    // CFState_cf1.x_d = (prevState_cf1.x_d * alpha) + (1.0 - alpha) * CFState_cf1.x_d;
    // CFState_cf1.y_d = (prevState_cf1.y_d * alpha) + (1.0 - alpha) * CFState_cf1.y_d;
    // CFState_cf1.z_d = (prevState_cf1.z_d * alpha) + (1.0 - alpha) * CFState_cf1.z_d;
    // CFState_cf1.roll_d = (prevState_cf1.roll_d * alpha) + (1.0 - alpha) * CFState_cf1.roll_d;
    // CFState_cf1.pitch_d = (prevState_cf1.pitch_d * alpha) + (1.0 - alpha) * CFState_cf1.pitch_d;
    // CFState_cf1.yaw_d = (prevState_cf1.yaw_d * alpha) + (1.0 - alpha) * CFState_cf1.yaw_d;

    // CFState_cf2.x = (prevState_cf2.x * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.x;
    // CFState_cf2.y = (prevState_cf2.y * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.y;
    // CFState_cf2.z = (prevState_cf2.z * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.z;
    // CFState_cf2.roll = (prevState_cf2.roll * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.roll;
    // CFState_cf2.pitch = (prevState_cf2.pitch * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.pitch;
    // CFState_cf2.yaw = (prevState_cf2.yaw * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.yaw;
    // CFState_cf2.x_d = (prevState_cf2.x_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.x_d;
    // CFState_cf2.y_d = (prevState_cf2.y_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.y_d;
    // CFState_cf2.z_d = (prevState_cf2.z_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.z_d;
    // CFState_cf2.roll_d = (prevState_cf2.roll_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.roll_d;
    // CFState_cf2.pitch_d = (prevState_cf2.pitch_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.pitch_d;
    // CFState_cf2.yaw_d = (prevState_cf2.yaw_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.yaw_d;

    // prevState_cf1 = CFState_cf1;
    // prevState_cf2 = CFState_cf2;

// #endif

// /* Draw square trajectory */
//
// srvGoTo_cf2.request.goal.x = 0.0;
// srvGoTo_cf2.request.goal.y = 0.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 0.0;
// srvGoTo_cf2.request.goal.y = 1.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 1.0;
// srvGoTo_cf2.request.goal.y = 1.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 1.0;
// srvGoTo_cf2.request.goal.y = 0.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 0.0;
// srvGoTo_cf2.request.goal.y = 0.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();

// /* Control loop */
// while (ros::ok()) {
//
//     int c = getch();   // call your non-blocking input function
//     if (c == '\n') {
//         std::cout << "LANDING" << std::endl;
//         break;
//     }
//
//     ros::spinOnce();
//
// 	// position_cmd.x = 0;
// 	// position_cmd.y = 0;
// 	// position_cmd.z = 2.0;
// 	// position_cmd.yaw = 0.0;
// 	// pub.publish(position_cmd);
//
//
// }


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_ENUMERATE_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_ENUMERATE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/swar/functions/enumerate.hpp>
#include <boost/simd/include/functions/simd/make.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      ((target_ < simd_ < type64_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      return make<result_type>(a0, a0+1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      ((target_ < simd_ < type32_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      return make<result_type>(a0, a0+1, a0+2, a0+3);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      ((target_ < simd_ < type16_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      return make<result_type>(a0,a0+1,a0+2,a0+3,a0+4,a0+5,a0+6,a0+7);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      ((target_ < simd_ < type8_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      return make<result_type>( a0    , a0+1  , a0+2  , a0+3
                              , a0+4  , a0+5  , a0+6  , a0+7
                              , a0+8  , a0+9  , a0+10 , a0+11
                              , a0+12 , a0+13 , a0+14 , a0+15
                              );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_ < simd_ < double_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return _mm_set_pd(a0+a1,a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_ < simd_ <  single_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return _mm_set_ps(a0+3*a1,a0+2*a1,a0+a1,a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_ < simd_ < ints64_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return make<result_type>(a0, a0+a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_ < simd_ < ints32_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return _mm_set_epi32(a0+3*a1,a0+2*a1,a0+a1,a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_ < simd_ < ints16_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return _mm_set_epi16(a0+7*a1,a0+6*a1,a0+5*a1,a0+4*a1,a0+3*a1,a0+2*a1,a0+a1,a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_ < simd_ < ints8_<T>
                                                        , boost::simd::tag::sse_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return _mm_set_epi8( a0+15*a1,a0+14*a1,a0+13*a1,a0+12*a1,a0+11*a1,a0+10*a1
                                        , a0+9*a1, a0+8*a1, a0+7*a1, a0+6*a1, a0+5*a1, a0+4*a1
                                        , a0+3*a1, a0+2*a1, a0+a1,   a0
                                        );
    }
  };
} } }

#endif
#endif


//==============================================================================
//         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_META_FLOAT_HPP_INCLUDED
#define BOOST_SIMD_SDK_META_FLOAT_HPP_INCLUDED

#include <boost/simd/sdk/config/types.hpp>
#include <boost/simd/sdk/meta/real_c.hpp>

namespace boost { namespace simd { namespace meta
{
 template<boost::simd::uint32_t Value>
 struct single_ : real_c<boost::simd::uint32_t,Value> {};
} } }

#endif