// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#include <map>
#include <vector>

#include <iDynTree/Sensors.h>
#include <iDynTree/Wrench.h>

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

#include <iDynTree/Model.h>
#include <iDynTree/VectorDynSize.h>

#include <cassert>
#include <iostream>

namespace iDynTree
{

Sensor::~Sensor()
{
}

JointSensor::~JointSensor()
{
}

LinkSensor::~LinkSensor()
{
}

///////////////////////////////////////////////////////////////////////////////
///// SensorList
///////////////////////////////////////////////////////////////////////////////

struct SensorsList::SensorsListPimpl
{
    std::vector<std::vector<Sensor*>> allSensors;
    typedef std::map<std::string, std::ptrdiff_t> SensorNameToIndexMap;
    std::vector<SensorNameToIndexMap> sensorsNameToIndex;
};

SensorsList::SensorsList()
    : pimpl(new SensorsListPimpl())
{
    // resize datastructures;
    this->pimpl->allSensors.resize(NR_OF_SENSOR_TYPES, std::vector<Sensor*>(0));
    this->pimpl->sensorsNameToIndex.resize(NR_OF_SENSOR_TYPES);
}

void SensorsList::constructor(const SensorsList& other)
{
    this->pimpl = new SensorsListPimpl();
    this->pimpl->allSensors.resize(NR_OF_SENSOR_TYPES, std::vector<Sensor*>(0));
    this->pimpl->sensorsNameToIndex.resize(NR_OF_SENSOR_TYPES);
    for (int sens_type = 0; sens_type < NR_OF_SENSOR_TYPES; sens_type++)
    {
        for (std::size_t sens = 0; sens < other.getNrOfSensors((SensorType)sens_type); sens++)
        {
            this->pimpl->allSensors[sens_type].push_back(
                other.pimpl->allSensors[sens_type][sens]->clone());
            std::string sensor_name = other.getSensor((SensorType)sens_type, sens)->getName();
            this->pimpl->sensorsNameToIndex[sens_type].insert(
                std::pair<std::string, std::ptrdiff_t>(sensor_name, sens));
        }
    }
}

SensorsList::SensorsList(const iDynTree::SensorsList& other)
{
    constructor(other);
}

SensorsList& SensorsList::operator=(const SensorsList& other)
{
    if (this != &other)
    {
        destructor();
        constructor(other);
    }
    return *this;
}

void SensorsList::destructor()
{
    for (int sensor_type = 0; sensor_type < NR_OF_SENSOR_TYPES; sensor_type++)
    {
        for (size_t sensor_index = 0; sensor_index < this->pimpl->allSensors[sensor_type].size();
             sensor_index++)
        {
            delete this->pimpl->allSensors[sensor_type][sensor_index];
        }
    }
    this->pimpl->allSensors.resize(0);
    this->pimpl->sensorsNameToIndex.resize(0);

    delete this->pimpl;
    this->pimpl = 0;
}

SensorsList::~SensorsList()
{
    this->destructor();
}

std::ptrdiff_t SensorsList::addSensor(const Sensor& sensor)
{
    Sensor* newSensor = sensor.clone();
    if (!newSensor->isValid())
    {
        std::cerr << "[ERR] SensorsTree::addSensor error : sensor  " << sensor.getName()
                  << "  isValid() method returns false" << std::endl;
        delete newSensor;
        return -1;
    }

    if (!(newSensor->getSensorType() >= 0))
    {
        std::cerr << "[ERR] SensorsTree::addSensor error : sensor  " << sensor.getName()
                  << " has an unknown sensor type " << newSensor->getSensorType() << std::endl;
        delete newSensor;
        return -1;
    }

    this->pimpl->allSensors[newSensor->getSensorType()].push_back(newSensor);
    std::size_t new_index = this->pimpl->allSensors[newSensor->getSensorType()].size() - 1;
    this->pimpl->sensorsNameToIndex[newSensor->getSensorType()].insert(
        std::pair<std::string, size_t>(newSensor->getName(), new_index));

    return new_index;
}

bool SensorsList::getSerialization(const SensorType& sensor_type,
                                   std::vector<std::string>& serializaton)
{
    serializaton.resize(0);
    for (size_t sensor_index = 0; sensor_index < this->pimpl->allSensors[sensor_type].size();
         sensor_index++)
    {
        std::string sensorName = this->pimpl->allSensors[sensor_type][sensor_index]->getName();
        serializaton.push_back(sensorName);
    }

    return true;
}

bool SensorsList::setSerialization(const SensorType& sensor_type,
                                   const std::vector<std::string>& serializaton)
{
    if (serializaton.size() != this->getNrOfSensors(sensor_type))
    {
        std::cerr << "[ERROR] SensorsTree::setSerialization error : wrong size of serializaton "
                     "vector"
                  << std::endl;
        return false;
    }

    std::vector<Sensor*> newVecSensors(serializaton.size());

    for (size_t i = 0; i < serializaton.size(); i++)
    {
        std::ptrdiff_t oldSensIndex = getSensorIndex(sensor_type, serializaton[i]);
        if (oldSensIndex == -1)
        {
            std::cerr << "[ERROR] SensorsTree::setSerialization error : sensor " << serializaton[i]
                      << " not found in sensor list." << std::endl;
            return false;
        }
        newVecSensors[i] = this->getSensor(sensor_type, oldSensIndex);
    }

    this->pimpl->allSensors[sensor_type] = newVecSensors;

    return true;
}

std::size_t SensorsList::getNrOfSensors(const SensorType& sensor_type) const
{
    return this->pimpl->allSensors[sensor_type].size();
}

bool SensorsList::getSensorIndex(const SensorType& sensor_type,
                                 const std::string& _sensor_name,
                                 std::ptrdiff_t& sensor_index) const
{
    SensorsListPimpl::SensorNameToIndexMap::const_iterator it;
    it = this->pimpl->sensorsNameToIndex[sensor_type].find(_sensor_name);
    if (it == this->pimpl->sensorsNameToIndex[sensor_type].end())
    {
        std::cerr << "[ERROR] getSensorIndex did not find sensor " << _sensor_name << std::endl;
        return false;
    } else
    {
        sensor_index = it->second;
        return true;
    }
}

std::ptrdiff_t
SensorsList::getSensorIndex(const SensorType& sensor_type, const std::string& _sensor_name) const
{
    std::ptrdiff_t retVal;
    bool ok = getSensorIndex(sensor_type, _sensor_name, retVal);

    if (!ok)
    {
        retVal = -1;
    }

    return retVal;
}

Sensor* SensorsList::getSensor(const SensorType& sensor_type, std::ptrdiff_t sensor_index) const
{
    if (sensor_index < (int)getNrOfSensors(sensor_type) && sensor_index >= 0)
    {
        return this->pimpl->allSensors[sensor_type][sensor_index];
    } else
    {
        std::cerr << "[ERROR] getSensor did not find sensor " << sensor_index << " of type "
                  << sensor_type << std::endl;
        return 0;
    }
}

size_t SensorsList::getSizeOfAllSensorsMeasurements() const
{
    size_t res = 0;
    for (int i = (int)SIX_AXIS_FORCE_TORQUE; i < NR_OF_SENSOR_TYPES; i++)
    {
        SensorType type = (SensorType)i;
        res += getSensorTypeSize(type) * getNrOfSensors(type);
    }
    return res;
}

bool SensorsList::isConsistent(const Model& model) const
{
    bool isConsistent = true;
    for (SensorsList::ConstIterator it = this->allSensorsIterator(); it.isValid(); ++it)
    {
        bool isCurrentSensorConsistent = (*it)->isConsistent(model);
        isConsistent = isConsistent && isCurrentSensorConsistent;
    }
    return isConsistent;
}

bool SensorsList::removeAllSensorsOfType(const iDynTree::SensorType& sensor_type)
{
    // data is cloned when sensors are added. We have to release the associated memory
    for (std::vector<Sensor*>::iterator it = this->pimpl->allSensors[sensor_type].begin();
         it != this->pimpl->allSensors[sensor_type].end();
         ++it)
    {
        delete *it;
    }
    this->pimpl->allSensors[sensor_type].clear();
    this->pimpl->sensorsNameToIndex[sensor_type].clear();
    return true;
}

bool SensorsList::removeSensor(const SensorType& sensor_type, const std::ptrdiff_t sensor_index)
{
    std::vector<Sensor*>& typeVector = this->pimpl->allSensors[sensor_type];
    if (sensor_index >= static_cast<std::ptrdiff_t>(typeVector.size()))
    {
        return false;
    }
    Sensor* s = typeVector[sensor_index];
    typeVector.erase(typeVector.begin() + sensor_index);
    SensorsListPimpl::SensorNameToIndexMap& nameToIndex
        = this->pimpl->sensorsNameToIndex[sensor_type];
    // We have to rebuild the name->index map as indices have changed
    nameToIndex.clear();
    for (size_t index = 0; index < typeVector.size(); ++index)
    {
        nameToIndex.insert(
            SensorsListPimpl::SensorNameToIndexMap::value_type(typeVector[index]->getName(),
                                                               index));
    }

    delete s;
    return true;
}

bool SensorsList::removeSensor(const iDynTree::SensorType& sensor_type,
                               const std::string& _sensor_name)
{
    std::ptrdiff_t index = getSensorIndex(sensor_type, _sensor_name);
    if (index < 0)
        return false;
    return removeSensor(sensor_type, index);
}

// Iterator implementation
SensorsList::Iterator SensorsList::allSensorsIterator()
{
    Iterator iterator(this->pimpl->allSensors);
    return iterator;
}

SensorsList::ConstIterator SensorsList::allSensorsIterator() const
{
    ConstIterator iterator(this->pimpl->allSensors);
    return iterator;
}

SensorsList::TypedIterator
SensorsList::sensorsIteratorForType(const iDynTree::SensorType& sensor_type)
{
    TypedIterator iterator(this->pimpl->allSensors[sensor_type]);
    return iterator;
}

SensorsList::ConstTypedIterator
SensorsList::sensorsIteratorForType(const iDynTree::SensorType& sensor_type) const
{
    ConstTypedIterator iterator(this->pimpl->allSensors[sensor_type]);
    return iterator;
}

SensorsList::TypedIterator::TypedIterator(std::vector<Sensor*>& list)
    : iteratingList(list)
{
    internalIterator = iteratingList.begin();
}

SensorsList::TypedIterator& SensorsList::TypedIterator::operator++()
{
    ++internalIterator;
    return *this;
}
SensorsList::TypedIterator SensorsList::TypedIterator::operator++(int)
{
    SensorsList::TypedIterator previous(*this);
    this->operator++();
    return previous;
}

bool SensorsList::TypedIterator::operator==(const SensorsList::TypedIterator& s) const
{
    return internalIterator == s.internalIterator;
}

bool SensorsList::TypedIterator::operator==(const SensorsList::ConstTypedIterator& s) const
{
    return internalIterator == s.internalIterator;
}

SensorsList::TypedIterator::reference SensorsList::TypedIterator::operator*() const
{
    return *internalIterator;
}
SensorsList::TypedIterator::pointer SensorsList::TypedIterator::operator->() const
{
    return internalIterator.operator->();
}

bool SensorsList::TypedIterator::isValid() const
{
    return internalIterator >= iteratingList.begin() && internalIterator < iteratingList.end();
}

SensorsList::ConstTypedIterator::ConstTypedIterator(std::vector<Sensor*>& list)
    : iteratingList(list)
{
    constructor();
}

SensorsList::ConstTypedIterator::ConstTypedIterator(const TypedIterator& it)
    : iteratingList(it.iteratingList)
{
    constructor();
}

void SensorsList::ConstTypedIterator::constructor()
{
    internalIterator = iteratingList.begin();
}

SensorsList::ConstTypedIterator& SensorsList::ConstTypedIterator::operator++()
{
    ++internalIterator;
    return *this;
}
SensorsList::ConstTypedIterator SensorsList::ConstTypedIterator::operator++(int)
{
    SensorsList::ConstTypedIterator previous(*this);
    this->operator++();
    return previous;
}

bool SensorsList::ConstTypedIterator::operator==(const SensorsList::ConstTypedIterator& s) const
{
    return internalIterator == s.internalIterator;
}

bool SensorsList::ConstTypedIterator::operator==(const SensorsList::TypedIterator& s) const
{
    return internalIterator == s.internalIterator;
}

SensorsList::ConstTypedIterator::reference SensorsList::ConstTypedIterator::operator*() const
{
    return internalIterator.operator*();
}
SensorsList::ConstTypedIterator::pointer SensorsList::ConstTypedIterator::operator->() const
{
    return internalIterator.operator->();
}

bool SensorsList::ConstTypedIterator::isValid() const
{
    return internalIterator >= iteratingList.begin() && internalIterator < iteratingList.end();
}

SensorsList::Iterator::Iterator(std::vector<std::vector<Sensor*>>& list)
    : iteratingList(list)
{
    // Start from the beginning
    externalIterator = iteratingList.begin();
    // While external list is empty, skip to the next one
    while (externalIterator != iteratingList.end() && externalIterator->empty())
    {
        ++externalIterator;
    }
    // if the iterator is still valid assign the internal
    // otherwise the iterator itself is no longer valid
    if (externalIterator != iteratingList.end())
    {
        internalIterator = externalIterator->begin();
    }
}

SensorsList::Iterator& SensorsList::Iterator::operator++()
{
    ++internalIterator;
    if (internalIterator >= externalIterator->end())
    {
        // end of inner list.
        // Move to next list
        do
        {
            ++externalIterator;
        } while (externalIterator != iteratingList.end() && externalIterator->empty());

        if (externalIterator != iteratingList.end())
        {
            internalIterator = externalIterator->begin();
        }
    }
    return *this;
}
SensorsList::Iterator SensorsList::Iterator::operator++(int)
{
    SensorsList::Iterator previous(*this);
    this->operator++();
    return previous;
}

bool SensorsList::Iterator::operator==(const SensorsList::Iterator& s) const
{
    return internalIterator == s.internalIterator;
}

bool SensorsList::Iterator::operator==(const SensorsList::ConstIterator& s) const
{
    return internalIterator == s.internalIterator;
}

SensorsList::Iterator::reference SensorsList::Iterator::operator*() const
{
    return *internalIterator;
}
SensorsList::Iterator::pointer SensorsList::Iterator::operator->() const
{
    return internalIterator.operator->();
}

bool SensorsList::Iterator::isValid() const
{
    return
        // Check for external list consistency
        externalIterator >= iteratingList.begin() && externalIterator < iteratingList.end() &&
        // Check for internal list consistency
        internalIterator >= externalIterator->begin() && internalIterator < externalIterator->end();
}

SensorsList::ConstIterator::ConstIterator(std::vector<std::vector<Sensor*>>& list)
    : iteratingList(list)
{
    constructor();
}

SensorsList::ConstIterator::ConstIterator(const SensorsList::Iterator& it)
    : iteratingList(it.iteratingList)
{
    constructor();
}

void SensorsList::ConstIterator::constructor()
{
    // Start from the beginning
    externalIterator = iteratingList.begin();
    // While external list is empty, skip to the next one
    while (externalIterator != iteratingList.end() && externalIterator->empty())
    {
        ++externalIterator;
    }
    // if the iterator is still valid assign the internal
    // otherwise the iterator itself is no longer valid
    if (externalIterator != iteratingList.end())
    {
        internalIterator = externalIterator->begin();
    }
}

SensorsList::ConstIterator& SensorsList::ConstIterator::operator++()
{
    ++internalIterator;
    if (internalIterator >= externalIterator->end())
    {
        // end of inner list.
        // Move to next list
        do
        {
            ++externalIterator;
        } while (externalIterator != iteratingList.end() && externalIterator->empty());

        if (externalIterator != iteratingList.end())
        {
            internalIterator = externalIterator->begin();
        }
    }
    return *this;
}
SensorsList::ConstIterator SensorsList::ConstIterator::operator++(int)
{
    SensorsList::ConstIterator previous(*this);
    this->operator++();
    return previous;
}

bool SensorsList::ConstIterator::operator==(const SensorsList::ConstIterator& s) const
{
    return internalIterator == s.internalIterator;
}

bool SensorsList::ConstIterator::operator==(const SensorsList::Iterator& s) const
{
    return internalIterator == s.internalIterator;
}

SensorsList::ConstIterator::reference SensorsList::ConstIterator::operator*() const
{
    return *internalIterator;
}
SensorsList::ConstIterator::pointer SensorsList::ConstIterator::operator->() const
{
    return internalIterator.operator->();
}

bool SensorsList::ConstIterator::isValid() const
{
    return
        // Check for external list consistency
        externalIterator >= iteratingList.begin() && externalIterator < iteratingList.end() &&
        // Check for internal list consistency
        internalIterator >= externalIterator->begin() && internalIterator < externalIterator->end();
}

///////////////////////////////////////////////////////////////////////////////
///// LinkSensor
///////////////////////////////////////////////////////////////////////////////

bool LinkSensor::isConsistent(const Model& model) const
{
    LinkIndex lnkIdxInModel = model.getLinkIndex(this->getParentLink());

    if (lnkIdxInModel == LINK_INVALID_INDEX)
    {
        std::cerr << "[ERROR] Sensor " << this->getName() << " is not consistent because the link "
                  << this->getParentLink() << " does not exist in the specified model" << std::endl;
        return false;
    }

    if (lnkIdxInModel != this->getParentLinkIndex())
    {
        std::cerr << "[ERROR] Sensor " << this->getName()
                  << " is not consistent because it is attached to link " << this->getParentLink()
                  << " that has index " << lnkIdxInModel << " in the model, while the sensor "
                  << " has it saved with link index " << this->getParentLinkIndex() << std::endl;
        return false;
    }

    return true;
}

///////////////////////////////////////////////////////////////////////////////
///// JointSensor
///////////////////////////////////////////////////////////////////////////////

bool JointSensor::isConsistent(const Model& model) const
{
    LinkIndex jntIdxInModel = model.getJointIndex(this->getParentJoint());

    if (jntIdxInModel == JOINT_INVALID_INDEX)
    {
        std::cerr << "[ERROR] Sensor " << this->getName() << " is not consistent because the joint "
                  << this->getParentJoint() << " does not exist in the specified model"
                  << std::endl;
        return false;
    }

    if (jntIdxInModel != this->getParentJointIndex())
    {
        std::cerr << "[ERROR] Sensor " << this->getName()
                  << " is not consistent because it is attached to joint " << this->getParentJoint()
                  << " that has index " << jntIdxInModel << " in the model, while the sensor "
                  << " has it saved with link index " << this->getParentJointIndex() << std::endl;
        return false;
    }

    return true;
}

///////////////////////////////////////////////////////////////////////////////
///// SensorMeasurements
///////////////////////////////////////////////////////////////////////////////

struct SensorsMeasurements::SensorsMeasurementsPrivateAttributes
{
    std::vector<iDynTree::Wrench> SixAxisFTSensorsMeasurements;
    std::vector<iDynTree::AngVelocity> GyroscopeMeasurements;
    std::vector<iDynTree::LinAcceleration> AccelerometerMeasurements;
    std::vector<Vector3> ThreeAxisAngularAccelerometerMeasurements;
    std::vector<Vector3> ThreeAxisForceTorqueContactMeasurements;
};

SensorsMeasurements::SensorsMeasurements()
    : pimpl(new SensorsMeasurementsPrivateAttributes)
{
}

SensorsMeasurements::SensorsMeasurements(const SensorsList& sensorsList)
{
    this->pimpl = new SensorsMeasurementsPrivateAttributes;
    this->pimpl->SixAxisFTSensorsMeasurements.resize(
        sensorsList.getNrOfSensors(SIX_AXIS_FORCE_TORQUE));
    this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(ACCELEROMETER));
    this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(GYROSCOPE));
    this->pimpl->ThreeAxisAngularAccelerometerMeasurements.resize(
        sensorsList.getNrOfSensors(THREE_AXIS_ANGULAR_ACCELEROMETER));
}
SensorsMeasurements::SensorsMeasurements(const SensorsMeasurements& other)
    : pimpl(new SensorsMeasurementsPrivateAttributes(*(other.pimpl)))
{
}

