// Copyright (c) 2023 Franka Emika GmbH
//
// 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.

#include <gmock/gmock.h>
#include <exception>
#include <rclcpp/rclcpp.hpp>

#include <franka_hardware/franka_hardware_interface.hpp>
#include <franka_hardware/model.hpp>
#include <franka_hardware/robot.hpp>

#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>

const std::string k_position_controller{"position"};
const std::string k_velocity_controller{"velocity"};
const std::string k_effort_controller{"effort"};
const std::string k_joint_name{"joint"};
const size_t k_number_of_joints{7};
const double k_EPS{1e-5};

class MockModel : public franka_hardware::Model {};

class MockRobot : public franka_hardware::Robot {
 public:
  MOCK_METHOD(void, initializeContinuousReading, (), (override));
  MOCK_METHOD(void, stopRobot, (), (override));
  MOCK_METHOD(void, initializeTorqueControl, (), (override));
  MOCK_METHOD(franka::RobotState, read, (), (override));
  MOCK_METHOD(MockModel*, getModel, (), (override));
};

auto createHardwareInfo() -> hardware_interface::HardwareInfo {
  hardware_interface::HardwareInfo info;
  std::unordered_map<std::string, std::string> hw_params;
  hw_params["robot_ip"] = "dummy_ip";

  info.hardware_parameters = hw_params;
  hardware_interface::InterfaceInfo command_interface, effort_state_interface,
      position_state_interface, velocity_state_interface;

  effort_state_interface.name = hardware_interface::HW_IF_EFFORT;
  position_state_interface.name = hardware_interface::HW_IF_POSITION;
  velocity_state_interface.name = hardware_interface::HW_IF_VELOCITY;

  std::vector<hardware_interface::InterfaceInfo> state_interfaces = {
      position_state_interface, velocity_state_interface, effort_state_interface};

  for (auto i = 0U; i < k_number_of_joints; i++) {
    hardware_interface::ComponentInfo joint;

    joint.name = k_joint_name + std::to_string(i + 1);

    command_interface.name = k_effort_controller;

    joint.command_interfaces.push_back(command_interface);
    joint.state_interfaces = state_interfaces;

    info.joints.push_back(joint);
  }

  return info;
}

TEST(FrankaHardwareInterfaceTest, when_on_init_called_expect_success) {
  auto mock_robot = std::make_unique<MockRobot>();
  const hardware_interface::HardwareInfo info = createHardwareInfo();
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
  auto return_type = franka_hardware_interface.on_init(info);

  EXPECT_EQ(return_type,
            rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
}

TEST(FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_set_when_read_called_return_ok) {
  franka::RobotState robot_state;
  MockModel mock_model;
  MockModel* model_address = &mock_model;

  auto mock_robot = std::make_unique<MockRobot>();
  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));

  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  auto time = rclcpp::Time(0, 0);
  auto duration = rclcpp::Duration(0, 0);
  auto return_type = franka_hardware_interface.read(time, duration);
  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
}

TEST(
    FrankaHardwareInterfaceTest,
    given_that_the_robot_interfaces_are_set_when_call_export_state_return_zero_values_and_correct_interface_names) {
  franka::RobotState robot_state;
  const size_t state_interface_size =
      23;  // position, effort and velocity states for every joint + robot state and model
  auto mock_robot = std::make_unique<MockRobot>();
  MockModel mock_model;
  MockModel* model_address = &mock_model;

  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  auto time = rclcpp::Time(0);
  auto duration = rclcpp::Duration(0, 0);
  auto return_type = franka_hardware_interface.read(time, duration);
  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
  auto states = franka_hardware_interface.export_state_interfaces();
  size_t joint_index = 0;

  // Get all the states except the last two reserved for robot state
  for (size_t i = 0; i < states.size() - 2; i++) {
    if (i % 3 == 0) {
      joint_index++;
    }
    const std::string joint_name = k_joint_name + std::to_string(joint_index);
    if (i % 3 == 0) {
      EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_position_controller);
    } else if (i % 3 == 1) {
      EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_velocity_controller);
    } else {
      EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_effort_controller);
    }
    EXPECT_EQ(states[i].get_value(), 0.0);
  }

  EXPECT_EQ(states.size(), state_interface_size);
}

TEST(
    FrankaHardwareInterfaceTest,
    given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_model_interface_exists) {
  franka::RobotState robot_state;
  const size_t state_interface_size =
      23;  // position, effort and velocity states for every joint + robot state and model
  auto mock_robot = std::make_unique<MockRobot>();

  MockModel mock_model;
  MockModel* model_address = &mock_model;

  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  auto time = rclcpp::Time(0);
  auto duration = rclcpp::Duration(0, 0);
  auto return_type = franka_hardware_interface.read(time, duration);
  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
  auto states = franka_hardware_interface.export_state_interfaces();
  EXPECT_EQ(states[state_interface_size - 1].get_name(),
            "panda/robot_model");  // Last state interface is the robot model state
  EXPECT_NEAR(states[state_interface_size - 1].get_value(),
              *reinterpret_cast<double*>(&model_address),
              k_EPS);  // testing that the casted mock_model ptr
                       // is correctly pushed to state interface
}

