/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * 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.
 *
*/
/* Desc: A link state
 * Author: Nate Koenig
 */

#ifndef _LINKSTATE_HH_
#define _LINKSTATE_HH_

#include <iostream>
#include <vector>
#include <string>

#include <sdf/sdf.hh>

#include "gazebo/physics/State.hh"
#include "gazebo/physics/CollisionState.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace physics
  {
    /// \addtogroup gazebo_physics
    /// \{

    /// \class LinkState LinkState.hh physics/physics.hh
    /// \brief Store state information of a physics::Link object
    ///
    /// This class captures the entire state of a Link at one
    /// specific time during a simulation run.
    ///
    /// State of a Link includes the state of itself all its child Collision
    /// entities.
    class GZ_PHYSICS_VISIBLE LinkState : public State
    {
      /// \brief Default constructor
      public: LinkState();

      /// \brief Constructor
      ///
      /// Build a LinkState from an existing Link.
      /// \param[in] _model Pointer to the Link from which to gather state
      /// info.
      /// \param[in] _realTime Real time stamp.
      /// \param[in] _simTime Sim time stamp
      /// \param[in] _iterations Simulation iterations.
      public: LinkState(const LinkPtr _link, const common::Time &_realTime,
                  const common::Time &_simTime, const uint64_t _iterations);

      /// \brief Constructor
      ///
      /// Build a LinkState from an existing Link.
      /// \param[in] _model Pointer to the Link from which to gather state
      /// info.
      public: explicit LinkState(const LinkPtr _link);

      /// \brief Constructor
      ///
      /// Build a LinkState from SDF data
      /// \param[in] _sdf SDF data to load a link state from.
      public: explicit LinkState(const sdf::ElementPtr _sdf);

      /// \brief Destructor.
      public: virtual ~LinkState();

      /// \brief Load a LinkState from a Link pointer.
      ///
      /// Build a LinkState from an existing Link.
      /// \param[in] _model Pointer to the Link from which to gather state
      /// info.
      /// \param[in] _realTime Real time stamp.
      /// \param[in] _simTime Sim time stamp.
      /// \param[in] _iterations Simulation iterations.
      public: void Load(const LinkPtr _link, const common::Time &_realTime,
                  const common::Time &_simTime, const uint64_t _iterations);

      /// \brief Load state from SDF element.
      ///
      /// Load LinkState information from stored data in and SDF::Element.
      /// \param[in] _elem Pointer to the SDF::Element containing state info.
      public: virtual void Load(const sdf::ElementPtr _elem);

      /// \brief Get the link pose.
      /// \return The math::Pose of the Link.
      public: const math::Pose &GetPose() const;

      /// \brief Get the link velocity.
      /// \return The velocity represented as a math::Pose.
      public: const math::Pose &GetVelocity() const;

      /// \brief Get the link acceleration.
      /// \return The acceleration represented as a math::Pose.
      public: const math::Pose &GetAcceleration() const;

      /// \brief Get the force applied to the Link.
      /// \return Magnitude of the force.
      public: const math::Pose &GetWrench() const;

      /// \brief Get the number of link states.
      ///
      /// This returns the number of Collisions recorded.
      /// \return Number of CollisionState recorded.
      public: unsigned int GetCollisionStateCount() const;

      /// \brief Get a collision state.
      ///
      /// Get a Collision State based on an index, where index is in the
      /// range of  0...LinkState::GetCollisionStateCount.
      /// \param[in] _index Index of the CollisionState.
      /// \return State of the Collision.
      /// \throws common::Exception When _index is invalid.
      public: CollisionState GetCollisionState(unsigned int _index) const;

      /// \brief Get a link state by link name.
      ///
      /// Searches through all CollisionStates.
      /// Returns the CollisionState with the matching name, if any.
      /// \param[in] _collisionName Name of the CollisionState
      /// \return State of the Collision.
      /// \throws common::Exception When _collisionName is invalid
      public: CollisionState GetCollisionState(
                  const std::string &_collisionName) const;

      /// \brief Get the collision states.
      /// \return A vector of collision states.
      public: const std::vector<CollisionState> &GetCollisionStates() const;

      /// \brief Return true if the values in the state are zero.
      /// \return True if the values in the state are zero.
      public: bool IsZero() const;

      /// \brief Populate a state SDF element with data from the object.
      /// \param[out] _sdf SDF element to populate.
      public: void FillSDF(sdf::ElementPtr _sdf);

      /// \brief Set the wall time when this state was generated
      /// \param[in] _time The absolute clock time when the State
      /// data was recorded.
      public: virtual void SetWallTime(const common::Time &_time);

      /// \brief Set the real time when this state was generated
      /// \param[in] _time Clock time since simulation was stated.
      public: virtual void SetRealTime(const common::Time &_time);

      /// \brief Set the sim time when this state was generated
      /// \param[in] _time Simulation time when the data was recorded.
      public: virtual void SetSimTime(const common::Time &_time);

      /// \brief Set the simulation iterations when this state was generated
      /// \param[in] _iterations Simulation iterations when the data was
      /// recorded.
      public: virtual void SetIterations(const uint64_t _iterations);

      /// \brief Assignment operator
      /// \param[in] _state State value
      /// \return this
      public: LinkState &operator=(const LinkState &_state);

      /// \brief Subtraction operator.
      /// \param[in] _pt A state to substract.
      /// \return The resulting state.
      public: LinkState operator-(const LinkState &_state) const;

      /// \brief Addition operator.
      /// \param[in] _pt A state to add.
      /// \return The resulting state.
      public: LinkState operator+(const LinkState &_state) const;

      /// \brief Stream insertion operator
      /// \param[in] _out output stream
      /// \param[in] _state Link state to output
      /// \return the stream
      public: inline friend std::ostream &operator<<(std::ostream &_out,
                  const gazebo::physics::LinkState &_state)
      {
        math::Vector3 q(_state.pose.rot.GetAsEuler());
        _out.unsetf(std::ios_base::floatfield);
        _out << std::setprecision(4)
          << "<link name='" << _state.name << "'>"
          << "<pose>"
          << ignition::math::precision(_state.pose.pos.x, 4) << " "
          << ignition::math::precision(_state.pose.pos.y, 4) << " "
          << ignition::math::precision(_state.pose.pos.z, 4) << " "
          << ignition::math::precision(q.x, 4) << " "
          << ignition::math::precision(q.y, 4) << " "
          << ignition::math::precision(q.z, 4) << " "
          << "</pose>";

        if (_state.RecordVelocity())
        {
          /// Disabling this for efficiency.
          q = _state.velocity.rot.GetAsEuler();
          _out.unsetf(std::ios_base::floatfield);
          _out << std::setprecision(4)
            << "<velocity>"
            << ignition::math::precision(_state.velocity.pos.x, 4) << " "
            << ignition::math::precision(_state.velocity.pos.y, 4) << " "
            << ignition::math::precision(_state.velocity.pos.z, 4) << " "
            << ignition::math::precision(q.x, 4) << " "
            << ignition::math::precision(q.y, 4) << " "
            << ignition::math::precision(q.z, 4) << " "
            << "</velocity>";
        }
        // << "<acceleration>" << _state.acceleration << "</acceleration>"
        // << "<wrench>" << _state.wrench << "</wrench>";

        /// Disabling this for efficiency.
        // for (std::vector<CollisionState>::const_iterator iter =
        //      _state.collisionStates.begin();
        //      iter != _state.collisionStates.end(); ++iter)
        // {
        //   _out << *iter;
        // }

        _out << "</link>";

        return _out;
      }

      /// \brief Set whether to record link velocity
      /// \param[in] _record True to record link velocity, false to leave it
      /// out of the log
      public: void SetRecordVelocity(const bool _record);

      /// \brief Get whether link velocity is recorded
      /// \return True if link velocity is recorded
      public: bool RecordVelocity() const;

      /// \brief 3D pose of the link relative to the model.
      private: math::Pose pose;

      /// \brief Velocity of the link (linear and angular).
      private: math::Pose velocity;

      /// \brief Acceleration of the link (linear and angular).
      private: math::Pose acceleration;

      /// \brief Force on the link(linear and angular).
      private: math::Pose wrench;

      /// \brief State of all the child Collision objects.
      private: std::vector<CollisionState> collisionStates;
    };
    /// \}
  }
}
#endif