SensorsMeasurements& SensorsMeasurements::operator=(const SensorsMeasurements& other)
{
    if (this != &other)
    {
        *pimpl = *(other.pimpl);
    }
    return *this;
}

SensorsMeasurements::~SensorsMeasurements()
{
    delete this->pimpl;
}

bool SensorsMeasurements::setNrOfSensors(const SensorType& sensor_type, std::size_t nrOfSensors)
{
    Wrench zeroWrench;
    LinAcceleration zeroLinAcc;
    AngVelocity zeroAngVel;
    bool ret = true;
    switch (sensor_type)
    {
    case SIX_AXIS_FORCE_TORQUE:
        zeroWrench.zero();
        this->pimpl->SixAxisFTSensorsMeasurements.resize(nrOfSensors, zeroWrench);
        break;
    case ACCELEROMETER:
        zeroLinAcc.zero();
        this->pimpl->AccelerometerMeasurements.resize(nrOfSensors, zeroLinAcc);
        break;
    case GYROSCOPE:
        zeroAngVel.zero();
        this->pimpl->GyroscopeMeasurements.resize(nrOfSensors, zeroAngVel);
        break;
    case THREE_AXIS_ANGULAR_ACCELEROMETER: {
        Vector3 zeroAngAcc;
        zeroAngAcc.zero();
        this->pimpl->ThreeAxisAngularAccelerometerMeasurements.resize(nrOfSensors, zeroAngAcc);
    }
    break;
    case THREE_AXIS_FORCE_TORQUE_CONTACT: {
        Vector3 zeroFT;
        zeroFT.zero();
        this->pimpl->ThreeAxisForceTorqueContactMeasurements.resize(nrOfSensors, zeroFT);
    }
    break;
    default:
        ret = false;
        break;
    }

    return ret;
}

