// Copyright (C) 2013 Massachusetts Institute of Technology, Lincoln Laboratory
// License: Boost Software License   See LICENSE.txt for the full license.
// Authors: Davis E. King (davis@dlib.net)

#include "basic_morph.h"
#include <mitie/count_min_sketch.h>
#include <string>
#include <vector>
#include <map>
#include <dlib/set.h>
#include <mitie/approximate_substring_set.h>

using namespace std;
using namespace dlib;
using namespace mitie;

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

inline void populate_substr (
    const std::string& str,
    std::string& substr,
    long idx,
    unsigned long len
)
/*!
    ensures
        - extracts the substring of str which starts at str[idx] and is of length
          len and stores it into substr.  If the substring is a prefix or suffix
          then the * character is prepended or appended to the substring so that
          prefix and suffix substrings are identifiable.
!*/
{
    substr.clear();

    if (idx == 0)
        substr += '*';

    for (unsigned long i = idx; i < idx+len && i < str.size(); ++i)
        substr += str[i];

    if (idx+len == str.size())
        substr += '*';
}

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

void get_most_common_substrings (
    const std::map<std::string, unsigned long>& words,
    std::vector<std::pair<unsigned long, std::string> >& substrs,
    const unsigned long max_num_parts
)
/*!
    ensures
        - #substrs.size() <= max_num_parts
        - for all valid i:
            - #substrs[i].first == the number of times the substring #substrs[i].second was observed.
!*/
{
    substrs.clear();
    mitie::count_min_sketch counts(10000000);

    // Note that we use * to denote the start or end of a string.

    std::string temp;
    for (std::map<std::string, unsigned long>::const_iterator i = words.begin(); i != words.end(); ++i)
    {
        for (unsigned long k = 0; k < i->first.size(); ++k)
        {
            for (unsigned long l = 1; l <= 5; ++l)
            {
                if (k+l > i->first.size())
                    break;

                populate_substr(i->first, temp, k, l);
                if (temp.size() <= 1)
                    continue;

                counts.increment(temp);
            }
        }
    }

    dlib::set<std::string>::kernel_1a cur_words;

    std::priority_queue<std::pair<long,std::string> > best_parts;
    for (std::map<std::string, unsigned long>::const_iterator i = words.begin(); i != words.end(); ++i)
    {
        for (unsigned long k = 0; k < i->first.size(); ++k)
        {
            for (unsigned long l = 1; l <= 5; ++l)
            {
                if (k+l > i->first.size())
                    break;

                populate_substr(i->first, temp, k, l);
                if (temp.size() <= 1)
                    continue;

                if (cur_words.is_member(temp))
                    continue;

                const long hits = counts.get_count(temp);
                if (best_parts.size() < max_num_parts || hits > -best_parts.top().first)
                {
                    if (best_parts.size() >= max_num_parts)
                    {
                        cur_words.destroy(best_parts.top().second);
                        best_parts.pop();
                    }

                    best_parts.push(std::make_pair(-hits, temp));
                    cur_words.add(temp);
                }
            }
        }
    }

    while (best_parts.size() != 0)
    {
        substrs.push_back(make_pair(-best_parts.top().first, best_parts.top().second));
        best_parts.pop();
    }

}

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

void measure_substring_set_approximation_error(
    const std::vector<std::pair<unsigned long,std::string> >& substrs,
    const std::vector<dlib::uint16>& substr_ids,
    const approximate_substring_set& substrs_set
)
{
    int num_hits = 0;
    int num_misses = 0;
    std::string temp;
    std::vector<uint16> hits;
    for (unsigned long i = 0; i < substrs.size(); ++i)
    {
        temp.clear();
        if (substrs[i].second[0] != '*')
            temp += "_";

        for (unsigned long j = 0; j < substrs[i].second.size(); ++j)
        {
            if (substrs[i].second[j] != '*')
                temp += substrs[i].second[j];
        }

        if (substrs[i].second[substrs[i].second.size()-1] != '*')
            temp += "_";

        substrs_set.find_substrings(temp, hits);
        if (std::find(hits.begin(), hits.end(), substr_ids[i]) != hits.end())
        {
            ++num_hits;
            cout << "hit:  " << temp << endl;
        }
        else
        {
            ++num_misses;
            cout << "miss: " << temp << endl;
        }
    }

    cout << "num_hits: "<< num_hits << endl;
    cout << "num_misses: "<< num_misses << endl;
}

void basic_morph(const command_line_parser& parser)
{
    std::map<std::string, unsigned long> words;
    ifstream fin("top_word_counts.dat", ios::binary);
    deserialize(words, fin);
    cout << "num words: "<< words.size() << endl;

    std::vector<std::pair<unsigned long,std::string> > substrs;
    get_most_common_substrings(words, substrs, 20000);
    // sort the substrings so the least frequently used ones come first.  This way, when we
    // use the approximate substring set object it will most accurately represent the more
    // frequent substrings.
    std::sort(substrs.begin(), substrs.end());

    // We normally don't look for one character patterns, but as a special case we will
    // always look to see if a string has a single number anywhere in it.
    substrs.push_back(make_pair(1, "#"));

    ofstream fout_log("substrings.txt");
    approximate_substring_set substrs_set;
    std::vector<dlib::uint16> substr_ids;
    for (unsigned long i = 0; i < substrs.size(); ++i)
    {
        substr_ids.push_back(substrs_set.add_substring(substrs[i].second));
        fout_log << substrs[i].first << " \t" << substrs[i].second << "\n";
    }

    //measure_substring_set_approximation_error(substrs, substr_ids, substrs_set);

    ofstream fout("substring_set.dat", ios::binary);
    serialize(substrs_set, fout);
}

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


#pragma once

#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace arbor
{

class Lot
    {
    public:
                                Lot();
                                ~Lot();

        void                    setSeed(unsigned seed);
        double                  uniform();
        double                  normal();
        double                  logUniform();

        typedef boost::shared_ptr<Lot> SharedPtr;

    private:

        typedef boost::variate_generator<boost::mt19937 &, boost::uniform_real<> > uniform_variate_generator_t;
        typedef boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > normal_variate_generator_t;

        unsigned                        _seed;
        boost::mt19937                  _generator;
        uniform_variate_generator_t *   _uniform_variate_generator;
        normal_variate_generator_t  *   _normal_variate_generator;
    };

inline Lot::Lot() : _seed(0), _generator(1), _uniform_variate_generator(0)
    {
    _generator.seed(static_cast<unsigned int>(std::time(0)));
    _uniform_variate_generator = new uniform_variate_generator_t(_generator, boost::uniform_real<>(0,1));
    _normal_variate_generator = new normal_variate_generator_t(_generator, boost::normal_distribution<>(0,1));
    }

inline Lot::~Lot()
    {
    }

inline void Lot::setSeed(unsigned seed)
    {
    _seed = seed;
    _generator.seed(_seed > 0 ? _seed : static_cast<unsigned int>(std::time(0)));
    }

inline double Lot::uniform()
    {
    return (*_uniform_variate_generator)();
    }

inline double Lot::normal()
    {
    return (*_normal_variate_generator)();
    }

inline double Lot::logUniform()
    {
    double u = (*_uniform_variate_generator)();
    assert(u > 0.0);
    return std::log(u);
    }

}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#include "itkFiberCurvatureFilter.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <vtkDoubleArray.h>
#include <vtkPointData.h>
#include <boost/progress.hpp>

namespace itk{

FiberCurvatureFilter::FiberCurvatureFilter()
    : m_AngularDeviation(30)
    , m_Distance(5.0)
    , m_RemoveFibers(false)
{

}

FiberCurvatureFilter::~FiberCurvatureFilter()
{

}

void FiberCurvatureFilter::GenerateData()
{
    vtkSmartPointer<vtkPolyData> inputPoly = m_InputFiberBundle->GetFiberPolyData();
    vtkSmartPointer<vtkPoints> vtkNewPoints = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> vtkNewCells = vtkSmartPointer<vtkCellArray>::New();

    MITK_INFO << "Applying curvature threshold";
    boost::progress_display disp(inputPoly->GetNumberOfCells());
    for (int i=0; i<inputPoly->GetNumberOfCells(); i++)
    {
        ++disp;
        vtkCell* cell = inputPoly->GetCell(i);
        int numPoints = cell->GetNumberOfPoints();
        vtkPoints* points = cell->GetPoints();

        // calculate curvatures
        vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
        for (int j=0; j<numPoints; j++)
        {
            double dist = 0;
            int c = j;
            std::vector< vnl_vector_fixed< float, 3 > > vectors;
            vnl_vector_fixed< float, 3 > meanV; meanV.fill(0.0);
            while(dist<m_Distance/2 && c>1)
            {
                double p1[3];
                points->GetPoint(c-1, p1);
                double p2[3];
                points->GetPoint(c, p2);

                vnl_vector_fixed< float, 3 > v;
                v[0] = p2[0]-p1[0];
                v[1] = p2[1]-p1[1];
                v[2] = p2[2]-p1[2];
                dist += v.magnitude();
                v.normalize();
                vectors.push_back(v);
                if (c==j)
                    meanV += v;
                c--;
            }
            c = j;
            dist = 0;
            while(dist<m_Distance/2 && c<numPoints-1)
            {
                double p1[3];
                points->GetPoint(c, p1);
                double p2[3];
                points->GetPoint(c+1, p2);

                vnl_vector_fixed< float, 3 > v;
                v[0] = p2[0]-p1[0];
                v[1] = p2[1]-p1[1];
                v[2] = p2[2]-p1[2];
                dist += v.magnitude();
                v.normalize();
                vectors.push_back(v);
                if (c==j)
                    meanV += v;
                c++;
            }
            meanV.normalize();

            double dev = 0;
            for (auto vec : vectors)
            {
                double angle = dot_product(meanV, vec);
                if (angle>1.0)
                    angle = 1.0;
                if (angle<-1.0)
                    angle = -1.0;
                dev += acos(angle)*180/M_PI;
            }
            if (vectors.size()>0)
                dev /= vectors.size();

            if (dev<m_AngularDeviation)
            {
                double p[3];
                points->GetPoint(j, p);
                vtkIdType id = vtkNewPoints->InsertNextPoint(p);
                container->GetPointIds()->InsertNextId(id);
            }
            else
            {
                if (m_RemoveFibers)
                {
                    container = vtkSmartPointer<vtkPolyLine>::New();
                    break;
                }

                if (container->GetNumberOfPoints()>0)
                    vtkNewCells->InsertNextCell(container);
                container = vtkSmartPointer<vtkPolyLine>::New();
            }
        }
        if (container->GetNumberOfPoints()>0)
            vtkNewCells->InsertNextCell(container);
    }

    vtkSmartPointer<vtkPolyData> outputPoly = vtkSmartPointer<vtkPolyData>::New();
    outputPoly->SetPoints(vtkNewPoints);
    outputPoly->SetLines(vtkNewCells);
    m_OutputFiberBundle = FiberBundle::New(outputPoly);
}

}


/* -*- c++ -*- */
/*
 * Copyright 2015 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio 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
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include "ldpc_par_mtrx_encoder_impl.h"
#include <math.h>
#include <boost/assign/list_of.hpp>
#include <volk/volk.h>
#include <sstream>
#include <stdio.h>
#include <vector>
#include <algorithm>            // for std::reverse
#include <string.h>             // for memcpy

namespace gr {
  namespace fec {
    namespace code {

      generic_encoder::sptr
      ldpc_par_mtrx_encoder::make(std::string alist_file, unsigned int gap)
      {
        code::ldpc_H_matrix::sptr H_obj = code::ldpc_H_matrix::make(alist_file, gap);
        return make_H(H_obj);
      }

      generic_encoder::sptr
      ldpc_par_mtrx_encoder::make_H(const code::ldpc_H_matrix::sptr H_obj)
      {
        return generic_encoder::sptr
          (new ldpc_par_mtrx_encoder_impl(H_obj));
      }

      ldpc_par_mtrx_encoder_impl::ldpc_par_mtrx_encoder_impl(const code::ldpc_H_matrix::sptr H_obj)
        : generic_encoder("ldpc_par_mtrx_encoder")
      {
        // LDPC parity check matrix to use for encoding
        d_H = H_obj;

        d_rate = static_cast<double>(d_H->n())/static_cast<double>(d_H->k());

        // Set frame size to k, the # of bits in the information word
        // All buffers and settings will be based on this value.
        set_frame_size(d_H->k());
      }

      ldpc_par_mtrx_encoder_impl::~ldpc_par_mtrx_encoder_impl()
      {
      }

      int
      ldpc_par_mtrx_encoder_impl::get_output_size()
      {
        //return outputSize;
        return d_output_size;
      }

      int
      ldpc_par_mtrx_encoder_impl::get_input_size()
      {
        //return inputSize;
        return d_frame_size;
      }

      bool
      ldpc_par_mtrx_encoder_impl::set_frame_size(unsigned int frame_size)
      {
        bool ret = true;

        if(frame_size % d_H->k() != 0) {
          GR_LOG_ERROR(d_logger, boost::format("Frame size (%1% bits) must be a "
                                               "multiple of the information word "
                                               "size of the LDPC matrix (%2%).") \
                       % frame_size % (d_H->k()));
          throw std::runtime_error("ldpc_par_mtrx_encoder: cannot use frame size.");
        }

        d_frame_size = frame_size;

        d_output_size = static_cast<int>(d_rate * d_frame_size);

        return ret;
      }

      double
      ldpc_par_mtrx_encoder_impl::rate()
      {
        return d_rate;
      }

      void
      ldpc_par_mtrx_encoder_impl::generic_work(void *inbuffer, void *outbuffer)
      {
        // Populate the information word
        const unsigned char *in = (const unsigned char *)inbuffer;
        unsigned char *out = (unsigned char*)outbuffer;

        int j = 0;
        for(int i = 0; i < get_input_size(); i+=d_H->k()) {
          d_H->encode(&out[j], &in[i]);
          j += d_H->n();
        }
      }

    } /* namespace code */
  } /* namespace fec */
} /* namespace gr */


