// Copyright (C) 2019 by Joseph Mirabel, LAAS-CNRS.
//
// This file is part of the hpp-corbaserver.
//
// This software is provided "as is" without warranty of any kind,
// either expressed or implied, including but not limited to the
// implied warranties of fitness for a particular purpose.
//
// See the COPYING file for more information.

#ifndef HPP_CORE_PATH_PLANNERS_IDL
#define HPP_CORE_PATH_PLANNERS_IDL
#include <hpp/common.idl>

#include <hpp/core_idl/paths.idl>

module hpp
{
  module core_idl {
    interface Path;

    interface ConnectedComponent {
      HPP_EXPOSE_MEMORY_DEALLOCATION(Error)
      floatSeqSeq nodes () raises(Error);
      //* hpp::core::NodeVector_t nodes = getT()->nodes();
      //* hpp::core::matrix_t configs (nodes.size(),
      //*                              nodes[0]->configuration().size());
      //* size_type i = 0;
      //* for (hpp::core::NodeVector_t::const_iterator _node = nodes.begin();
      //*      _node != nodes.end(); ++_node)
      //*   configs.row(i++) = (*_node)->configuration();
      //* return corbaServer::matrixToFloatSeqSeq(configs);
    }; // interface ConnectedComponent

    typedef sequence<ConnectedComponent> ConnectedComponentSeq;

    interface Roadmap {
      HPP_EXPOSE_MEMORY_DEALLOCATION(Error)

      void clear () raises (Error);

      void addNode (in floatSeq config) raises (Error);

      void addNodeAndEdge (in floatSeq cfgfrom, in floatSeq cfgto, in Path path_) raises (Error);
      //* core::PathPtr_t path (corbaServer::reference_to_servant_base<core::Path>(server_, path_)->get());
      //* getT()->addEdge(getT()->addNode(corbaServer::floatSeqToVector(cfgfrom)),
      //*                 getT()->addNode(corbaServer::floatSeqToVector(cfgto  )), path);

      void addNodeAndEdges (in floatSeq cfgfrom, in floatSeq cfgto, in Path path_) raises (Error);
      //* core::PathPtr_t path (corbaServer::reference_to_servant_base<core::Path>(server_, path_)->get());
      //* getT()->addEdges(getT()->addNode(corbaServer::floatSeqToVector(cfgfrom)),
      //*                  getT()->addNode(corbaServer::floatSeqToVector(cfgto  )), path);

      /// \warning config should be of the correct size and normalized. It
      /// cannot be checked here because the robot is not accessible.
      floatSeq nearestNode (in floatSeq config, out value_type distance,
          in boolean reverse) raises (Error);
      //* if (getT()->nodes().size() == 0) throw std::logic_error(
      //*     "hpp::core::Roadmap::nearestNode: roadmap is empty");
      //* return corbaServer::vectorToFloatSeq(getT()->nearestNode(
      //*   corbaServer::floatSeqToVector(config), distance, reverse)->configuration());

      floatSeqSeq nearestNodes (in floatSeq config, out size_type k) raises (Error);
      //* hpp::core::Nodes_t nodes = getT()->nearestNodes(
      //*   corbaServer::floatSeqToVector(config), k);
      //* hpp::core::matrix_t configs (nodes.size(), config.length());
      //* size_type i = 0;
      //* for (hpp::core::Nodes_t::const_iterator _node = nodes.begin(); _node != nodes.end(); ++_node)
      //*   configs.row(i++) = (*_node)->configuration();
      //* return corbaServer::matrixToFloatSeqSeq(configs);

      size_type getNbNodes () raises (Error);
      //* return getT()->nodes().size();

      floatSeq getNode (in size_type i) raises (Error);
      //* return corbaServer::vectorToFloatSeq((*std::next(getT()->nodes().begin(), i))->configuration());

      size_type getNbEdges () raises (Error);
      //* return getT()->edges().size();

      Path getEdge (in size_type i) raises (Error);
      //* return corbaServer::makeServant<hpp::core_idl::Path_ptr> (server_,
      //*   new Path (server_, (*std::next(getT()->edges().begin(),i))->path()));
      ConnectedComponentSeq getConnectedComponents () raises(Error);
      //-> connectedComponents
    }; // interface Roadmap

    interface PathPlanner
    {
      HPP_EXPOSE_MEMORY_DEALLOCATION(Error)

      PathVector solve () raises (Error);

      void startSolve () raises (Error);

      void tryConnectInitAndGoals () raises (Error);

      void oneStep () raises (Error);

      PathVector computePath () raises (Error);

      PathVector finishSolve (in PathVector path) raises (Error);

      void interrupt () raises (Error);

      void maxIterations (in size_type n) raises (Error);

      void timeOut(in value_type seconds) raises (Error);

      Roadmap getRoadmap() raises (Error);
      //-> roadmap

      void stopWhenProblemIsSolved(in boolean enable) raises (Error);
    }; // interface PathPlanner

    interface PathOptimizer
    {
      HPP_EXPOSE_MEMORY_DEALLOCATION(Error)

      PathVector optimize (in PathVector path) raises (Error);

      void interrupt () raises (Error);

      void maxIterations (in size_type n) raises (Error);

      void timeOut(in value_type seconds) raises (Error);
    }; // interface PathOptimizer

  }; // module core
}; // module hpp

//* #include <iterator>
//* #include <hpp/pinocchio/serialization.hh>
//* #include <hpp/core/connected-component.hh>
//* #include <hpp/core/path-planner.hh>
//* #include <hpp/core/path-optimizer.hh>
//* #include <hpp/core/roadmap.hh>
//* #include <hpp/core/edge.hh>
//* #include <hpp/core/node.hh>
//* #include <hpp/core_idl/paths.hh>

#endif // HPP_CORE_PATH_PLANNERS_IDL
