#include "idyntree_sensors.h"
#include "error_utilities.h"

#include <pybind11/operators.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <iDynTree/Sensors/AccelerometerSensor.h>
#include <iDynTree/Sensors/Sensors.h>
#include <iDynTree/Sensors/SixAxisForceTorqueSensor.h>

namespace iDynTree {
namespace bindings {
namespace {

class SensorsListHelper {
 public:
   static void addSensor(SensorsList* this_, const Sensor& sensor) {
     if (this_->addSensor(sensor)) {
       raisePythonException(PyExc_ValueError, "Failed to add sensor.");
     }
   }
};

namespace py = ::pybind11;

}  // namespace

void iDynTreeSensorsBindings(pybind11::module& module) {
  py::enum_<SensorType>(module, "SensorType")
      .value("SIX_AXIS_FORCE_TORQUE", SIX_AXIS_FORCE_TORQUE)
      .value("ACCELEROMETER", ACCELEROMETER)
      .value("GYROSCOPE", GYROSCOPE)
      .value("THREE_AXIS_ANGULAR_ACCELEROMETER",
             THREE_AXIS_ANGULAR_ACCELEROMETER)
      .value("THREE_AXIS_FORCE_TORQUE_CONTACT", THREE_AXIS_FORCE_TORQUE_CONTACT)
      .export_values();

  py::class_<Sensor>(module, "_Sensor")
      .def_property("name", &Sensor::getName, &Sensor::setName)
      .def_property_readonly("sensor_type", &Sensor::getSensorType);

  py::class_<LinkSensor, Sensor>(module, "_LinkSensor")
      .def_property("parent_link", &LinkSensor::getParentLink,
                    &LinkSensor::setParentLink)
      .def_property("parent_link_index", &LinkSensor::getParentLinkIndex,
                    &LinkSensor::setParentLinkIndex)
      .def_property("link_sensor_transform",
                    &LinkSensor::getLinkSensorTransform,
                    &LinkSensor::setLinkSensorTransform);

  py::class_<AccelerometerSensor, LinkSensor>(module, "AccelerometerSensor")
      .def(py::init());

  py::class_<JointSensor, Sensor>(module, "_JointSensor")
      .def_property("parent_joint", &JointSensor::getParentJoint,
                    &JointSensor::setParentJoint)
      .def_property("parent_joint_index", &JointSensor::getParentJointIndex,
                    &JointSensor::setParentJointIndex);

  py::class_<SixAxisForceTorqueSensor, JointSensor>(module,
                                                    "SixAxisForceTorqueSensor")
      .def(py::init());

  py::class_<SensorsList>(module, "SensorsList")
      .def(py::init())
      .def("add_sensor", &SensorsListHelper::addSensor)
      .def("get_nr_of_sensors", &SensorsList::getNrOfSensors);
}

}  // namespace bindings
}  // namespace iDynTree