#include <math.h>
#include <ctime>
#include <iostream>
#include <string>
#include <vector>

// Boost includes
#include <boost/shared_ptr.hpp>

// ROS includes
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>

// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/point_cloud2_iterator.h>

// RWS includes
#include <rws2018_libs/team.h>
#include <rws2018_msgs/MakeAPlay.h>
#include <rws2018_msgs/GameQuery.h>

// defines
#define DEFAULT_TIME 0.05

using namespace std;
using namespace ros;
using namespace tf;

float randomizePosition(float *x, float *y)
{
  srand(static_cast<unsigned>(time(0)));
  *x = (((static_cast<float>(rand()) / static_cast<float>(RAND_MAX)) - 0.5) * 10);
  *y = (((static_cast<float>(rand()) / static_cast<float>(RAND_MAX)) - 0.5) * 10);
}

double x_sum = 0;
void cloud_cb(const sensor_msgs::PointCloud2Ptr &input)
{
  for (int i = 0; i < input->height; i++)
  {
    for (int j = 0; j < input->width; j++)
    {

      float x, y, z;
      x = y = z = 0;

      unsigned char *pt;

      pt = (input->data).data() + input->row_step * i + j * input->point_step;

      memcpy(&x, pt, 4);

      //memcpy(&y, pt + 4, 4);

      //memcpy(&z, pt + 8, 4);

      x_sum += x;
    }
  }

  ROS_INFO("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA %f", x_sum);
}

string my_pc_guess = "onion";

namespace rws_nsilva
{
class Player
{
public:
  Player(string name)
  {
    this->name = name;
  }

  string name;

  void setTeamName(int index)
  {
    if (index == 0)
      setTeamName("red");
    else if (index == 1)
      setTeamName("green");
    else if (index == 2)
      setTeamName("blue");
    else
      setTeamName("none");
  }

  void setTeamName(string team)
  {
    if (team == "red" || team == "green" || team == "blue")
    {
      team_name = team;
    }
    else
      ROS_ERROR("For player %s team name %s is invalid.", this->name.c_str(), team.c_str());
  }

  string getTeamName(void)
  {
    return this->team_name;
  }

private:
  string team_name;
};

class MyPlayer : public Player
{
public:
  boost::shared_ptr<Team> red_team;
  boost::shared_ptr<Team> green_team;
  boost::shared_ptr<Team> blue_team;

  boost::shared_ptr<Team> my_team;
  boost::shared_ptr<Team> my_preys;
  boost::shared_ptr<Team> my_hunters;

  TransformBroadcaster br;
  Transform transform;

  boost::shared_ptr<Subscriber> sub;
  NodeHandle nh;
  Publisher vis_pub;
  boost::shared_ptr<ros::ServiceServer> game_query_srv;
  TransformListener listener;
  ros::Subscriber sub_pc;

  float x, y;

  MyPlayer(string argin_name, string argin_team) : Player(argin_name)
  {
    red_team = boost::shared_ptr<Team>(new Team("red"));
    green_team = boost::shared_ptr<Team>(new Team("green"));
    blue_team = boost::shared_ptr<Team>(new Team("blue"));

    vis_pub = nh.advertise<visualization_msgs::Marker>("/bocas", 0);

    game_query_srv = boost::shared_ptr<ros::ServiceServer>(new ros::ServiceServer());
    *game_query_srv = nh.advertiseService("/" + name + "/game_query", &MyPlayer::respondToGameQuery, this);
    // Create a ROS subscriber for the input point cloud
    sub_pc = nh.subscribe("/object_point_cloud", 1, cloud_cb);

    if (red_team->playerBelongsToTeam(name))
    {
      my_team = red_team;
      my_preys = green_team;
      my_hunters = blue_team;
    }
    else if (green_team->playerBelongsToTeam(name))
    {
      my_team = green_team;
      my_preys = blue_team;
      my_hunters = red_team;
    }
    else if (blue_team->playerBelongsToTeam(name))
    {
      my_team = blue_team;
      my_preys = red_team;
      my_hunters = green_team;
    }

    struct timeval t1;
    gettimeofday(&t1, NULL);
    srand(t1.tv_usec);
    double start_x = ((double)rand() / (double)RAND_MAX) * 10 - 5;
    double start_y = ((double)rand() / (double)RAND_MAX) * 10 - 5;
    printf("start_x=%f start_y=%f\n", start_x, start_y);
    warp(start_x, start_y, M_PI / 2);

    sub = boost::shared_ptr<Subscriber>(new Subscriber());
    *sub = nh.subscribe("/make_a_play", 100, &MyPlayer::move, this);

    setTeamName(argin_team);

    printReport();
  }

  void warp(double x, double y, double alfa)
  {
    transform.setOrigin(Vector3(x, y, 0.0));
    transform.setOrigin(Vector3(x, y, 0.0));

    Quaternion q;
    q.setRPY(0, 0, alfa);
    transform.setRotation(q);

    br.sendTransform(StampedTransform(transform, Time::now(), "world", "nsilva"));
    ROS_INFO("%s: Warping to x=%f, y=%f, alpha=%f", name.c_str(), x, y, alfa);
  }

  bool respondToGameQuery(rws2018_msgs::GameQuery::Request &req,
                          rws2018_msgs::GameQuery::Response &res)
  {
    ROS_WARN("I am %s and I am responding to a service request!", name.c_str());

    res.resposta = "nao percebo nada disto";
    return true;
  }

  string guessFunc()
  {
    return "";
  }

  void move(const rws2018_msgs::MakeAPlay::ConstPtr &msg)
  {
    double x = transform.getOrigin().x();
    double y = transform.getOrigin().y();
    double a = 0;

    // AI PART
    double displacement = 6; // computed using AI
    string closestPlayer = getClosestPlayer(msg);

    my_pc_guess = guessFunc();
    ROS_INFO("CLOSEST %s", closestPlayer.c_str());
    if (closestPlayer[closestPlayer.length() - 1] == '@')
    {
      closestPlayer = closestPlayer.substr(0, closestPlayer.size() - 1);
      ROS_INFO("Running away from %s", closestPlayer.c_str());
      closestPlayer = "world";
    }
    else
    {
      ROS_INFO("Chasing %s", closestPlayer.c_str());
    }

    // if outside arena, get away
    if (x > 5 || x < -5 || y > 5 || y < -5)
    {
      closestPlayer = "world";
    }
    double delta_alpha = getAngleToPLayer(closestPlayer);

    string marker_string = "Distance to " + closestPlayer + ": ";

    double distanceToPlayer = getDistancetoPlayer(closestPlayer);

    char distance_string[32];
    sprintf(distance_string, "%f", distanceToPlayer);

    marker_string = marker_string + distance_string;

    showMarker(marker_string);
    if (isnan(delta_alpha))
      delta_alpha = 0;

    // CONSTRAINSTS PART
    double dist_max = msg->turtle;
    double dist_with_constrainsts;

    if (displacement > dist_max)
      displacement = dist_max;

    double delta_alpha_max = M_PI / 30;
    if (fabs(delta_alpha) > fabs(delta_alpha_max))
      delta_alpha = delta_alpha_max * delta_alpha / fabs(delta_alpha);

    Transform my_move_transform;
    my_move_transform.setOrigin(Vector3(displacement, 0.0, 0.0));

    // Update position
    transform.setOrigin(Vector3(x, y, 0.0));

    Quaternion q;
    q.setRPY(0, 0, delta_alpha);
    my_move_transform.setRotation(q);

    transform = transform * my_move_transform;

    br.sendTransform(StampedTransform(transform, Time::now(), "world", "nsilva"));
  }

  double getAngleToPLayer(string other_player, double time_to_wait = DEFAULT_TIME)
  {
    StampedTransform t; // The transform object
    Time now = Time(0); // get the latest transform received

    try
    {
      listener.waitForTransform("nsilva", other_player, now, Duration(time_to_wait));
      listener.lookupTransform("nsilva", other_player, now, t);
    }
    catch (TransformException &ex)
    {
      ROS_ERROR("%s", ex.what());
      return NAN;
    }

    return atan2(t.getOrigin().y(), t.getOrigin().x());
  }

  double getDistancetoPlayer(string other_player, double time_to_wait = DEFAULT_TIME)
  {
    StampedTransform t; // The transform object
    Time now = Time(0); // get the latest transform received

    try
    {
      listener.waitForTransform("nsilva", other_player, now, Duration(time_to_wait));
      listener.lookupTransform("nsilva", other_player, now, t);
    }
    catch (TransformException &ex)
    {
      ROS_ERROR("%s", ex.what());
      return NAN;
    }

    return sqrt(pow(t.getOrigin().x(), 2) + pow(t.getOrigin().y(), 2));
  }

  string getClosestPlayer(const rws2018_msgs::MakeAPlay::ConstPtr &msg)
  {
    double distance = 99;
    string player;

    for (int i = 0; i < msg->red_alive.size(); i++)
    {
      if (distance > getDistancetoPlayer(msg->red_alive[i]))
      {
        distance = getDistancetoPlayer(msg->red_alive[i]);
        player = msg->red_alive[i];
      }
    }

    for (int i = 0; i < msg->green_alive.size(); i++)
    {
      if (distance > getDistancetoPlayer(msg->green_alive[i]))
      {
        distance = getDistancetoPlayer(msg->green_alive[i]);
        player = msg->green_alive[i] + "@";
      }
    }

    return player;
  }

  void showMarker(string text)
  {
    visualization_msgs::Marker marker;
    marker.header.frame_id = "nsilva";
    marker.header.stamp = Time();
    marker.ns = "my_namespace";
    marker.id = 0;
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.x = 0;
    marker.pose.position.y = -0.3;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.text = text;
    marker.scale.z = 0.4;
    marker.color.a = 1.0; // Don't forget to set the alpha!
    marker.color.r = 0;
    marker.color.g = 0;
    marker.color.b = 1;

    vis_pub.publish(marker);
  }

  void printReport()
  {
    ROS_INFO("My name is %s, my team is %s and I am gonna catch you all >:)", name.c_str(), getTeamName().c_str());
  }
};
}

int main(int argc, char **argv)
{
  init(argc, argv, "nsilva");
  NodeHandle nh;
  Rate loop_rate(11);

  // Creating an instance of class Player
  rws_nsilva::MyPlayer my_player("nsilva", "blue");

  spin();
}


#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/Joy.h>
#include <math.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <linux/input-event-codes.h>

#include "origarm_ros/Command_ABL.h"
#include "origarm_ros/Seg_ABL.h"
#include "origarm_ros/SegOpening.h"
#include "origarm_ros/Command_Pre_Open.h"
#include "origarm_ros/Command_Position.h"
#include "origarm_ros/keynumber.h"
#include "origarm_ros/modenumber.h"
#include "origarm_ros/segnumber.h"

#include "myGlobalData.h"
#include "myFunction.h"
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using Eigen::Matrix3f;
using Eigen::Matrix4f;
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::VectorXf;

using namespace std;

//save into files
static int save_flag = 0;
string trajPointFileName = "";
ofstream savedata;

//joystick mapping
static float joyLx;
static float joyLy;
static float joyRx;
static float joyRy;
static float joyLT;
static float joyRT;
static int joyLB; //mode[5]
static int joyRB; //mode[4]
static int joyX;  //mode[2]
static int joyY;  //mode[3]
static int joyA;  //mode[0]
static int joyB;  //mode[1]
static int joyCrossY;
static int joyCrossX;
static int joyUp;
static int joyDown;
static int joyLeft;
static int joyRight;

static int last_joyRB;
static int last_joyLB;
static int last_joyX;
static int last_joyY;
static int last_joyA;
static int last_joyB;
static int last_joyUp;
static int last_joyDown;
static int last_joyLeft;
static int last_joyRight;

static int enable;
static int disable;
static int last_enable;
static int last_disable;
static int status;

//Write ABL
static int segNumber;
static float alpha;
static float beta;
static float length = g_length0;
static float segAlpha_2[2];
static float segBeta_2[2];
static float segLength_2[2];
static float segAlpha_[3];
static float segBeta_[3];
static float segLength_[3];
static float segAlpha[9];
static float segBeta[9];
static float segLength[9];

static float a_scale = 0.001;
static float b_scale = 0.002;
static float l_scale = 0.00003;

static float l_max = 0.08;
static float l_min = 0.03;
static float a_max = 30/180.0f*M_PI;//M_PI / 2;
static float a_min = -30/180.0f*M_PI;//-M_PI / 2;
static float b_max = M_PI;
static float b_min = -M_PI;

//write Opening
static float belloConfigurationR = 0.06;
static float Rmax = 1.0;

