/*=========================================================================

   Library: iMSTK

   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
   & Imaging in Medicine, Rensselaer Polytechnic Institute.

   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.txt

   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 "imstkDeviceControl.h"
#include "imstkMath.h"

namespace imstk
{
///
/// \class TrackingDeviceControl
///
/// \brief Base class for all DeviceControls that track
/// something in space (position and orientation)
///
class TrackingDeviceControl : public DeviceControl
{
public:
    enum InvertFlag
    {
        transX = 0x01,
        transY = 0x02,
        transZ = 0x04,
        rotX = 0x08,
        rotY = 0x10,
        rotZ = 0x20
    };

protected:
    TrackingDeviceControl();
    TrackingDeviceControl(std::shared_ptr<DeviceClient> device);

public:
    virtual ~TrackingDeviceControl() override = default;

public:
    ///
    /// \brief Apply forces to the haptic device
    ///
    virtual void applyForces() {}

    ///
    /// \brief Sets the tracking data to be out of date or up to date
    ///
    void setTrackerToOutOfDate() { m_trackingDataUptoDate = false; }
    void setTrackerToUpToDate() { m_trackingDataUptoDate = true; }

    ///
    /// \brief Returns true if the tracking data is already updated in current frame. Else, false.
    ///
    bool isTrackerUpToDate() const { return m_trackingDataUptoDate; }

    ///
    /// \brief Get the latest position
    ///
    const Vec3d& getPosition() const { return m_currentPos; };

    ///
    /// \brief Set the position of the tracker
    ///
    void setPosition(const Vec3d& pos) { this->m_currentPos = pos; }

    ///
    /// \brief Get the latest rotation
    ///
    const Quatd& getRotation() const { return m_currentOrientation; };

    ///
    /// \brief Set the orientation of the tracker
    ///
    void setRotation(const Quatd& orientation) { this->m_currentOrientation = orientation; }

    ///
    /// \brief Set/Get whether to compute the velocity from previous and current samples
    /// Useful if a device does not provide the quantity
    ///
    void setComputeVelocity(const bool computeVelocity) { m_computeVelocity = computeVelocity; }
    bool getComputeVelocity() const { return m_computeVelocity; }

    ///
    /// \brief Set/Get whether to compute the anular velocity from previous and current samples
    /// Useful if the device does not provide the quantity
    ///
    void setComputeAngularVelocity(const bool computeAngularVelocity) { m_computeAngularVelocity = computeAngularVelocity; }
    bool getComputeAngularVelocity() const { return m_computeAngularVelocity; }

    ///
    /// \brief Get/Set the angular velocity
    ///
    const Vec3d& getAngularVelocity() const { return m_currentAngularVelocity; }
    void setAngularVelocity(const Vec3d& angularVelocity) { m_currentAngularVelocity = angularVelocity; }

    ///
    /// \brief Get/Set the linear velocity
    ///
    const Vec3d& getVelocity() const { return m_currentVelocity; }
    void setVelocity(const Vec3d& velocity) { m_currentVelocity = velocity; }

    ///
    /// \brief Get/Set the current scaling factor
    ///
    double getTranslationScaling() const;
    void setTranslationScaling(const double scaling);

    ///
    /// \brief Get/Set the translation offset
    ///
    const Vec3d& getTranslationOffset() const;
    void setTranslationOffset(const Vec3d& t);

    ///
    /// \brief Get/Set the rotation offset
    ///
    const Quatd& getRotationOffset();
    void setRotationOffset(const Quatd& r);

    ///
    /// \brief Get/Set the inversion flags
    ///
    unsigned char getInversionFlags();
    void setInversionFlags(const unsigned char f);

    ///
    /// \brief Update tracking data
    ///
    virtual bool updateTrackingData(const double dt);

protected:
    double m_scaling = 1.0;                                ///< Scaling factor for physical to virtual translations
    Vec3d  m_translationOffset;                            ///< Translation concatenated to the device translation
    Quatd  m_rotationOffset;                               ///< Rotation concatenated to the device rotation
    unsigned char m_invertFlags = 0x00;                    ///< Invert flags to be masked with DeviceTracker::InvertFlag

    Vec3d m_currentPos;
    Quatd m_currentOrientation;
    Vec3d m_currentVelocity;
    Vec3d m_currentAngularVelocity;

    Vec3d m_currentDisplacement;
    Quatd m_currentRotation;

    bool m_trackingDataUptoDate = false;
    // If on, will use current and previous positions to produce velocity, if off, will ask device for velocity
    bool m_computeVelocity = false;
    // If on, will use current and previous rotations to produce angular velocity, if off, will ask device for angular velocity
    bool m_computeAngularVelocity = false;
};
} // imstk