TEST(
    FrankaHardwareInterfaceTest,
    given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_state_interface_exists) {
  const size_t state_interface_size =
      23;  // position, effort and velocity states for every joint + robot state and model
  auto mock_robot = std::make_unique<MockRobot>();

  franka::RobotState robot_state;
  franka::RobotState* robot_state_address = &robot_state;

  MockModel mock_model;
  MockModel* model_address = &mock_model;

  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  auto time = rclcpp::Time(0);
  auto duration = rclcpp::Duration(0, 0);
  auto return_type = franka_hardware_interface.read(time, duration);
  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
  auto states = franka_hardware_interface.export_state_interfaces();
  EXPECT_EQ(states[state_interface_size - 2].get_name(),
            "panda/robot_state");  // Last state interface is the robot model state
  EXPECT_NEAR(states[state_interface_size - 2].get_value(),
              *reinterpret_cast<double*>(&robot_state_address),
              k_EPS);  // testing that the casted robot state ptr
                       // is correctly pushed to state interface
}

TEST(FrankaHardwareInterfaceTest,
     when_prepare_command_mode_interface_for_stop_effort_interfaces_expect_ok) {
  auto mock_robot = std::make_unique<MockRobot>();
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  std::vector<std::string> stop_interface;

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    stop_interface.push_back(joint_name + "/" + k_effort_controller);
  }
  std::vector<std::string> start_interface = {};
  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);
}

TEST(
    FrankaHardwareInterfaceTest,
    when_prepare_command_mode_interface_is_called_with_invalid_start_interface_number_expect_throw) {
  auto mock_robot = std::make_unique<MockRobot>();
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  std::vector<std::string> stop_interface;

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    stop_interface.push_back(joint_name + "/" + k_effort_controller);
  }
  std::vector<std::string> start_interface = {"fr3_joint1/effort"};
  EXPECT_THROW(
      franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
      std::invalid_argument);
}

TEST(FrankaHardwareInterfaceTest,
     when_prepare_command_mode_interface_for_start_effort_interfaces_expect_ok) {
  auto mock_robot = std::make_unique<MockRobot>();
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  std::vector<std::string> start_interface;

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    start_interface.push_back(joint_name + "/" + k_effort_controller);
  }

  std::vector<std::string> stop_interface = {};

  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);
}

TEST(
    FrankaHardwareInterfaceTest,
    when_prepare_command_mode_interface_is_called_with_invalid_stop_interface_number_expect_throw) {
  auto mock_robot = std::make_unique<MockRobot>();
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  std::vector<std::string> start_interface;

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    start_interface.push_back(joint_name + "/" + k_effort_controller);
  }

  std::vector<std::string> stop_interface = {"fr3_joint1/effort"};

  EXPECT_THROW(
      franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
      std::invalid_argument);
}

TEST(FrankaHardwareIntefaceTest, when_write_called_expect_ok) {
  auto mock_robot = std::make_unique<MockRobot>();
  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);

  const auto time = rclcpp::Time(0, 0);
  const auto duration = rclcpp::Duration(0, 0);

  EXPECT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK);
}

TEST(FrankaHardwareInterfaceTest, when_on_activate_called_expect_success) {
  franka::RobotState robot_state;

  MockModel mock_model;
  MockModel* model_address = &mock_model;

  auto mock_robot = std::make_unique<MockRobot>();
  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));

  EXPECT_CALL(*mock_robot, initializeContinuousReading());

  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  EXPECT_EQ(franka_hardware_interface.on_activate(rclcpp_lifecycle::State()),
            rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
}

TEST(FrankaHardwareInterfaceTest, when_on_deactivate_called_expect_success) {
  franka::RobotState robot_state;

  auto mock_robot = std::make_unique<MockRobot>();
  EXPECT_CALL(*mock_robot, stopRobot());

  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  EXPECT_EQ(franka_hardware_interface.on_deactivate(rclcpp_lifecycle::State()),
            rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
}

TEST(FrankaHardwareInterfaceTest,
     given_start_effort_interface_prepared_when_perform_comamnd_mode_switch_called_expect_ok) {
  auto mock_robot = std::make_unique<MockRobot>();
  EXPECT_CALL(*mock_robot, stopRobot());
  EXPECT_CALL(*mock_robot, initializeTorqueControl());

  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  std::vector<std::string> start_interface;

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    start_interface.push_back(joint_name + "/" + k_effort_controller);
  }

  std::vector<std::string> stop_interface = {};

  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);

  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);
}

TEST(FrankaHardwareInterfaceTest,
     given_that_effort_control_started_perform_command_mode_switch_stop_expect_ok) {
  auto mock_robot = std::make_unique<MockRobot>();
  EXPECT_CALL(*mock_robot, stopRobot()).Times(2).WillRepeatedly(testing::Return());
  EXPECT_CALL(*mock_robot, initializeTorqueControl());
  EXPECT_CALL(*mock_robot, initializeContinuousReading());

  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));

  const auto hardware_info = createHardwareInfo();
  franka_hardware_interface.on_init(hardware_info);
  std::vector<std::string> start_interface;

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    start_interface.push_back(joint_name + "/" + k_effort_controller);
  }

  std::vector<std::string> stop_interface = {};

  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);

  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);

  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
    const std::string joint_name = k_joint_name + std::to_string(i);
    stop_interface.push_back(joint_name + "/" + k_effort_controller);
  }

  start_interface.clear();

  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);

  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
            hardware_interface::return_type::OK);
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}