static float rawAngle;
static float rawAmplitude;
static float rawAmplitudeMax;
static float angleCommand;
static float amplitudeCommand;
static float bellowProjection[6];
static float bellowConfigurationPx[6];
static float bellowConfigurationPy[6];
static float openingBase;
static float OpeningResult[6];

//Write XYZ unit:m
static float x = g_originX;
static float y = g_originY;
static float z = g_originZ;
static float segx_ = g_originX;
static float segy_ = g_originY;
static float segz_ = g_originZ * 6;
static float segqx_ = 1;
static float segqy_ = 0;
static float segqz_ = 0;
static float segqw_ = 1;

static float x_scale = 0.00001;
static float y_scale = 0.00001;
static float z_scale = 0.0001;

static float x_max = 0.06;
static float x_min = -0.06;
static float y_max = 0.06;
static float y_min = -0.06;

static float z_max = 0.08;
static float z_min = 0.01;
static float z_max9 = z_max * 9;
static float z_min9 = z_min * 9;
static float z_max6 = z_max * 6;
static float z_min6 = z_min * 6;

//Internal parameters
//ABL calculated by XYZ
static float alphad;
static float betad;
static float lengthd;
static float segAlphad_[3];
static float segBetad_[3];
static float segLengthd_[3];
static float segAlphad[9];
static float segBetad[9];
static float segLengthd[9];

origarm_ros::Command_ABL Cmd_ABL;
origarm_ros::Command_Position Cmd_Position;
origarm_ros::modenumber modenumber;
origarm_ros::modenumber modenumberLast;
origarm_ros::segnumber segnumber;
origarm_ros::segnumber segnumberLast;

//control mode
//mode[0]: 1 abl; mode[1]: 3 abl; mode[2]: 9 abl; mode[3]: 1 xyz; mode[4]: 3 xyz; mode[5]:
//mode[0]: joyA;  mode[1]: joyB;  mode[2]: joyX;  mode[3]: joyY;  mode[4]: joyRB; mode[5]:

//moddifed version
//mode[0]: 1 abl; mode[1]: 3 abl; mode[2]: 9 abl; mode[3]: 2 abl; mode[4]: 3 xyz; mode[5]:
//mode[0]: joyA;  mode[1]: joyB;  mode[2]: joyX;  mode[3]: joyY;  mode[4]: joyRB; mode[5]:
int mode;

std::string getTimeString()
{
	time_t rawtime;
	struct tm *timeinfo;
	char buffer[100];

	time(&rawtime);
	timeinfo = localtime(&rawtime);
	struct timeval time_now
	{
	};
	gettimeofday(&time_now, nullptr);
	time_t msecs_time = time_now.tv_usec;
	time_t msec = (msecs_time / 1000) % 1000;

	strftime(buffer, 100, "%G_%h_%d_%H_%M_%S", timeinfo);
	std::string ret = buffer;
	//std::string ret=ret1+"_"+std::to_string(msec);
	return ret;
}

//keyboard callback
void keyCallback(const origarm_ros::keynumber &key)
{
	int i = 0;
	switch (key.keycodePressed)
	{
	case KEY_0:
		segNumber = 0;
		break;
	case KEY_1:
		segNumber = 1;
		break;
	case KEY_2:
		segNumber = 2;
		break;
	case KEY_3:
		segNumber = 3;
		break;
	case KEY_4:
		segNumber = 4;
		break;
	case KEY_5:
		segNumber = 5;
		break;
	case KEY_6:
		segNumber = 6;
		break;
	case KEY_7:
		segNumber = 7;
		break;
	case KEY_8:
		segNumber = 8;
		break;
	case KEY_9:
		segNumber = 9;
		break;
	}
}

//joystick callback
void joyCallback(const sensor_msgs::Joy::ConstPtr &joy)
{
	//joystick mapping
	joyLx = joy->axes[0];
	joyLy = joy->axes[1];
	joyRx = joy->axes[3];
	joyRy = joy->axes[4];
	joyLT = joy->axes[2];
	joyRT = joy->axes[5];

	//enable = joy->buttons[0];
	disable = joy->buttons[6]; //back

	joyA = joy->buttons[0];
	joyB = joy->buttons[1];
	joyX = joy->buttons[2];
	joyY = joy->buttons[3];
	joyLB = joy->buttons[4];
	joyRB = joy->buttons[5];

	joyCrossX = joy->axes[6];
	joyCrossY = joy->axes[7];

	if (joyCrossX == 1)
	{
		joyLeft = 1;
	}
	else if (joyCrossX == -1)
	{
		joyRight = 1;
	}
	else
	{
		joyLeft = 0;
		joyRight = 0;
	}

	if (joyCrossY == 1)
	{
		joyUp = 1;
	}
	else if (joyCrossY == -1)
	{
		joyDown = 1;
	}
	else
	{
		joyUp = 0;
		joyDown = 0;
	}

	if (joyA == 1 && last_joyA == 0)
	{
		mode = 0;
	}
	else if (joyB == 1 && last_joyB == 0)
	{
		mode = 1;
	}
	else if (joyY == 1 && last_joyY == 0)
	{
		mode = 3;
	}
	else if (joyX == 1 && last_joyX == 0)
	{
		mode = 2;
	}
	else if (joyRB == 1 && last_joyRB == 0)
	{
		mode = 4;
	}

	//save into files
	if (joyLB == 1 && last_joyLB == 0)
	{
		save_flag = 1;
	}
	else
	{
		save_flag = 0;
	}

	//only when joyRT && joyLT pressed together, joystick starts to control
	if (joyLT == -1 && joyRT == -1)
	{
		enable = 1;
	}
	else
	{
		enable = 0;
	}

	if (enable == 1 && last_enable == 0)
	{
		status = 1;
	}
	else if (disable == 1 && last_disable == 0)
	{
		status = 0;
	}

	last_enable = enable;
	last_disable = disable;

	last_joyA = joyA;
	last_joyB = joyB;
	last_joyX = joyX;
	last_joyY = joyY;
	last_joyUp = joyUp;
	last_joyDown = joyDown;
	last_joyLeft = joyLeft;
	last_joyRight = joyRight;
	last_joyRB = joyRB;
	last_joyLB = joyLB;
}

//ABL from ik
void ABLCallback(const origarm_ros::Command_ABL &msg)
{
	alphad = msg.segment[0].A;
	betad = msg.segment[0].B;
	lengthd = msg.segment[0].L;

	for (int i = 0; i < 3; i++)
	{
		segAlphad_[i] = msg.segment[int(i * 2)].A;
		segBetad_[i] = msg.segment[int(i * 2)].B;
		segLengthd_[i] = msg.segment[int(i * 2)].L;
	}
}

float constrain2PI(float s) // constrian angle to [-pi,pi)
{
	if (s >= M_PI)
	{
		s = s - 2 * M_PI;
	}
	else if (s < -M_PI)
	{
		s = s + 2 * M_PI;
	}

	return s;
}

//Joystick->ABL (joyLy->alpha, joyRx->beta, joyLT & joyRT->length)
void writeABL1(int joystickFLag)
{
	if (joystickFLag == 1)
	{
		if (joyLy > 0.05)
		{
			alpha = alpha + a_scale;
		}
		else if (joyLy < -0.05)
		{
			alpha = alpha - a_scale;
		}

		if (joyRx > 0.05)
		{
			if (joyRx > 0.5)
			{
				beta = beta + 2 * b_scale;
			}
			else
			{
				beta = beta + b_scale;
			}
		}
		else if (joyRx < -0.05)
		{
			if (joyRx < -0.5)
			{
				beta = beta - 2 * b_scale;
			}
			else
			{
				beta = beta - b_scale;
			}
		}

		if (joyLT != 1 && joyRT != 1)
		{
		}
		else if (abs(joyLT - 1) > 0.05)
		{
			if (abs(joyLT - 1) > 1)
			{
				length = length + 2 * l_scale;
			}
			else
			{
				length = length + l_scale;
			}
		}
		else if (abs(joyRT - 1) > 0.05)
		{
			if (abs(joyRT - 1) > 0.05)
			{
				if (abs(joyRT - 1) > 1)
				{
					length = length - 2 * l_scale;
				}
				else
				{
					length = length - l_scale;
				}
			}
		}
	}
	else if (joystickFLag == 0) //calculate  ABL from xyz
	{
		alpha = alphad;
		beta = betad;
		length = lengthd;
	}

	beta = constrain2PI(beta);

	alpha = CONSTRAIN(alpha, a_min, a_max);
	beta = CONSTRAIN(beta, b_min, b_max);
	length = CONSTRAIN(length, l_min, l_max);
}

void writeABL2(int joystickFLag)
{
	if (joystickFLag == 1)
	{
		if (joyLy > 0.05)
		{
			segAlpha_2[segNumber] = segAlpha_2[segNumber] + a_scale;
		}
		else if (joyLy < -0.05)
		{
			segAlpha_2[segNumber] = segAlpha_2[segNumber] - a_scale;
		}

		if (joyRx > 0.05)
		{
			if (joyRx > 0.5)
			{
				segBeta_2[segNumber] = segBeta_2[segNumber] + 2 * b_scale;
			}
			else
			{
				segBeta_2[segNumber] = segBeta_2[segNumber] + b_scale;
			}
		}
		else if (joyRx < -0.05)
		{
			if (joyRx < -0.5)
			{
				segBeta_2[segNumber] = segBeta_2[segNumber] - 2 * b_scale;
			}
			else
			{
				segBeta_2[segNumber] = segBeta_2[segNumber] - b_scale;
			}
		}

		if (joyLT != 1 && joyRT != 1)
		{
		}
		else if (abs(joyLT - 1) > 0.05)
		{
			if (abs(joyLT - 1) > 1)
			{
				segLength_2[segNumber] = segLength_2[segNumber] + 2 * l_scale;
			}
			else
			{
				segLength_2[segNumber] = segLength_2[segNumber] + l_scale;
			}
		}
		else if (abs(joyRT - 1) > 0.05)
		{
			if (abs(joyRT - 1) > 0.05)
			{
				if (abs(joyRT - 1) > 1)
				{
					segLength_2[segNumber] = segLength_2[segNumber] - 2 * l_scale;
				}
				else
				{
					segLength_2[segNumber] = segLength_2[segNumber] - l_scale;
				}
			}
		}
	}
	else // not consider the consistency when switching mode, once switching mode from / to mode[2], 2 abl, it will remain its last pose
	{
		segAlpha_2[segNumber] = 0;
		segBeta_2[segNumber] = 0;
		segLength_2[segNumber] = 0.055 * 3;
	}

	for (int i = 0; i < 2; i++)
	{
		segBeta_2[i] = constrain2PI(segBeta_2[i]);
		segAlpha_2[i] = CONSTRAIN(segAlpha_2[i], a_min * 3, a_max * 3);
		segBeta_2[i] = CONSTRAIN(segBeta_2[i], b_min, b_max);
		segLength_2[i] = CONSTRAIN(segLength_2[i], l_min * 3, l_max * 3);
	}
}

void writeABL3(int joystickFLag)
{
	if (joystickFLag == 1)
	{
		if (segNumber < 3)
		{
			if (joyLy > 0.05)
			{
				segAlpha_[segNumber] = segAlpha_[segNumber] + a_scale;
			}
			else if (joyLy < -0.05)
			{
				segAlpha_[segNumber] = segAlpha_[segNumber] - a_scale;
			}

			if (joyRx > 0.05)
			{
				if (joyRx > 0.5)
				{
					segBeta_[segNumber] = segBeta_[segNumber] + 2 * b_scale;
				}
				else
				{
					segBeta_[segNumber] = segBeta_[segNumber] + b_scale;
				}
			}
			else if (joyRx < -0.05)
			{
				if (joyRx < -0.5)
				{
					segBeta_[segNumber] = segBeta_[segNumber] - 2 * b_scale;
				}
				else
				{
					segBeta_[segNumber] = segBeta_[segNumber] - b_scale;
				}
			}

			if (joyLT != 1 && joyRT != 1)
			{
			}
			else if (abs(joyLT - 1) > 0.05)
			{
				if (abs(joyLT - 1) > 1)
				{
					segLength_[segNumber] = segLength_[segNumber] + 2 * l_scale;
				}
				else
				{
					segLength_[segNumber] = segLength_[segNumber] + l_scale;
				}
			}
			else if (abs(joyRT - 1) > 0.05)
			{
				if (abs(joyRT - 1) > 0.05)
				{
					if (abs(joyRT - 1) > 1)
					{
						segLength_[segNumber] = segLength_[segNumber] - 2 * l_scale;
					}
					else
					{
						segLength_[segNumber] = segLength_[segNumber] - l_scale;
					}
				}
			}
		}
	}
	else if (joystickFLag == 0)
	{
		for (int i = 0; i < 3; i++)
		{
			segAlpha_[i] = segAlphad_[i] * 2;
			segBeta_[i] = segBetad_[i];
			segLength_[i] = segLengthd_[i] * 2;
		}
	}

	for (int i = 0; i < 3; i++)
	{
		segBeta_[i] = constrain2PI(segBeta_[i]);
		segAlpha_[i] = CONSTRAIN(segAlpha_[i], a_min * 2, a_max * 2);
		segBeta_[i] = CONSTRAIN(segBeta_[i], b_min, b_max);
		segLength_[i] = CONSTRAIN(segLength_[i], l_min * 2, l_max * 2);
	}
}