bool SensorsMeasurements::resize(const SensorsList& sensorsList)
{
    Wrench zeroWrench;
    zeroWrench.zero();
    LinAcceleration zeroLinAcc;
    zeroLinAcc.zero();
    AngVelocity zeroAngVel;
    zeroAngVel.zero();
    Vector3 zeroVec3;
    zeroVec3.zero();

    this->pimpl->SixAxisFTSensorsMeasurements.resize(sensorsList.getNrOfSensors(
                                                         iDynTree::SIX_AXIS_FORCE_TORQUE),
                                                     zeroWrench);
    this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(
                                                      iDynTree::ACCELEROMETER),
                                                  zeroLinAcc);
    this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::GYROSCOPE),
                                              zeroAngVel);
    this->pimpl->ThreeAxisAngularAccelerometerMeasurements
        .resize(sensorsList.getNrOfSensors(iDynTree::THREE_AXIS_ANGULAR_ACCELEROMETER), zeroAngVel);
    this->pimpl->ThreeAxisForceTorqueContactMeasurements
        .resize(sensorsList.getNrOfSensors(THREE_AXIS_FORCE_TORQUE_CONTACT), zeroVec3);

    return true;
}

bool SensorsMeasurements::toVector(VectorDynSize& measurementVector) const
{
    std::size_t itr;
    LinAcceleration thisLinAcc;
    AngVelocity thisAngVel;
    Wrench thisWrench;
    std::size_t numFT = this->pimpl->SixAxisFTSensorsMeasurements.size();
    std::size_t numAcc = this->pimpl->AccelerometerMeasurements.size();
    std::size_t numGyro = this->pimpl->GyroscopeMeasurements.size();
    std::size_t numThreeAxisAngularAcc
        = this->pimpl->ThreeAxisAngularAccelerometerMeasurements.size();
    std::size_t numThreeAxisFT = this->pimpl->ThreeAxisForceTorqueContactMeasurements.size();

    bool ok = true;

    measurementVector.resize(6 * numFT + 3 * numAcc + 3 * numGyro + 3 * numThreeAxisAngularAcc
                             + 3 * numThreeAxisFT);

    for (itr = 0; itr < numFT; itr++)
    {
        thisWrench = this->pimpl->SixAxisFTSensorsMeasurements.at(itr);

        ok&& measurementVector.setVal(6 * itr, thisWrench.getVal(0));
        ok&& measurementVector.setVal(6 * itr + 1, thisWrench.getVal(1));
        ok&& measurementVector.setVal(6 * itr + 2, thisWrench.getVal(2));
        ok&& measurementVector.setVal(6 * itr + 3, thisWrench.getVal(3));
        ok&& measurementVector.setVal(6 * itr + 4, thisWrench.getVal(4));
        ok&& measurementVector.setVal(6 * itr + 5, thisWrench.getVal(5));
    }
    for (itr = 0; itr < numAcc; itr++)
    {
        thisLinAcc = this->pimpl->AccelerometerMeasurements.at(itr);
        ok&& measurementVector.setVal(6 * numFT + 3 * itr, thisLinAcc.getVal(0));
        ok&& measurementVector.setVal(6 * numFT + 3 * itr + 1, thisLinAcc.getVal(1));
        ok&& measurementVector.setVal(6 * numFT + 3 * itr + 2, thisLinAcc.getVal(2));
    }
    for (itr = 0; itr < numGyro; itr++)
    {
        thisAngVel = this->pimpl->GyroscopeMeasurements.at(itr);
        ok&& measurementVector.setVal(6 * numFT + 3 * numAcc + 3 * itr, thisAngVel.getVal(0));
        ok&& measurementVector.setVal(6 * numFT + 3 * numAcc + 3 * itr + 1, thisAngVel.getVal(1));
        ok&& measurementVector.setVal(6 * numFT + 3 * numAcc + 3 * itr + 2, thisAngVel.getVal(2));
    }
    for (itr = 0; itr < numThreeAxisAngularAcc; itr++)
    {
        std::ptrdiff_t offset = 6 * numFT + 3 * numAcc + 3 * numGyro + 3 * itr;
        Vector3 thisAngAcc = this->pimpl->ThreeAxisAngularAccelerometerMeasurements.at(itr);
        ok&& measurementVector.setVal(offset, thisAngAcc.getVal(0));
        ok&& measurementVector.setVal(offset + 1, thisAngAcc.getVal(1));
        ok&& measurementVector.setVal(offset + 2, thisAngAcc.getVal(2));
    }
    for (itr = 0; itr < numThreeAxisFT; itr++)
    {
        std::ptrdiff_t offset
            = 6 * numFT + 3 * numAcc + 3 * numGyro + 3 * numThreeAxisAngularAcc + 3 * itr;
        Vector3 thisThreeAxisFT = this->pimpl->ThreeAxisForceTorqueContactMeasurements.at(itr);
        ok&& measurementVector.setVal(offset, thisThreeAxisFT.getVal(0));
        ok&& measurementVector.setVal(offset + 1, thisThreeAxisFT.getVal(1));
        ok&& measurementVector.setVal(offset + 2, thisThreeAxisFT.getVal(2));
    }

    return ok;
}

