/*
 * Copyright 2017, 2018 Simon Rasmussen (refactor)
 *
 * Copyright 2015, 2016 Thomas Timm Andersen (original version)
 *
 * 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.
 */

#pragma once
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/ros.h>
#include <atomic>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/hardware_interface.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/rt_state.h"

class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
{
private:
  ros::NodeHandle nh_;
  ros::Time lastUpdate_;
  controller_manager::ControllerManager controller_;
  bool robot_state_received_;

  // state interfaces
  JointInterface joint_interface_;
  WrenchInterface wrench_interface_;
  // controller interfaces
  PositionInterface position_interface_;
  VelocityInterface velocity_interface_;

  // currently activated controller
  HardwareInterface* active_interface_;
  // map of switchable controllers
  std::map<std::string, HardwareInterface*> available_interfaces_;

  std::atomic<bool> service_enabled_;
  std::atomic<uint32_t> service_cooldown_;

  // helper functions to map interfaces
  template <typename T>
  void registerInterface(T* interface)
  {
    RobotHW::registerInterface<typename T::parent_type>(interface);
  }
  template <typename T>
  void registerControllereInterface(T* interface)
  {
    registerInterface(interface);
    available_interfaces_[T::INTERFACE_NAME] = interface;
  }

  void read(RTShared& state);
  bool update();
  bool write();
  void reset();

public:
  ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
                double max_vel_change, std::string tcp_link);
  virtual ~ROSController()
  {
  }
  // from RobotHW
  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                const std::list<hardware_interface::ControllerInfo>& stop_list);
  // from URRTPacketConsumer
  virtual void setupConsumer();
  virtual bool consume(RTState_V1_6__7& state)
  {
    read(state);
    return update();
  }
  virtual bool consume(RTState_V1_8& state)
  {
    read(state);
    return update();
  }
  virtual bool consume(RTState_V3_0__1& state)
  {
    read(state);
    return update();
  }
  virtual bool consume(RTState_V3_2__3& state)
  {
    read(state);
    return update();
  }

  virtual void onTimeout();

  virtual void onRobotStateChange(RobotState state);
};