void writeABL9(int joystickFLag)
{
	if (joystickFLag == 1) // calculated from joystick
	{
		if (joyLy > 0.05)
		{
			segAlpha[segNumber] = segAlpha[segNumber] + a_scale;
		}
		else if (joyLy < -0.05)
		{
			segAlpha[segNumber] = segAlpha[segNumber] - a_scale;
		}

		if (joyRx > 0.05)
		{
			if (joyRx > 0.5)
			{
				segBeta[segNumber] = segBeta[segNumber] + 2 * b_scale;
			}
			else
			{
				segBeta[segNumber] = segBeta[segNumber] + b_scale;
			}
		}
		else if (joyRx < -0.05)
		{
			if (joyRx < -0.5)
			{
				segBeta[segNumber] = segBeta[segNumber] - 2 * b_scale;
			}
			else
			{
				segBeta[segNumber] = segBeta[segNumber] - b_scale;
			}
		}

		if (joyLT != 1 && joyRT != 1)
		{
		}
		else if (abs(joyLT - 1) > 0.05)
		{
			if (abs(joyLT - 1) > 1)
			{
				segLength[segNumber] = segLength[segNumber] + 2 * l_scale;
			}
			else
			{
				segLength[segNumber] = segLength[segNumber] + l_scale;
			}
		}
		else if (abs(joyRT - 1) > 0.05)
		{
			if (abs(joyRT - 1) > 0.05)
			{
				if (abs(joyRT - 1) > 1)
				{
					segLength[segNumber] = segLength[segNumber] - 2 * l_scale;
				}
				else
				{
					segLength[segNumber] = segLength[segNumber] - l_scale;
				}
			}
		}
	}
	else if (joystickFLag == 0) //calcuated from XYZ_9 control mode
	{
		for (int i = 0; i < 9; i++)
		{
			segAlpha[i] = segAlphad[i];
			segBeta[i] = segBetad[i];
			segLength[i] = segLengthd[i];
		}
	}
	else if (joystickFLag == 2) //calculated from ABL_3 control mode
	{
		for (int i = 0; i < 6; i++)
		{
			segAlpha[i] = segAlpha_[int(i / 2)] / 2;
			segBeta[i] = segBeta_[int(i / 2)];
			segLength[i] = segLength_[int(i / 2)] / 2;
		}
	}

	for (int i = 0; i < 9; i++)
	{
		segBeta[i] = constrain2PI(segBeta[i]);
		segAlpha[i] = CONSTRAIN(segAlpha[i], a_min, a_max);
		segBeta[i] = CONSTRAIN(segBeta[i], b_min, b_max);
		segLength[i] = CONSTRAIN(segLength[i], l_min, l_max);
	}
}

//Joystick->Opening
void WriteOpening()
{
	//openingBase for elongation
	if (joyLy > 0)
	{
		openingBase = 0.5;
	}
	else if (joyLy < 0)
	{
		openingBase = -0.8;
	}
	else
	{
		openingBase = 0;
	}

	//opening for rotation
	if (joyRx == 0 && joyRy == 0)
	{
		rawAngle = 0;
		rawAmplitude = 0;
		rawAmplitudeMax = Rmax;
	}
	else
	{
		rawAngle = atan2(joyRx, joyRy);
		rawAmplitude = sqrt(joyRx * joyRx + joyRy * joyRy);

		if (rawAmplitude > 0)
		{
			if (rawAngle < M_PI * 0.25 || rawAngle > M_PI * 0.75)
			{
				rawAmplitudeMax = Rmax / cos(rawAngle);
			}
			else
			{
				rawAmplitudeMax = Rmax / sin(rawAngle);
			}
		}
		else
		{
			if (rawAngle > -M_PI * 0.25 || rawAngle < -M_PI * 0.75)
			{
				rawAmplitudeMax = Rmax / cos(rawAngle + M_PI);
			}
			else
			{
				rawAmplitudeMax = Rmax / sin(rawAngle + M_PI);
			}
		}
	}

	angleCommand = rawAngle;
	amplitudeCommand = abs(rawAmplitude / rawAmplitudeMax);

	for (int i = 0; i < 6; i++)
	{
		bellowProjection[i] = cos(angleCommand) * bellowConfigurationPx[i] + sin(angleCommand) * bellowConfigurationPy[i];
		OpeningResult[i] = -(bellowProjection[i] / belloConfigurationR * amplitudeCommand * 0.5) + openingBase;
	}
}

//Joystick->XYZ (joyRx->x, joyRy->y, joyLy->z)
void writeXYZ1(int joystickFLag)
{
	if (joystickFLag == 1)
	{
		if (joyRx > 0.05)
		{
			x = x + x_scale;
		}
		else if (joyRx < -0.05)
		{
			x = x - x_scale;
		}

		if (joyRy > 0.05)
		{
			y = y + y_scale;
		}
		else if (joyRy < -0.05)
		{
			y = y - y_scale;
		}

		if (joyLy > 0.05)
		{
			z = z + z_scale;
		}
		else if (joyLy < -0.05)
		{
			z = z - z_scale;
		}
	}
	else if (joystickFLag == 0)
	{
		if (abs(alpha) < 1e-4)
		{
			x = 0;
			y = 0;
			z = length;
		}
		else
		{
			x = length * cos(beta) * (1 - cos(alpha)) / alpha;
			y = length * sin(beta) * (1 - cos(alpha)) / alpha;
			z = length * sin(alpha) / alpha;
		}
	}

	x = CONSTRAIN(x, x_min, x_max);
	y = CONSTRAIN(y, y_min, y_max);
	z = CONSTRAIN(z, z_min, z_max);
}

void writeXYZ3(int joystickFLag)
{
	if (joystickFLag == 1)
	{
		if (joyRx > 0.05)
		{
			segx_ = segx_ + x_scale;
		}
		else if (joyRx < -0.05)
		{
			segx_ = segx_ - x_scale;
		}

		if (joyRy > 0.05)
		{
			segy_ = segy_ + y_scale;
		}
		else if (joyRy < -0.05)
		{
			segy_ = segy_ - y_scale;
		}

		if (joyLy > 0.05)
		{
			segz_ = segz_ + z_scale;
		}
		else if (joyLy < -0.05)
		{
			segz_ = segz_ - z_scale;
		}
	}
	else if (joystickFLag == 0)
	{
		Matrix4f T = Matrix4f::Identity();
		Matrix4f Tr;
		Matrix3f R;

		Quaternionf q;
		float a[3];
		float b[3];
		float l[3];

		for (int i = 0; i < 3; i++)
		{
			a[i] = segAlpha_[i];
			b[i] = segBeta_[i];
			l[i] = segLength_[i];
		}

		for (int i = 0; i < 3; i++)
		{
			if (abs(a[i]) < 1e-4)
			{
				Tr << cos(b[i]), -sin(b[i]), 0, 0,
					sin(b[i]), cos(b[i]), 0, 0,
					0, 0, 1, l[i],
					0, 0, 0, 1;
			}
			else
			{
				Tr << cos(b[i]) * cos(b[i]) * (cos(a[i]) - 1) + 1, 0.5 * sin(2 * b[i]) * (cos(a[i]) - 1), cos(b[i]) * sin(a[i]), (l[i] * cos(b[i]) * (1 - cos(a[i]))) / a[i],
					0.5 * sin(2 * b[i]) * (cos(a[i]) - 1), sin(b[i]) * sin(b[i]) * (cos(a[i]) - 1) + 1, sin(b[i]) * sin(a[i]), (l[i] * sin(b[i]) * (1 - cos(a[i]))) / a[i],
					-sin(a[i]) * cos(b[i]), -sin(a[i]) * sin(b[i]), cos(a[i]), l[i] * sin(a[i]) / a[i],
					0, 0, 0, 1;
			}

			T = T * Tr;
		}

		segx_ = T(0, 3);
		segy_ = T(1, 3);
		segz_ = T(2, 3);

		R = T.block<3, 3>(0, 0);
		q = R;

		segqx_ = q.x();
		segqy_ = q.y();
		segqz_ = q.z();
		segqw_ = q.w();
	}

	segx_ = CONSTRAIN(segx_, x_min, x_max);
	segy_ = CONSTRAIN(segy_, y_min, y_max);
	segz_ = CONSTRAIN(segz_, z_min6, z_max6);
}

void wrapAlphaAndBeta()
{
	for (int i = 0; i < SEGNUM; i++)
	{
		if (Cmd_ABL.segment[i].A < 0)
		{
			Cmd_ABL.segment[i].A = -Cmd_ABL.segment[i].A;
			Cmd_ABL.segment[i].B = constrain2PI(Cmd_ABL.segment[i].B + M_PI);
		}
	}
}

void Init_parameter()
{
	//for Write ABL
	for (int i = 0; i < 9; i++)
	{
		segLength[i] = g_length0;
	}

	for (int i = 0; i < 3; i++)
	{
		segLength_[i] = g_length0 * 2;
	}

	for (int i = 0; i < 2; i++)
	{
		segLength_2[i] = g_length0 * 3;
	}

	//for Write Opening
	for (int i = 0; i < 6; i++)
	{
		bellowConfigurationPx[i] = belloConfigurationR * cos(i * 2 * M_PI / 6);
		bellowConfigurationPy[i] = belloConfigurationR * sin(i * 2 * M_PI / 6);
	}
}

