#include "ros2_control_can/ros2_control_can.h"


#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"

#include "rclcpp/rclcpp.hpp"
#include <math.h>

#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

#include <linux/can.h>
#include <linux/can/raw.h>

#define RADSTORPM(a)(a*60.0/(2.0*M_PI))
#define RPMSTORADS(a)(a*2.0*M_PI/60.0)
#define REVSTORAD(a)(a*2.0*M_PI)
#define RADSTOREV(a)(a/(2.0*M_PI))

namespace ros2_control_can
{


Ros2ControlCAN::Ros2ControlCAN()
    : logger_(rclcpp::get_logger("Ros2ControlCAN"))
{}

CallbackReturn Ros2ControlCAN::on_init(const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
  {
    return CallbackReturn::ERROR;
  }

  RCLCPP_INFO(logger_, "Configuring...");

  time_ = std::chrono::system_clock::now();

  cfg_.front_left_wheel_name = info_.hardware_parameters["front_left_wheel_name"];
  cfg_.front_right_wheel_name = info_.hardware_parameters["front_right_wheel_name"];
  cfg_.rear_left_wheel_name = info_.hardware_parameters["rear_left_wheel_name"];
  cfg_.rear_right_wheel_name = info_.hardware_parameters["rear_right_wheel_name"];
  cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]);
  cfg_.device = info_.hardware_parameters["device"];

  // Set up the wheels
  f_l_wheel_.setup(cfg_.front_left_wheel_name);
  f_r_wheel_.setup(cfg_.front_right_wheel_name);
  r_l_wheel_.setup(cfg_.front_left_wheel_name);
  r_r_wheel_.setup(cfg_.front_right_wheel_name);

  // init tx frame
  can1__main_ft24_jetson_tx_init(&tx_frame);

  RCLCPP_INFO(logger_, "Finished Configuration");

  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> Ros2ControlCAN::export_state_interfaces()
{
  // We need to set up a position and a velocity interface for each wheel

  std::vector<hardware_interface::StateInterface> state_interfaces;

  state_interfaces.emplace_back(hardware_interface::StateInterface(f_l_wheel_.name, hardware_interface::HW_IF_POSITION, &f_l_wheel_.steer));
  state_interfaces.emplace_back(hardware_interface::StateInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
  state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
  state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));
  state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_POSITION, &r_l_wheel_.pos));
  state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_r_wheel_.pos));

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> Ros2ControlCAN::export_command_interfaces()
{
  // We need to set up a velocity command interface for each wheel

  std::vector<hardware_interface::CommandInterface> command_interfaces;

  command_interfaces.emplace_back(hardware_interface::CommandInterface(f_l_wheel_.name, hardware_interface::HW_IF_POSITION, &f_l_wheel_.steer));
  command_interfaces.emplace_back(hardware_interface::CommandInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
  command_interfaces.emplace_back(hardware_interface::CommandInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
  command_interfaces.emplace_back(hardware_interface::CommandInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));

  return command_interfaces;
}

CallbackReturn Ros2ControlCAN::on_activate(const rclcpp_lifecycle::State & previous_state)
{
  RCLCPP_INFO(logger_, "Starting Controller...");

  if ((socket_instance = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
    perror("Socket");
    return CallbackReturn::ERROR;
  }

  struct ifreq ifr;
  const char *cstr = cfg_.device.c_str();
  strcpy(ifr.ifr_name, cstr );
  ioctl(socket_instance, SIOCGIFINDEX, &ifr);

  struct sockaddr_can addr;
  memset(&addr, 0, sizeof(addr));
  addr.can_family = AF_CAN;
  addr.can_ifindex = ifr.ifr_ifindex;
  if (bind(socket_instance, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
    perror("Bind");
    return CallbackReturn::ERROR;
  }

  struct can_filter rfilter[1];
  rfilter[0].can_id   = 0x550;
  rfilter[0].can_mask = 0xFF0;
  //rfilter[1].can_id   = 0x200;
  //rfilter[1].can_mask = 0x700;
  setsockopt(socket_instance, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));

    return CallbackReturn::SUCCESS;
  }

CallbackReturn Ros2ControlCAN::on_deactivate(const rclcpp_lifecycle::State & previous_state)
{
  RCLCPP_INFO(logger_, "Stopping Controller...");

  if (close(socket_instance) < 0) {
    perror("Close");
    return CallbackReturn::ERROR;
  }

  return CallbackReturn::SUCCESS;
}

return_type Ros2ControlCAN::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
{

  // TODO fix chrono duration

  // Calculate time delta
  auto new_time = std::chrono::system_clock::now();
  std::chrono::duration<double> diff = new_time - time_;
  double deltaSeconds = diff.count();
  time_ = new_time;


  if (false)
  {
    return return_type::ERROR;
  }

  // convert rpm to rad/s
  //l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
  //r_wheel_.vel = RPMSTORADS(r_wheel_.vel);


  return return_type::OK;  
}

return_type Ros2ControlCAN::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
{
  if (false)
  {
    return return_type::ERROR;
  }
  
  // send can frame
  tx_frame.jetson_steering_angle = can1__main_ft24_jetson_tx_jetson_steering_angle_encode((f_l_wheel_.steer + f_r_wheel_.steer)/2.0);
  
  // pack tx_frame
  int tx_size = can1__main_ft24_jetson_tx_pack(can_buffer, &tx_frame, 64);

  if (tx_size < 0) {
    perror("Pack");
    return return_type::ERROR;
  }

  // send tx_frame
  struct can_frame frame;
  frame.can_id = 0x550;
  frame.can_dlc = tx_size;
  memcpy(frame.data, can_buffer, tx_size);
  if (::write(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
    perror("Write");
    return return_type::ERROR;
  }

  return return_type::OK;  
}

} // namespace ros2_control_can

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  ros2_control_can::Ros2ControlCAN,
  hardware_interface::SystemInterface
)