bool SensorsMeasurements::setMeasurement(const SensorType& sensor_type,
                                         const std::ptrdiff_t& sensor_index,
                                         const iDynTree::Wrench& measurement)
{
    if (sensor_type == SIX_AXIS_FORCE_TORQUE)
    {
        if (static_cast<std::size_t>(sensor_index)
            < this->pimpl->SixAxisFTSensorsMeasurements.size())
        {
            this->pimpl->SixAxisFTSensorsMeasurements[sensor_index] = measurement;
            return true;
        } else
        {
            std::cerr << "[ERROR] setMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->SixAxisFTSensorsMeasurements.size() << std::endl;
            return false;
        }
    }

    return false;
}

bool SensorsMeasurements::setMeasurement(const SensorType& sensor_type,
                                         const std::ptrdiff_t& sensor_index,
                                         const Vector3& measurement)
{
    if (sensor_type == ACCELEROMETER)
    {
        if (static_cast<std::size_t>(sensor_index) < this->pimpl->AccelerometerMeasurements.size())
        {
            this->pimpl->AccelerometerMeasurements[sensor_index] = measurement;
            return true;
        } else
        {
            std::cerr << "[ERROR] setMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->AccelerometerMeasurements.size() << std::endl;
            return false;
        }
    }

    if (sensor_type == GYROSCOPE)
    {
        if (static_cast<std::size_t>(sensor_index) < this->pimpl->GyroscopeMeasurements.size())
        {
            this->pimpl->GyroscopeMeasurements[sensor_index] = measurement;
            return true;
        } else
        {
            std::cerr << "[ERROR] setMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->GyroscopeMeasurements.size() << std::endl;
            return false;
        }
    }

    if (sensor_type == THREE_AXIS_ANGULAR_ACCELEROMETER)
    {
        if (static_cast<std::size_t>(sensor_index)
            < this->pimpl->ThreeAxisAngularAccelerometerMeasurements.size())
        {
            this->pimpl->ThreeAxisAngularAccelerometerMeasurements[sensor_index] = measurement;
            return true;
        } else
        {
            std::cerr << "[ERROR] setMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->ThreeAxisAngularAccelerometerMeasurements.size() << std::endl;
            return false;
        }
    }

    if (sensor_type == THREE_AXIS_FORCE_TORQUE_CONTACT)
    {
        if (static_cast<std::size_t>(sensor_index)
            < this->pimpl->ThreeAxisForceTorqueContactMeasurements.size())
        {
            this->pimpl->ThreeAxisForceTorqueContactMeasurements[sensor_index] = measurement;
            return true;
        } else
        {
            std::cerr << "[ERROR] setMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->ThreeAxisForceTorqueContactMeasurements.size() << std::endl;
            return false;
        }
    }

    return false;
}