void help_menu()
{
	cout << "===============================================================================" << endl;
	cout << "                              JOYSCTIK USER MENU                               " << endl;
	cout << "-------------------------------------------------------------------------------" << endl;
	cout << "           MODE         |"
		 << " KEY |                    KEYCODE                 " << endl;
	cout << " 0: 1 segment abl(test) |"
		 << " A   |"
		 << "  alpha  +: joyLy +, alpha  -: joyLy -; " << endl;
	cout << " 1: 3 segment abl       |"
		 << " B   |"
		 << "  beta   +: joyRx +, beta   -: joyRx -; " << endl;
	cout << " 2: 6 segment abl       |"
		 << " X   |"
		 << "  length +: joyLT  , length -: joyRT  ; " << endl;
	cout << "                                                                               " << endl;
	cout << " 3: 2 segment abl       |"
		 << " Y   |"
		 << "  x+: joyRx +, x-: joyRx -;             " << endl;
	// cout << " 3: 1 segment xyz(test) |"
	// 	 << " Y   |"
	// 	 << "  x+: joyRx +, x-: joyRx -;             " << endl;
	cout << " 4: 6 segment xyz       |"
		 << " RB  |"
		 << "  y+: joyRy +, y-: joyRy -;             " << endl;
	cout << "                        |"
		 << "     |"
		 << "  z+: joyLy +, z-: joyLy -;             " << endl;
	cout << "                                                                               " << endl;
	cout << " save data into file    |"
		 << " LB  |"
		 << "                                        " << endl;
	cout << "                                                                               " << endl;
	cout << "===============================================================================" << endl;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "joy_teleopt");
	ros::NodeHandle nh;
	ros::Rate r(100); //Hz

	ros::Subscriber sub1 = nh.subscribe("joy", 1, joyCallback);
	ros::Subscriber sub2 = nh.subscribe("key_number", 1, keyCallback);
	ros::Subscriber sub3 = nh.subscribe("Cmd_ABL_ik", 1, ABLCallback);
	ros::Publisher pub1 = nh.advertise<origarm_ros::Command_ABL>("Cmd_ABL_joy", 100);
	ros::Publisher pub3 = nh.advertise<origarm_ros::Command_Position>("Cmd_Position", 100);
	ros::Publisher pub4 = nh.advertise<origarm_ros::modenumber>("modenumber", 100);
	ros::Publisher pub5 = nh.advertise<origarm_ros::segnumber>("segnumber", 100);
	//ros::Publisher  pub5  = nh.advertise<origarm_ros::SegOpening>("Cmd_Opening", 100);

	Init_parameter();

	trajPointFileName = "trajp_" + getTimeString();
	savedata.open("/home/ubuntu/catkin_ws/src/origarm_ros/data/" + trajPointFileName + ".txt", ios::trunc);

	while (ros::ok())
	{
		//print help menu
		help_menu();

		//check whether joystick is available
		if (status == 1)
		{
			//control mode
			//mode[0]: 1 abl; mode[1]: 3 abl; mode[2]: 9 abl; mode[3]: 2 abl; mode[4]: 3 xyz;
			//mode[0]: joyA;  mode[1]: joyB;  mode[2]: joyX;  mode[3]: joyY;  mode[4]: joyRB;

			if (mode == 0) //ABL_1 control mode
			{
				writeABL1(1); //joystick -> ABL1
				writeXYZ1(0); //ABL1     -> XYZ1
			}
			else if (mode == 1) //ABL_3 control mode
			{
				writeABL3(1); //joystick -> ABL3
				writeABL9(2); //ABL3     -> ABL9
				writeXYZ3(0); //ABL3     -> XYZ3
			}
			else if (mode == 2) //ABL_9 control mode
			{
				writeABL9(1); //joystick -> ABL9
			}
			// else if (mode == 3) //XYZ_1 control mode
			// {
			// 	writeXYZ1(1); //joystick -> XYZ1
			// 	writeABL1(0); //XYZ1     -> ABL1
			// }
			else if (mode == 3)
			{
				writeABL2(1); //joystick  -> ABL2
			}
			else if (mode == 4) //XYZ_3 control mode
			{
				writeXYZ3(1); //joystick -> XYZ3
				writeABL3(0); //XYZ3     -> ABL3
				writeABL9(2); //ABL3     -> ABL9
			}

			//WriteOpening();

			//ROS_INFO("status: %d", status);
			//ROS_INFO("segment:%d", segNumber);
			//ROS_INFO("mode   :%d", mode);
		}
		else if (status == 0)
		{
			//Reset
			for (int i = 0; i < 9; i++)
			{
				segAlpha[i] = 0;
				segBeta[i] = 0;
				segLength[i] = g_length0;
			}

			for (int i = 0; i < 3; i++)
			{
				segAlpha_[i] = 0;
				segBeta_[i] = 0;
				segLength_[i] = g_length0 * 2.0;
			}

			for (int i = 0; i < 2; i++)
			{
				segAlpha_2[i] = 0;
				segBeta_2[i] = 0;
				segLength_2[i] = 0.165;
			}

			alpha = 0;
			beta = 0;
			length = g_length0;

			x = 0;
			y = 0;
			z = g_originZ;

			segx_ = 0;
			segy_ = 0;
			segz_ = g_originZ * 6;

			for (int i = 0; i < 6; i++)
			{
				OpeningResult[i] = 0;
			}

			//ROS_INFO("status: %d", status);
		}

		modenumber.modeNumber = mode;
		modenumber.status = status;

		segnumber.segmentNumber = segNumber;

		if (mode == 0)
		{
			for (int i = 0; i < 6; i++)
			{
				Cmd_ABL.segment[i].A = alpha;
				Cmd_ABL.segment[i].B = beta;
				Cmd_ABL.segment[i].L = length;
			}
			for (int i = 6; i < SEGNUM; i++)
			{
				Cmd_ABL.segment[i].A = 0;
				Cmd_ABL.segment[i].B = 0;
				Cmd_ABL.segment[i].L = g_length0;
			}

			printf("ABL1: alpha: %f, beta: %f, length: %f\r\n", alpha, beta, length);
		}
		else if (mode == 1)
		{
			for (int i = 0; i < 6; i++)
			{
				Cmd_ABL.segment[i].A = segAlpha_[int(i / 2)] / 2;
				Cmd_ABL.segment[i].B = segBeta_[int(i / 2)];
				Cmd_ABL.segment[i].L = segLength_[int(i / 2)] / 2;

				printf("ABL3: alpha: %f, beta: %f, length: %f\r\n", segAlpha_[int(i / 2)] / 2, segBeta_[int(i / 2)], segLength_[int(i / 2)] / 2);
			}

			for (int i = 6; i < SEGNUM; i++)
			{
				Cmd_ABL.segment[i].A = 0;
				Cmd_ABL.segment[i].B = 0;
				Cmd_ABL.segment[i].L = g_length0;
			}
		}
		else if (mode == 2)
		{
			for (int i = 0; i < 6; i++)
			{
				Cmd_ABL.segment[i].A = segAlpha[i];
				Cmd_ABL.segment[i].B = segBeta[i];
				Cmd_ABL.segment[i].L = segLength[i];

				printf("ABL6: alpha: %f, beta: %f, length: %f\r\n", segAlpha[i], segBeta[i], segLength[i]);
			}

			for (int i = 6; i < SEGNUM; i++)
			{
				Cmd_ABL.segment[i].A = 0;
				Cmd_ABL.segment[i].B = 0;
				Cmd_ABL.segment[i].L = g_length0;
			}
		}
		/*else if (mode == 3)
		{
			Cmd_Position.pose.position.x = x;
			Cmd_Position.pose.position.y = y;
			Cmd_Position.pose.position.z = z;
			Cmd_Position.pose.orientation.x = 1;
			Cmd_Position.pose.orientation.w = 1;

			printf("XYZ1: x: %f, y: %f, z: %f\r\n", x, y, z);

			pub3.publish(Cmd_Position);
		}*/
		else if (mode == 3)
		{
			for (int i = 0; i < 6; i++)
			{
				Cmd_ABL.segment[i].A = segAlpha_2[int(i / 3)] / 3;
				Cmd_ABL.segment[i].B = segBeta_2[int(i / 3)];
				Cmd_ABL.segment[i].L = segLength_2[int(i / 3)] / 3;

				printf("ABL2: alpha: %f, beta: %f, length: %f\r\n", segAlpha_2[int(i / 3)] / 3, segBeta_2[int(i / 3)], segLength_2[int(i / 3)] / 3);
			}

			for (int i = 6; i < SEGNUM; i++)
			{
				Cmd_ABL.segment[i].A = 0;
				Cmd_ABL.segment[i].B = 0;
				Cmd_ABL.segment[i].L = g_length0;
			}
		}

		wrapAlphaAndBeta();

		pub1.publish(Cmd_ABL);

		if (mode == 4)
		{
			Cmd_Position.pose.position.x = segx_;
			Cmd_Position.pose.position.y = segy_;
			Cmd_Position.pose.position.z = segz_;
			Cmd_Position.pose.orientation.x = segqx_;
			Cmd_Position.pose.orientation.y = segqy_;
			Cmd_Position.pose.orientation.z = segqz_;
			Cmd_Position.pose.orientation.w = segqw_;

			printf("XYZ6: x: %f, y: %f, z: %f\r\n", segx_, segy_, segz_);

			pub3.publish(Cmd_Position);
		}

		//write into files
		if (save_flag == 1)
		{
			if (mode == 0)
			{
				savedata << 0 << " " << 10000 << " " << 10000 << " " << 10000
						 << " " << alpha / 6 << " " << beta << " " << length / 6
						 << " " << alpha / 6 << " " << beta << " " << length / 6
						 << " " << alpha / 6 << " " << beta << " " << length / 6
						 << " " << alpha / 6 << " " << beta << " " << length / 6
						 << " " << alpha / 6 << " " << beta << " " << length / 6
						 << " " << alpha / 6 << " " << beta << " " << length / 6
						 << endl;
				printf("%s\n", "write mode 0 data, 1 segment");
			}
			else if (mode == 1)
			{
				savedata << 1 << " " << 10000 << " " << 10000 << " " << 10000
						 << " " << segAlpha_[0] / 2 << " " << segBeta_[0] << " " << segLength_[0] / 2
						 << " " << segAlpha_[0] / 2 << " " << segBeta_[0] << " " << segLength_[0] / 2
						 << " " << segAlpha_[1] / 2 << " " << segBeta_[1] << " " << segLength_[1] / 2
						 << " " << segAlpha_[1] / 2 << " " << segBeta_[1] << " " << segLength_[1] / 2
						 << " " << segAlpha_[2] / 2 << " " << segBeta_[2] << " " << segLength_[2] / 2
						 << " " << segAlpha_[2] / 2 << " " << segBeta_[2] << " " << segLength_[2] / 2
						 << endl;
				printf("%s\n", "write mode 1 data, 3 segments");
			}
			else if (mode == 2)
			{
				savedata << 2 << " " << 10000 << " " << 10000 << " " << 10000
						 << " " << segAlpha[0] << " " << segBeta[0] << " " << segLength[0]
						 << " " << segAlpha[1] << " " << segBeta[1] << " " << segLength[1]
						 << " " << segAlpha[2] << " " << segBeta[2] << " " << segLength[2]
						 << " " << segAlpha[3] << " " << segBeta[3] << " " << segLength[3]
						 << " " << segAlpha[4] << " " << segBeta[4] << " " << segLength[4]
						 << " " << segAlpha[5] << " " << segBeta[5] << " " << segLength[5]
						 << endl;

				printf("%s\n", "write mode 2 data, 9 segments");
			}
			else if (mode == 3)
			{
				savedata << 3 << " " << 10000 << " " << 10000 << " " << 10000
						 << " " << segAlpha_2[0] / 3 << " " << segBeta_2[0] << " " << segLength_2[0] / 3
						 << " " << segAlpha_2[0] / 3 << " " << segBeta_2[0] << " " << segLength_2[0] / 3
						 << " " << segAlpha_2[0] / 3 << " " << segBeta_2[0] << " " << segLength_2[0] / 3
						 << " " << segAlpha_2[1] / 3 << " " << segBeta_2[1] << " " << segLength_2[1] / 3
						 << " " << segAlpha_2[1] / 3 << " " << segBeta_2[1] << " " << segLength_2[1] / 3
						 << " " << segAlpha_2[1] / 3 << " " << segBeta_2[1] << " " << segLength_2[1] / 3
						 << endl;

				printf("%s\n", "write mode 3 data, 2 segments");
			}
			else if (mode == 4)
			{
				savedata << 4 << " " << segx_ << " " << segy_ << " " << segz_
						 << " " << segAlphad_[0] << " " << segBetad_[0] << " " << segLengthd_[0]
						 << " " << segAlphad_[0] << " " << segBetad_[0] << " " << segLengthd_[0]
						 << " " << segAlphad_[1] << " " << segBetad_[1] << " " << segLengthd_[1]
						 << " " << segAlphad_[1] << " " << segBetad_[1] << " " << segLengthd_[1]
						 << " " << segAlphad_[2] << " " << segBetad_[2] << " " << segLengthd_[2]
						 << " " << segAlphad_[2] << " " << segBetad_[2] << " " << segLengthd_[2]
						 << endl;
				printf("%s\n", "write mode 4 data");
			}
		}

		if (modenumberLast.modeNumber != modenumber.modeNumber || modenumberLast.status != modenumberLast.status)
		{
			pub4.publish(modenumber);
			modenumberLast.modeNumber = modenumber.modeNumber;
			modenumberLast.status = modenumberLast.status;
		}
		if (segnumberLast.segmentNumber != segnumber.segmentNumber)
		{
			pub5.publish(segnumber);
			segnumberLast.segmentNumber = segnumber.segmentNumber;
		}

		ros::spinOnce();
		r.sleep(); //sleep for 1/r sec
				   //usleep(10000); // N*us
	}

	savedata.close();
	return 0;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_ALGO_LM_MANY_CLASSES_SCHUR_COMPLEMENT_SCHUR_COMPLEMENT5_HPP__
#define __OPTIMISATION2_ALGO_LM_MANY_CLASSES_SCHUR_COMPLEMENT_SCHUR_COMPLEMENT5_HPP__

#include "../levmar.hpp"
#include <libv/lma/lm/ba/initialize.hpp>
#include <libv/lma/lm/ba/compute_s.hpp>
#include <libv/lma/lm/bundle/make_sparse_indice.hpp>
#include "schur.hpp"
#include "implicit_schur.hpp"
#include <boost/fusion/include/pair.hpp>
#include <libv/lma/lm/ba/tuple_to_mat.hpp>

namespace lma
{
  template<class BA, class ExplictSCHUR, class ImplicitSCHUR> struct BABYS
  {
    const BA& ba;
    const ExplictSCHUR& eschur;
    const ImplicitSCHUR& ischur;

    typedef typename ExplictSCHUR::TypeS Hessian;
    typedef typename ExplictSCHUR::OptimizeKeys OptimizeKeys;

    const typename ExplictSCHUR::TupleS& A() const { return eschur.s; }
    const typename ImplicitSCHUR::TupleResiduUs& B() const { return ischur.bs; }

    BABYS(const BA& ba_, const ExplictSCHUR& schur_, const ImplicitSCHUR& ischur_):ba(ba_),eschur(schur_),ischur(ischur_){}

    template<class AP, class P> void prodAP(AP& ap, const P& p) const
    {
//       mpl::for_each<Hessian,ttt::wrap_>(prod_ap_p(ap,eschur.s,p));
//       typedef typename MetaProd<typename TrigSup<Hessian>::type,typename ImplicitSCHUR::TupleResiduUs>::type keys;
//       std::cout << std::endl << ttt::name<keys>() << std::endl;
//       for_each<keys>(std::tie(ap,eschur.s,p),AABB());

      for_each<MetaProd<typename AddTranspose<Hessian>::type,typename ImplicitSCHUR::TupleResiduUs>>(std::tie(ap,eschur.s,p),AABB());
    }
  };

  template<class VAB> struct CopyReverseIndice
  {
    const VAB& vab;
    CopyReverseIndice(const VAB& vab_):vab(vab_){}

    template<template<class,class> class Pair, class Key, class Value> void operator()(Pair<Key,Value>& pair) const
    {
      auto& sics = pair.second;
      bf::for_each(sics,CopyReverseIndice<VAB>(vab));
    }

    template<template<class,class> class Pair, class Key, class A, class B> void operator()(Pair<Key,SIC2<A,B>>& pair) const
    {
      auto& sics_ab = pair.second;
      const auto& sic_ba = bf::at_key<B>(bf::at_key<A>(vab));

     // trop dur de savoir ce que ca fait et si c'est vraiment utile...
      for(auto i = sic_ba.first() ; i < sic_ba.size() ; ++i)
        for(auto j = sic_ba.first(i) ; j < sic_ba.size(i) ; ++j)
          sics_ab.add(sic_ba(i,j),i,j);
    }
  };

  template<class BA, class NumericTag, class KeyUs> struct SchurExplicit
  {
    typedef typename BA::Keys Keys;

    // K_ est le nombre de famille à mettre dans KeyUs
    static const size_t K = mpl::size<Keys>::value - Size<NumericTag>::value;

    typedef typename For<K,mpl::size<Keys>::value,Keys,PushBack>::type KeyVs;

//     typedef typename MakeH<typename BA::Float,KeyUs>::type TypeS;
    typedef typename ListS<typename BA::ListeHessien,KeyUs>::type TypeS;

    typedef typename br::as_map< TypeS >::type TupleS;

    struct Vec { std::vector< bf::vector<int,int,int,int,int> > liste_op; };

    typedef typename br::as_map<
				 typename mpl::transform<
							  typename mpl::transform<TypeS,br::first<mpl::_1>>::type,
							  bf::pair<mpl::_1,Vec>
							>::type
			       >::type PrecalcVector;

    typedef KeyUs OptimizeKeys;
    typedef typename MakeSparseIndiceContainer2<Keys>::type VABMap;

    // tuple de vector des indices pré-calculés pour le calcul de S.

    typedef typename br::as_map<typename mpl::transform<KeyVs,bf::pair<mpl::_1,PrecalcVector>>::type>::type TUPLEVECTORINDICE;
    TUPLEVECTORINDICE tuple_v;
    VABMap svab;
    TupleS s;

    const TupleS& A() const { return s; }

    SchurExplicit()
    {
//       std::cout << " schur : " << ttt::name<TypeS>() << std::endl;
    }

    template<class Indices> void init(const Indices& vab)
    {
      bf::for_each(svab,CopyReverseIndice<Indices>(vab));
    }
  };

  struct UpdateIndice
  {
    template<class Pair> void operator()(Pair& pair) const
    {
      pair.second.indice.update();
      pair.second.resize(pair.second.indice);
    }
  };

  template<class Bdl, class NormEq, class MatrixTag_, class K> class ExplicitSchur : ImplicitSchur<Bdl,NormEq,MatrixTag_,K>
  {
    public:
    typedef MatrixTag_ MatrixTag;
    typedef ImplicitSchur<Bdl,NormEq,MatrixTag_,K> parent;
    typedef Bdl Bundle;
    typedef utils::Tic<false> Tic;

    typedef Bas<Bundle,MatrixTag> Ba;
    typedef typename Ba::Hessian Hessian;
    typedef typename Ba::Keys Keys;
    typedef typename parent::SchurCont ParentSchur;
    typedef SchurExplicit<Ba,K,typename ParentSchur::KeyUs> SchurCont;

    typedef typename ParentSchur::TypeVs TypeVs;
    typedef typename ParentSchur::TypeWs TypeWs;
    typedef typename ParentSchur::ListResidu ResiduUs;

    SchurCont schur_;

    typedef typename SchurCont::KeyVs KeyVs;

    template<class Config> ExplicitSchur(Config config):parent(config){}

    void init(Bundle& bundle_, Ba& ba_)
    {
      parent::init(bundle_,ba_);
      bf::for_each(schur_.s,detail::ResizeInterInit<Bdl>(bundle_));
      schur_.init(bundle_.vab_map);
      auto tie1 = std::tie(bundle_, ba_.h);
      for_each<MetaProd<TypeWs,Transpose<TypeWs>>>(std::tie(schur_.s,parent::schur_.ys,ba_.h),AA_INIT_S_1_AA<decltype(tie1)>(tie1));

      bf::for_each(schur_.s,UpdateIndice());
      /*bf::for_each(schur_.s,[](auto& pair)
      {
        pair.second.indice.update();
        pair.second.resize(pair.second.indice);
      });*/
      auto tie2 = std::tie(schur_.tuple_v,schur_.svab, bundle_);
      for_each<MetaProd<TypeWs,Transpose<TypeWs>>>(std::tie(schur_.s,parent::schur_.ys,ba_.h),AA_INIT_S_2_AA<decltype(tie2)>(tie2));
    }

    void init_zero()
    {
      parent::init_zero();
      bf::for_each(schur_.s,detail::SetZero());//! facultatif ?
    }

    using parent::save_h;
    using parent::reload_h;
    using parent::get_vs;

    void solve(Ba& ba_, Bundle& bundle_)
    {
      parent::inv_v(ba_.h);
      parent::compute_y(ba_);
      parent::compute_b(ba_);
      compute_s(ba_,bundle_);
      compute_delta_a(ba_);
      parent::compute_delta_b(ba_);
    }

    void compute_s(Ba& ba_, Bundle& )
    {
      Tic tic("Compute S");
      bf::for_each(schur_.s,AssignSame<Hessian>(ba_.h));
//       bf::for_each(schur_.s,[&](auto& pair)
//       {
//         std::cout << " Assign " << pair.second.name() << std::endl;
//         AssignSame<Hessian>(ba_.h)(pair);
//
// //         auto a = to_mat<typename SchurCont::OptimizeKeys,typename SchurCont::TypeS>(schur_.s,size_tuple<mpl::size<typename SchurCont::OptimizeKeys>::value>(ba_.delta));
//
// //         std::cout << " S = U \n " << a << std::endl;
//       }
//       );

//       typedef typename SchurCont::OptimizeKeys aa;
//       typedef typename SchurCont::TypeS bb;
//       auto a = to_mat<typename SchurCont::OptimizeKeys,typename SchurCont::TypeS>(schur_.s,size_tuple<mpl::size<typename SchurCont::OptimizeKeys>::value>(ba_.delta));

//       std::cout << " S = U \n " << a << std::endl;
      // S = Y * W
      for_each<MetaProd<TypeWs,Transpose<TypeWs>> >(std::tie(schur_.s,parent::schur_.ys,ba_.h),AA_COMPUTE_S_AA<typename SchurCont::TUPLEVECTORINDICE>(schur_.tuple_v));

//       a = to_mat<typename SchurCont::OptimizeKeys,typename SchurCont::TypeS>(schur_.s,size_tuple<mpl::size<typename SchurCont::OptimizeKeys>::value>(ba_.delta));

//       std::cout << " S -= YW\n " << a << std::endl;

      tic.disp();
    }

    void compute_delta_a(Ba& ba_)
    {
      Tic tic("Compute DA");
      parent::norm_eq(BABYS<Ba,SchurCont,ParentSchur>(ba_,schur_,parent::schur_),ba_.delta,MatrixTag());
      tic.disp();
    }
  };
}// eon

namespace ttt
{
  template<class A, class B, class C, int K> struct Name<lma::ExplicitSchur<A,B,C,v::numeric_tag<K>>> { static std::string name(){ return "ExplicitSchur<" + ttt::name<B>() + "," + ttt::name<C>() + "," + lma::to_string(K) + ">"; } };
}

#endif


// Copyright (C) 2014 Massachusetts Institute of Technology, Lincoln Laboratory
// License: Boost Software License   See LICENSE.txt for the full license.
// Authors: Davis E. King (davis@dlib.net)

#include <dlib/algs.h>
#include <mitie/binary_relation_detector.h>
#include <dlib/hash.h>
#include <vector>

using namespace dlib;
using namespace mitie;

namespace
{

    inline std::pair<uint64,uint64> hash_string (
        const std::string& str,
        const uint32 seed
    )
    {
        if (str.size() == 0)
            return make_pair(0, 0);
        return murmur_hash3_128bit(&str[0], str.size(), seed);
    }

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

    inline void accum_123gram_feats (
        sparse_vector_type& vect,
        const std::pair<unsigned long, unsigned long>& range,
        const std::vector<std::string>& tokens,
        const unsigned long num_hash_dims,
        const unsigned long offset,
        const unsigned long hash_seed
    )
    {
        std::pair<uint64,uint64> h[3];
        h[0] = make_pair(0,0);
        h[1] = make_pair(0,0);
        h[2] = make_pair(0,0);
        for (unsigned long i = range.first; i < range.second; ++i)
        {
            // shift h right 1
            h[2] = h[1];
            h[1] = h[0];
            h[0] = hash_string(tokens[i], hash_seed);

            std::pair<uint64,uint64> temp;
            double sign;

            // add a 1-gram feature
            sign = (h[0].second&1) ? +1 : -1;
            vect.push_back(make_pair<unsigned long>(h[0].first%num_hash_dims + offset, sign));

            if (i > range.first)
            {
                // add a 2-gram feature
                temp = murmur_hash3_128bit_3(h[0].first, h[1].first, 0);
                sign = (temp.second&1) ? +1 : -1;
                vect.push_back(make_pair<unsigned long>(temp.first%num_hash_dims + offset, sign));
            }
            if (i > range.first+1)
            {
                // add a 3-gram feature
                temp = murmur_hash3_128bit_3(h[0].first, h[1].first, h[2].first);
                sign = (temp.second&1) ? +1 : -1;
                vect.push_back(make_pair<unsigned long>(temp.first%num_hash_dims + offset, sign));
            }
        }
    }

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

    inline std::pair<unsigned long,double> make_feat (
        uint32 v1,
        uint32 v2,
        uint32 v3,
        const unsigned long num_hash_dims,
        const unsigned long offset
    )
    {
        std::pair<uint64,uint64> temp = murmur_hash3_128bit_3(v1,v2,v3);
        double sign = (temp.second&1) ? +1 : -1;
        return make_pair<unsigned long>(temp.first%num_hash_dims + offset, sign);
    }

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

    inline uint32 hash_range(
        const std::vector<std::string>& tokens,
        const std::pair<unsigned long, unsigned long>& range,
        const unsigned long hash_seed
    )
    {
        uint32 h = hash_seed;
        for (unsigned long i = range.first; i < range.second; ++i)
        {
            h = dlib::hash(tokens[i], h);
        }
        return h;
    }

}

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

namespace mitie
{

    binary_relation extract_binary_relation (
        const std::vector<std::string>& tokens,
        const std::pair<unsigned long, unsigned long>& rel_arg1,
        const std::pair<unsigned long, unsigned long>& rel_arg2,
        const total_word_feature_extractor& tfe
    )
    {
        DLIB_CASSERT(rel_arg1.first < rel_arg1.second && rel_arg1.second <= tokens.size(),"invalid inputs");
        DLIB_CASSERT(rel_arg2.first < rel_arg2.second && rel_arg2.second <= tokens.size(),"invalid inputs");

        // get dense word features for the two arguments.
        matrix<float,0,1> arg1, arg2, temp;
        for (unsigned long i = rel_arg1.first; i < rel_arg1.second; ++i)
        {
            tfe.get_feature_vector(tokens[i], temp);
            arg1 += temp;
        }
        arg1 /= (rel_arg1.second-rel_arg1.first);
        for (unsigned long i = rel_arg2.first; i < rel_arg2.second; ++i)
        {
            tfe.get_feature_vector(tokens[i], temp);
            arg2 += temp;
        }
        arg2 /= (rel_arg2.second-rel_arg2.first);
        // Put the dense vectors into the sparse format
        binary_relation rel;
        rel.total_word_feature_extractor_fingerprint = tfe.get_fingerprint();
        long offset = 0;
        for (long i = 0; i < arg1.size(); ++i)
            rel.feats.push_back(make_pair(offset + i, arg1(i)));
        offset += arg1.size();
        for (long i = 0; i < arg2.size(); ++i)
            rel.feats.push_back(make_pair(offset + i, arg2(i)));
        offset += arg2.size();

        typedef std::pair<unsigned long, unsigned long> range_t;
        range_t range1 = rel_arg1;
        range_t range2 = rel_arg2;
        unsigned int hash_seed = 0;
        if (range1.first > range2.first)
        {
            swap(range1, range2);
            // Use a different hash seed because that allows us to model the ordering of the
            // arguments.
            hash_seed = 100000;
        }
        const unsigned int win = 2;
        range_t rbefore_first  = make_pair(range1.first>=win?range1.first-win:0, range1.first);
        range_t rbetween       = make_pair(min(range1.second,range2.second), max(range1.first,range2.first));
        range_t rafter_second  = make_pair(range2.second, range2.second+win<=tokens.size()?range2.second+win:tokens.size());
        const unsigned int win2 = 5;
        range_t rbefore_first2  = make_pair(range1.first>=win2?range1.first-win2:0, range1.first);
        range_t rafter_second2  = make_pair(range2.second, range2.second+win2<=tokens.size()?range2.second+win2:tokens.size());

        const long num_hash_dims = 100000;
        accum_123gram_feats(rel.feats, rbefore_first,  tokens, num_hash_dims, offset, hash_seed); ++hash_seed;
        accum_123gram_feats(rel.feats, rbefore_first2, tokens, num_hash_dims, offset, hash_seed); ++hash_seed;
        accum_123gram_feats(rel.feats, rbetween,       tokens, num_hash_dims, offset, hash_seed); ++hash_seed;
        accum_123gram_feats(rel.feats, rafter_second,  tokens, num_hash_dims, offset, hash_seed); ++hash_seed;
        accum_123gram_feats(rel.feats, rafter_second2, tokens, num_hash_dims, offset, hash_seed); ++hash_seed;

        const uint32 h1 = hash_range(tokens, rbefore_first, hash_seed);
        const uint32 h2 = hash_range(tokens, rbetween, hash_seed);
        const uint32 h3 = hash_range(tokens, rafter_second, hash_seed);

        rel.feats.push_back(make_feat(h1,h2,0,  num_hash_dims, offset));
        rel.feats.push_back(make_feat( 0,h2,0,  num_hash_dims, offset));
        rel.feats.push_back(make_feat( 0,h2,h3, num_hash_dims, offset));
        rel.feats.push_back(make_feat(h1,h2,h3, num_hash_dims, offset));

        make_sparse_vector_inplace(rel.feats);
        return rel;
    }

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

}