bool SensorsMeasurements::getMeasurement(const SensorType& sensor_type,
                                         const std::ptrdiff_t& sensor_index,
                                         Wrench& measurement) const
{
    if (sensor_type == SIX_AXIS_FORCE_TORQUE)
    {
        if (static_cast<std::size_t>(sensor_index)
            < this->pimpl->SixAxisFTSensorsMeasurements.size())
        {
            measurement = this->pimpl->SixAxisFTSensorsMeasurements[sensor_index];
            return true;
        } else
        {
            std::cerr << "[ERROR] getMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->SixAxisFTSensorsMeasurements.size() << std::endl;
            return false;
        }
    }

    return false;
}

bool SensorsMeasurements::getMeasurement(const SensorType& sensor_type,
                                         const std::ptrdiff_t& sensor_index,
                                         Vector3& measurement) const
{
    if (sensor_type == ACCELEROMETER)
    {
        if (static_cast<std::size_t>(sensor_index) < this->pimpl->AccelerometerMeasurements.size())
        {
            measurement = this->pimpl->AccelerometerMeasurements[sensor_index];
            return true;
        } else
        {
            std::cerr << "[ERROR] getMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->AccelerometerMeasurements.size() << std::endl;
            return false;
        }
    }

    if (sensor_type == GYROSCOPE)
    {
        if (static_cast<std::size_t>(sensor_index) < this->pimpl->GyroscopeMeasurements.size())
        {
            measurement = this->pimpl->GyroscopeMeasurements[sensor_index];
            return true;
        } else
        {
            std::cerr << "[ERROR] getMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->GyroscopeMeasurements.size() << std::endl;
            return false;
        }
    }

    if (sensor_type == THREE_AXIS_ANGULAR_ACCELEROMETER)
    {
        if (static_cast<std::size_t>(sensor_index)
            < this->pimpl->ThreeAxisAngularAccelerometerMeasurements.size())
        {
            measurement = this->pimpl->ThreeAxisAngularAccelerometerMeasurements[sensor_index];
            return true;
        } else
        {
            std::cerr << "[ERROR] getMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->ThreeAxisAngularAccelerometerMeasurements.size() << std::endl;
            return false;
        }
    }

    if (sensor_type == THREE_AXIS_FORCE_TORQUE_CONTACT)
    {
        if (static_cast<std::size_t>(sensor_index)
            < this->pimpl->ThreeAxisForceTorqueContactMeasurements.size())
        {
            measurement = this->pimpl->ThreeAxisForceTorqueContactMeasurements[sensor_index];
            return true;
        } else
        {
            std::cerr << "[ERROR] getMeasurement failed: sensor_index " << sensor_index
                      << "is out of bounds, because nrOfSensors is "
                      << this->pimpl->ThreeAxisForceTorqueContactMeasurements.size() << std::endl;
            return false;
        }
    }

    return false;
}