/*
** map.hh
** Login : <mouret@asuncion.lip6.fr>
** Started on  Mon Jan 14 16:38:00 2008 Jean-Baptiste MOURET
** $Id$
**
** Copyright (C) 2008 Jean-Baptiste MOURET
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation; either version 2 of the License, or
** (at your option) any later version.
**
** 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
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/

#ifndef   	MAP_HH_
# define   	MAP_HH_

#include <vector>
#include <fstream>
#include <cmath>
#include <cassert>
#include <boost/shared_ptr.hpp>
#include <iostream>

#include "misc.hpp"
#include "goal.hpp"
#include "illuminated_switch.hpp"

namespace fastsim
{

  // 0, 0 is top left
  // map must be a square
  class Map
  {
  public:
    enum status_t { free = 0, obstacle = 255 };
    Map(const char* fname, float real_w) :
      _real_w(real_w),
      _real_h(real_w)
    {
      try
	{
	  _read_file(fname);
	}
      catch (Exception e)
	{
	  std::cerr<<"error : "<<e.get_msg()<<std::endl;
	}
      assert(_w == _h);
      _fx = _w / _real_w;
    }
    // copy ONLY the picture (no goal, illuminated switches, etc)
    // REASON:
    // we want to avoid reading the data data but we don't want to
    // require good copy constructors for all objects
    Map(const Map& o) :
      _data(o._data),
      _w(o._w),
      _h(o._h),
      _real_w(o._real_w),
      _real_h(o._real_h),
      _fx(o._fx)
    {
      // check that we are copying an empty map
      assert(_goals.size() == 0);
      assert(_illuminated_switches.size() == 0);
    }
    status_t get_pixel(unsigned x, unsigned y) const
    {
      return (y * _h + x >= 0 && y * _h + x < _data.size()) ?
	_data[y * _h + x]
	: _data[_data.size()-1];
    }
    void set_pixel(unsigned x, unsigned y, status_t v)
    {
      if (y * _h + x >= 0 && y * _h + x < _data.size())
	_data[y * _h + x] = v;

    }
    status_t get_real(float x, float y) const { return get_pixel(real_to_pixel(x), real_to_pixel(y)); }
    int real_to_pixel(float x) const { return (unsigned) roundf(x * _fx); }
    float pixel_to_real(unsigned i) const { return i / _fx; }
    float get_real_w() const { return _real_w;}
    float get_real_h() const { return _real_h;}
    unsigned get_pixel_w() const { return _w;}
    unsigned get_pixel_h() const { return _h;}
    // see
    // http://lifc.univ-fcomte.fr/~dedu/projects/bresenham/index.html
    // in PIXEL coordinates
    bool check_inter_pixel(int x1, int y1,
			   int x2, int y2,
			   int& x_res, int& y_res) const;
    // in world coordinates
    bool check_inter_real(float x1, float y1,
			  float x2, float y2,
			  float& x_res, float &y_res) const
    {
      int px = 0, py = 0;
      bool res = check_inter_pixel(real_to_pixel(x1), real_to_pixel(y1),
				   real_to_pixel(x2), real_to_pixel(y2),
				   px, py);
      x_res = pixel_to_real(px);
      y_res = pixel_to_real(py);
      return res;
    }
    // in world coordinate, check intersection with an illuminated switch (on)
    // return the color of the closest illuminated switch if there is an
    // intersection, -1 otherwise
    int check_inter_is(float x1, float y1,
		       float x2, float y2) const ;

    const std::vector<Goal>& get_goals() const { return _goals; }
    void add_goal(const Goal& g) { _goals.push_back(g); }

    typedef boost::shared_ptr<IlluminatedSwitch> ill_sw_t;
    void add_illuminated_switch(ill_sw_t is)
    { _illuminated_switches.push_back(is); }
    const std::vector<ill_sw_t>& get_illuminated_switches() const
    { return _illuminated_switches; }
    ill_sw_t get_illuminated_switch_by_color(int color)
    {
      for (size_t i = 0; i < _illuminated_switches.size(); ++i)
	if (_illuminated_switches[i]->get_color() == color)
	  return _illuminated_switches[i];
      assert(0);
      return ill_sw_t();
    }
    void clear_illuminated_switches()
    { _illuminated_switches.clear(); }

    void update(const Posture& robot_pos)
    {
      for (size_t i = 0; i < _illuminated_switches.size(); ++i)
	_illuminated_switches[i]->try_to_activate(robot_pos);
    }
    void terrain_switch(const std::string &fname)
    {
      _read_file(fname);
    }

    // Draws a rectangle with (x,y) the upper left point and (lx,ly) the size
    void draw_rect(int x, int y, int lx, int ly);

  protected:

    //
    void _read_file(const std::string &fname);
    bool _get_bit(char c, int i) const { return (c & (1 << (7 - i))); }
    //
    bool _try_pixel(int x, int y) const;
    //
    bool _check_inter_ray_circle(float x1, float y1,
				 float x2, float y2,
				 float xr, float yr, float radius) const;

    //
    std::vector<status_t> _data;
    std::vector<Goal> _goals;
    std::vector<ill_sw_t> _illuminated_switches;
    //
    int _w, _h;
    float _real_w, _real_h;
    float _fx;
  };
}

#endif	    /* !MAP_HH_ */


/**
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef __STOUT_INTERVAL_HPP__
#define __STOUT_INTERVAL_HPP__

#include <functional> // For std::less.
#include <iostream>

#include <boost/icl/interval.hpp>
#include <boost/icl/interval_set.hpp>

// Forward declarations.
template <typename T>
class Interval;

template <typename T>
class IntervalSet;

// Represents a bound (left or right) for an interval. A bound can
// either be open or closed.
template <typename T>
class Bound
{
public:
  // Creates an open bound.
  static Bound<T> open(const T& value)
  {
    return Bound<T>(OPEN, value);
  }

  // Creates an closed bound.
  static Bound<T> closed(const T& value)
  {
    return Bound<T>(CLOSED, value);
  }

  // Intervals can be created using the comma operator. For example:
  //   (2, 6):  (Bound<int>::open(2), Bound<int>::open(6))
  //   (3, 4]:  (Bound<int>::open(3), Bound<int>::closed(4))
  //   [0, 5):  (Bound<int>::closed(0), Bound<int>::open(5))
  //   [1, 2]:  (Bound<int>::closed(1), Bound<int>::closed(2))
  Interval<T> operator , (const Bound<T>& right) const;

private:
  enum Type
  {
    OPEN,
    CLOSED,
  };

  Bound(const Type _type, const T& _value)
    : type(_type), value(_value) {}

  const Type type;
  const T value;
};

// Represents an interval.
template <typename T>
class Interval
{
public:
  // We must provide a public default constructor as the boost
  // interval set expects that.
  Interval() {}

  // Returns the inclusive lower bound of this interval.
  T lower() const { return data.lower(); }

  // Returns the exclusive upper bound of this interval.
  T upper() const { return data.upper(); }

  // Checks if this interval intersects with another interval.
  bool intersects(const Interval<T>& interval) const;

  // Checks if this interval intersects with an interval set.
  bool intersects(const IntervalSet<T>& set) const;

  bool operator==(const Interval<T>& that) const
  {
    return data == that.data;
  }

  bool operator!=(const Interval<T>& that) const
  {
    return !(*this == that);
  }

private:
  friend class Bound<T>;

  template <typename X>
  friend std::ostream& operator<<(
      std::ostream& stream,
      const Interval<X>& interval);

  Interval(const boost::icl::right_open_interval<T, std::less>& _data)
    : data(_data) {}

  // We store the interval in right open format: [lower, upper).
  // Notice that we use composition here instead of inheritance to
  // prevent Interval from being passed to bare boost icl containers.
  boost::icl::right_open_interval<T, std::less> data;
};

template <typename T>
std::ostream& operator<<(std::ostream& stream, const Interval<T>& interval)
{
  return stream << interval.data;
}

template <typename T>
Interval<T> Bound<T>::operator , (const Bound<T>& right) const
{
  if (type == OPEN) {
    if (right.type == OPEN) {
      // For example: (1, 3).
      return boost::icl::static_interval<
          boost::icl::right_open_interval<T, std::less>, // Target.
          boost::icl::is_discrete<T>::value,
          boost::icl::interval_bounds::static_open,      // Input bounds.
          boost::icl::interval_bounds::static_right_open // Output bounds.
      >::construct(value, right.value);
    } else {
      // For example: (1, 3].
      return boost::icl::static_interval<
          boost::icl::right_open_interval<T, std::less>, // Target.
          boost::icl::is_discrete<T>::value,
          boost::icl::interval_bounds::static_left_open, // Input bounds.
          boost::icl::interval_bounds::static_right_open // Output bounds.
      >::construct(value, right.value);
    }
  } else {
    if (right.type == OPEN) {
      // For example: [1, 3).
      return boost::icl::static_interval<
          boost::icl::right_open_interval<T, std::less>,  // Target.
          boost::icl::is_discrete<T>::value,
          boost::icl::interval_bounds::static_right_open, // Input bounds.
          boost::icl::interval_bounds::static_right_open  // Output bounds.
      >::construct(value, right.value);
    } else {
      // For example: [1, 3].
      return boost::icl::static_interval<
          boost::icl::right_open_interval<T, std::less>,  // Target.
          boost::icl::is_discrete<T>::value,
          boost::icl::interval_bounds::static_closed,     // Input bounds.
          boost::icl::interval_bounds::static_right_open  // Output bounds.
      >::construct(value, right.value);
    }
  }
}

// Modeled after boost interval_set. Provides a compact representation
// of a set by merging adjacent elements into intervals.
template <typename T>
class IntervalSet : public boost::icl::interval_set<T, std::less, Interval<T> >
{
public:
  IntervalSet() {}

  explicit IntervalSet(const T& value)
  {
    Base::add(value);
  }

  explicit IntervalSet(const Interval<T>& interval)
  {
    Base::add(interval);
  }

  IntervalSet(const Bound<T>& lower, const Bound<T>& upper)
  {
    Base::add((lower, upper));
  }

  // Checks if an element is in this set.
  bool contains(const T& value) const
  {
    return boost::icl::contains(static_cast<const Base&>(*this), value);
  }

  // Checks if an interval is in this set.
  bool contains(const Interval<T>& interval) const
  {
    // TODO(jieyu): Boost has an issue regarding unqualified lookup in
    // template (http://clang.llvm.org/compatibility.html#dep_lookup),
    // and gcc-4.8 complains about it. We use a workaround here by
    // delegating this call to the IntervalSet version below.
    return contains(IntervalSet<T>(interval));
  }

  // Checks if an interval set is a subset of this set.
  bool contains(const IntervalSet<T>& set) const
  {
    return boost::icl::contains(
        static_cast<const Base&>(*this),
        static_cast<const Base&>(set));
  }

  // Checks if this set intersects with an interval.
  bool intersects(const Interval<T>& interval) const
  {
    return boost::icl::intersects(static_cast<const Base&>(*this), interval);
  }

  // Checks if this set intersects with another interval set.
  bool intersects(const IntervalSet<T>& set) const
  {
    return boost::icl::intersects(
        static_cast<const Base&>(*this),
        static_cast<const Base&>(set));
  }

  // Returns the number of intervals in this set.
  size_t intervalCount() const
  {
    return boost::icl::interval_count(static_cast<const Base&>(*this));
  }

  // Overloaded operators.
  bool operator==(const IntervalSet<T>& that) const
  {
    return static_cast<const Base&>(*this) == static_cast<const Base&>(that);
  }

  bool operator!=(const IntervalSet<T>& that) const
  {
    return !(*this == that);
  }

  IntervalSet<T>& operator+=(const T& value)
  {
    static_cast<Base&>(*this) += value;
    return *this;
  }

  IntervalSet<T>& operator+=(const Interval<T>& interval)
  {
    static_cast<Base&>(*this) += interval;
    return *this;
  }

  IntervalSet<T>& operator+=(const IntervalSet<T>& set)
  {
    static_cast<Base&>(*this) += static_cast<const Base&>(set);
    return *this;
  }

  IntervalSet<T>& operator-=(const T& value)
  {
    static_cast<Base&>(*this) -= value;
    return *this;
  }

  IntervalSet<T>& operator-=(const Interval<T>& interval)
  {
    static_cast<Base&>(*this) -= interval;
    return *this;
  }

  IntervalSet<T>& operator-=(const IntervalSet<T>& set)
  {
    static_cast<Base&>(*this) -= static_cast<const Base&>(set);
    return *this;
  }

  IntervalSet<T>& operator&=(const T& value)
  {
    static_cast<Base&>(*this) &= value;
    return *this;
  }

  IntervalSet<T>& operator&=(const Interval<T>& interval)
  {
    static_cast<Base&>(*this) &= interval;
    return *this;
  }

  IntervalSet<T>& operator&=(const IntervalSet<T>& set)
  {
    static_cast<Base&>(*this) &= static_cast<const Base&>(set);
    return *this;
  }

private:
  template <typename X>
  friend std::ostream& operator<<(
      std::ostream& stream,
      const IntervalSet<X>& set);

  // We use typedef here to make the code less verbose.
  typedef boost::icl::interval_set<T, std::less, Interval<T> > Base;
};

template <typename T>
std::ostream& operator<<(std::ostream& stream, const IntervalSet<T>& set)
{
  return stream << static_cast<const typename IntervalSet<T>::Base&>(set);
}

template <typename T>
bool Interval<T>::intersects(const Interval<T>& interval) const
{
  return IntervalSet<T>(*this).intersects(interval);
}

template <typename T>
bool Interval<T>::intersects(const IntervalSet<T>& set) const
{
  return set.intersects(*this);
}

template <typename T, typename X>
IntervalSet<T> operator+(const IntervalSet<T>& set, const X& x)
{
  IntervalSet<T> result(set);
  result += x;
  return result;
}

template <typename T, typename X>
IntervalSet<T> operator-(const IntervalSet<T>& set, const X& x)
{
  IntervalSet<T> result(set);
  result -= x;
  return result;
}

// Defines type traits for the custom Interval above. These type
// traits are required by the boost interval set.
namespace boost {
namespace icl {

template <typename T>
struct interval_traits<Interval<T> >
{
  typedef interval_traits type;
  typedef T domain_type;
  typedef std::less<T> domain_compare;

  static Interval<T> construct(const T& lower, const T& upper)
  {
    return (Bound<T>::closed(lower), Bound<T>::open(upper));
  }

  static T lower(const Interval<T>& interval)
  {
    return interval.lower();
  }

  static T upper(const Interval<T>& interval)
  {
    return interval.upper();
  }
};

template <typename T>
struct interval_bound_type<Interval<T> >
  : public interval_bound_type<right_open_interval<T, std::less> >
{
  typedef interval_bound_type type;
};

} // namespace icl {
} // namespace boost {

#endif // __STOUT_INTERVAL_HPP__


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <boost/assign.hpp>

//#include <Core/Datatypes/Legacy/Field/MeshFactory.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/Legacy/Field/TriSurfMesh.h>
#include <Core/Datatypes/Mesh/VirtualMeshFacade.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>

#include <Core/GeometryPrimitives/Point.h>

#include <iostream>

using namespace SCIRun;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Geometry;

using ::testing::_;
using ::testing::DefaultValue;
using ::testing::Return;
using namespace boost::assign;

class TriSurfMeshFacadeTests : public ::testing::Test
{
protected:
  void SetUp() override
  {
    {
      FieldInformation fi("TriSurfMesh", LINEARDATA_E, "double");
      basicTriangleMesh_ = CreateMesh(fi);
      auto triangleVMesh = basicTriangleMesh_->vmesh();
      triangleVMesh->add_point(Point(0.0, 0.0, 0.0));
      triangleVMesh->add_point(Point(1.0, 0.0, 0.0));
      triangleVMesh->add_point(Point(0.5, 1.0, 0.0));

      VMesh::Node::array_type vdata;
      vdata += 0, 1, 2;
      triangleVMesh->add_elem(vdata);
    }

    {
      FieldInformation fi("TriSurfMesh", LINEARDATA_E, "double");
      cubeMesh_ = CreateMesh(fi);
      auto cubeVMesh = cubeMesh_->vmesh();
      cubeVMesh->add_point(Point(0.0, 1.0, 0.0));
      cubeVMesh->add_point(Point(0.0, 0.0, 0.0));
      cubeVMesh->add_point(Point(1.0, 1.0, 0.0));
      cubeVMesh->add_point(Point(1.0, 0.0, 0.0));
      cubeVMesh->add_point(Point(1.0, 0.0, -1.0));
      cubeVMesh->add_point(Point(1.0, 1.0, -1.0));
      cubeVMesh->add_point(Point(0.0, 1.0, -1.0));
      cubeVMesh->add_point(Point(0.0, 0.0, -1.0));

      VMesh::Node::array_type vdata1;
      vdata1 += 0, 1, 7;
      cubeVMesh->add_elem(vdata1);
      VMesh::Node::array_type vdata2;
      vdata2 += 0, 7, 6;
      cubeVMesh->add_elem(vdata2);
      VMesh::Node::array_type vdata3;
      vdata3 += 1, 0, 2;
      cubeVMesh->add_elem(vdata2);
      VMesh::Node::array_type vdata4;
      vdata4 += 1, 3, 2;
      cubeVMesh->add_elem(vdata4);
      VMesh::Node::array_type vdata5;
      vdata5 += 2, 3, 4;
      cubeVMesh->add_elem(vdata5);
      VMesh::Node::array_type vdata6;
      vdata6 += 2, 4, 5;
      cubeVMesh->add_elem(vdata6);
      VMesh::Node::array_type vdata7;
      vdata7 += 4, 7, 1;
      cubeVMesh->add_elem(vdata7);
      VMesh::Node::array_type vdata8;
      vdata8 += 4, 3, 1;
      cubeVMesh->add_elem(vdata8);
      VMesh::Node::array_type vdata9;
      vdata9 += 5, 6, 0;
      cubeVMesh->add_elem(vdata9);
      VMesh::Node::array_type vdata10;
      vdata10 += 5, 2, 0;
      cubeVMesh->add_elem(vdata10);
      VMesh::Node::array_type vdata11;
      vdata11 += 7, 6, 5;
      cubeVMesh->add_elem(vdata11);
      VMesh::Node::array_type vdata12;
      vdata12 += 7, 4, 5;
      cubeVMesh->add_elem(vdata12);
    }

    {
      FieldInformation fi("TriSurfMesh", LINEARDATA_E, "double");
      tetrahedronMesh_ = CreateMesh(fi);
      auto tetrahedronVMesh = tetrahedronMesh_->vmesh();
      tetrahedronVMesh->add_point(Point(1.0, 0.0, -0.707));
      tetrahedronVMesh->add_point(Point(-1.0, 0.0, -0.707));
      tetrahedronVMesh->add_point(Point(0.0, 1.0, 0.707));
      tetrahedronVMesh->add_point(Point(0.0, -1.0, 0.707));

      VMesh::Node::array_type vdata1;
      vdata1 += 0, 1, 2;
      tetrahedronVMesh->add_elem(vdata1);
      VMesh::Node::array_type vdata2;
      vdata2 += 0, 1, 3;
      tetrahedronVMesh->add_elem(vdata2);
      VMesh::Node::array_type vdata3;
      vdata3 += 1, 2, 3;
      tetrahedronVMesh->add_elem(vdata2);
      VMesh::Node::array_type vdata4;
      vdata4 += 0, 2, 3;
      tetrahedronVMesh->add_elem(vdata4);
    }
  }

  MeshHandle basicTriangleMesh_;
  MeshHandle cubeMesh_;
  MeshHandle tetrahedronMesh_;
};

/// @todo: move to utils file (duplicated in LatticeVolumeMeshFacadeTests.cc)
namespace
{
  template <typename T>
  std::string join(const T& list)
  {
    std::ostringstream oss;
    const int SIZE = list.size();
    for (int i = 0; i < SIZE; ++i)
    {
      oss << list[i];
      if (i < SIZE - 1)
        oss << ", ";
    }
    return oss.str();
  }
}

TEST_F(TriSurfMeshFacadeTests, BasicTriangleTest)
{
  ASSERT_TRUE(basicTriangleMesh_.get() != nullptr);

  auto facade(basicTriangleMesh_->getFacade());

  EXPECT_EQ(3, facade->numNodes());
  EXPECT_EQ(1, facade->numFaces());
  EXPECT_EQ(1, facade->numElements());
  EXPECT_EQ(3, facade->numEdges());
}

TEST_F(TriSurfMeshFacadeTests, CubeTest)
{
  ASSERT_TRUE(cubeMesh_.get() != nullptr);

  auto facade(cubeMesh_->getFacade());

  EXPECT_EQ(8, facade->numNodes());
  EXPECT_EQ(12, facade->numFaces());
  EXPECT_EQ(12, facade->numElements());
  EXPECT_EQ(18, facade->numEdges());
}

TEST_F(TriSurfMeshFacadeTests, TetrahedronTest)
{
  ASSERT_TRUE(tetrahedronMesh_.get() != nullptr);

  auto facade(tetrahedronMesh_->getFacade());

  EXPECT_EQ(4, facade->numNodes());
  EXPECT_EQ(4, facade->numFaces());
  EXPECT_EQ(4, facade->numElements());
  EXPECT_EQ(6, facade->numEdges());
}

TEST_F(TriSurfMeshFacadeTests, BasicTriangleEdgeIterationTest)
{
  auto facade(basicTriangleMesh_->getFacade());

  std::ostringstream ostr;
  for (const auto& edge : facade->edges())
  {
    auto nodesFromEdge = edge.nodeIndices();
    auto nodePoints = edge.nodePoints();
    ostr << "Edge " << edge.index() << " nodes=[" << nodesFromEdge[0] << " point=" << nodePoints[0].get_string()
    << ", " << nodesFromEdge[1] << " point=" << nodePoints[1].get_string() << "]" << std::endl;
  }

  EXPECT_EQ(
            "Edge 0 nodes=[2 point=[0.5, 1, 0], 0 point=[0, 0, 0]]\n"
            "Edge 1 nodes=[1 point=[1, 0, 0], 2 point=[0.5, 1, 0]]\n"
            "Edge 2 nodes=[0 point=[0, 0, 0], 1 point=[1, 0, 0]]\n"
            , ostr.str());
}

TEST_F(TriSurfMeshFacadeTests, BasicTriangleFaceIterationTest)
{
  auto facade(basicTriangleMesh_->getFacade());
  ASSERT_TRUE(facade.get() != nullptr);
  std::ostringstream ostr;
  for (const FaceInfo<VMesh>& face : facade->faces())
  {
    auto faceID = face.index();
    auto edges = face.edgeIndices();
    auto nodes = face.nodeIndices();
    ostr << "Face " << faceID << " edges=[" << join(edges) << "]" << std::endl;
    ostr << "Face " << faceID << " nodes=[" << join(nodes) << "]" << std::endl;
  }

  EXPECT_EQ("Face 0 edges=[2, 1, 0]\n"
            "Face 0 nodes=[0, 1, 2]\n"
            ,ostr.str());
}

TEST_F(TriSurfMeshFacadeTests, BasicTriangleNodeIterationTest)
{
  auto facade(basicTriangleMesh_->getFacade());
  ASSERT_TRUE(facade.get() != nullptr);
  std::ostringstream ostr;
  for (const NodeInfo<VMesh>& node : facade->nodes())
  {
    // special case, since this is essentially a 2D mesh with a single element,
    // the last edge value is not filled
    auto edges = node.edgeIndices();
    ostr << "Node " << node.index() << " point=" << node.point().get_string() << " edges=[" << join(edges) << "]" << std::endl;
  }

  EXPECT_EQ("Node 0 point=[0, 0, 0] edges=[2, 1, 0]\n"
            "Node 1 point=[1, 0, 0] edges=[2, 1, 0]\n"
            "Node 2 point=[0.5, 1, 0] edges=[2, 1, 0]\n"
            ,ostr.str());
}


//==============================================================================
//         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_ARITH_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_ARITH_HPP_INCLUDED

/*!
 * \file
**/

#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_swar
 * \defgroup boost_simd_swar_arith arith
 *
 * \par Description
 * arithmetic sequences with optionnal start
 * (default 0) and step (default 1).
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/arith.hpp>
 * \endcode
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class Target>
 *   meta::call<tag::arith_(as_<Target>)>::type
 *   arith();
 *   template <class A0, class Target>
 *   meta::call<tag::arith_(A0,as_<Target>)>::type
 *   arith(const A0 & seed);
 *   template <class A0, class A1, class Target>
 *   meta::call<tag::arith_(A0, A1, as_<Target>)>::type
 *   arith(const A0 & seed, const A1& step);
 * }
 * \endcode
 *
 * \param seed the initial value of the SIMD enumeration
 *
 * \return a vector \c v of the same value type as Target which values are
 * given by:
 *
 * \code
 * v = [ seed seed+step ... seed + (cardinal -1)*step ]
 * \endcode
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag arith_ of functor arith
     *        in namespace boost::simd::tag for toolbox boost.simd.arith
    **/
    struct arith_ : ext::unspecified_<arith_> { typedef ext::unspecified_<arith_> parent; };
  }

  template<class T> BOOST_FORCEINLINE
  typename  boost::dispatch::meta::
            call<tag::arith_(boost::dispatch::meta::as_<T>)
                            >::type
  arith()
  {
    typename boost::dispatch::make_functor<tag::arith_>::type callee;
    return callee(boost::dispatch::meta::as_<T>());
  }

  template<class T,class A0> BOOST_FORCEINLINE
  typename  boost::dispatch::meta::
            call<tag::arith_( A0 const&
                                , boost::dispatch::meta::as_<T>
                                )
                            >::type
  arith(A0 const& a0)
  {
    typename boost::dispatch::make_functor<tag::arith_, A0>::type callee;
    return callee(a0,boost::dispatch::meta::as_<T>());
  }

  template<class T,class A0, class A1> BOOST_FORCEINLINE
  typename  boost::dispatch::meta::
            call<tag::arith_( A0 const&,
                                  A1 const&,
                                  boost::dispatch::meta::as_<T>
                                )
                            >::type
  arith(A0 const& a0, A1 const& a1)
  {
    typename boost::dispatch::make_functor<tag::arith_, A0>::type callee;
    return callee(a0,a1,boost::dispatch::meta::as_<T>());
  }
} }

#endif