std::size_t SensorsMeasurements::getNrOfSensors(const SensorType& sensor_type) const
{
    std::size_t returnVal = 0;
    switch (sensor_type)
    {
    case SIX_AXIS_FORCE_TORQUE:
        returnVal = this->pimpl->SixAxisFTSensorsMeasurements.size();
        break;
    case ACCELEROMETER:
        returnVal = this->pimpl->AccelerometerMeasurements.size();
        break;
    case GYROSCOPE:
        returnVal = this->pimpl->GyroscopeMeasurements.size();
        break;
    case THREE_AXIS_ANGULAR_ACCELEROMETER:
        returnVal = this->pimpl->ThreeAxisAngularAccelerometerMeasurements.size();
        break;
    case THREE_AXIS_FORCE_TORQUE_CONTACT:
        returnVal = this->pimpl->ThreeAxisForceTorqueContactMeasurements.size();
        break;
    default:
        returnVal = 0;
    }
    return (returnVal);
}

size_t SensorsMeasurements::getSizeOfAllSensorsMeasurements() const
{
    size_t res = 0;
    for (int i = (int)SIX_AXIS_FORCE_TORQUE; i < NR_OF_SENSOR_TYPES; i++)
    {
        SensorType type = (SensorType)i;
        res += getSensorTypeSize(type) * getNrOfSensors(type);
    }
    return res;
}

} // namespace iDynTree
