/*
 * 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.
 *
*/

#include <vector>

#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Road2d.hh"
#include "gazebo/rendering/RenderEngine.hh"
#include "gazebo/rendering/VisualPrivate.hh"

namespace gazebo
{
  namespace rendering
  {
    /// \brief A road segment
    class RoadSegment
    {
      /// \brief Load the road segment from message data.
      /// \param[in] _msg The robot data.
      public: void Load(msgs::Road _msg);

      /// \brief Name of the road segment.
      public: std::string name;

      /// \brief Points that make up the middle of the road.
      public: std::vector<ignition::math::Vector3d> points;

      /// \brief Width of the road.
      public: double width;

      /// \brief Texture of the road
      public: std::string texture;
    };

    /// \brief Private data for the Road2d class.
    class Road2dPrivate : public VisualPrivate
    {
      /// \brief All the road segments.
      public: std::vector<RoadSegment> segments;
    };
  }
}

using namespace gazebo;
using namespace rendering;

/////////////////////////////////////////////////
Road2d::Road2d()
  : Visual(*new Road2dPrivate, "roads", rendering::get_scene())
{
}

/////////////////////////////////////////////////
Road2d::Road2d(const std::string &_name, VisualPtr _parent)
  : Visual(*new Road2dPrivate, _name, _parent)
{
  Road2dPrivate *dPtr =
      reinterpret_cast<Road2dPrivate *>(this->dataPtr);

  dPtr->type = VT_VISUAL;
}

/////////////////////////////////////////////////
Road2d::~Road2d()
{
  Road2dPrivate *dPtr =
      reinterpret_cast<Road2dPrivate *>(this->dataPtr);

  dPtr->segments.clear();
}

//////////////////////////////////////////////////
void Road2d::Load(msgs::Road _msg)
{
  this->Load();

  Road2dPrivate *dPtr =
      reinterpret_cast<Road2dPrivate *>(this->dataPtr);

  RoadSegment segment;
  segment.Load(_msg);

  Ogre::MovableObject *obj = dynamic_cast<Ogre::MovableObject *>
      (this->GetSceneNode()->getCreator()->createEntity(
      segment.name, segment.name));
  obj->setRenderQueueGroup(obj->getRenderQueueGroup()+1);
  this->AttachObject(obj);
  dPtr->segments.push_back(segment);

  // make the road visual not selectable
  this->SetVisibilityFlags(GZ_VISIBILITY_ALL & (~GZ_VISIBILITY_SELECTABLE));
}

/////////////////////////////////////////////////
void RoadSegment::Load(msgs::Road _msg)
{
  this->width = _msg.width();

  for (int i = 0; i < _msg.point_size(); ++i)
  {
    this->points.push_back(msgs::ConvertIgn(_msg.point(i)));
  }

  this->name = _msg.name();

  /// Create the mesh
  Ogre::MeshPtr mesh =
      Ogre::MeshManager::getSingleton().createManual(this->name,
      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);


  Ogre::SubMesh *subMesh = mesh->createSubMesh();
  subMesh->useSharedVertices = false;
  subMesh->operationType = Ogre::RenderOperation::OT_TRIANGLE_STRIP;
  subMesh->vertexData = new Ogre::VertexData();
  Ogre::VertexData *vertexData = subMesh->vertexData;
  Ogre::VertexDeclaration *vertexDecl = vertexData->vertexDeclaration;

  size_t currOffset = 0;

  // Positions
  vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
  currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);

  // Normals
  vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
  currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);

  // two dimensional ->roadObjtexture coordinates
  vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT2,
                         Ogre::VES_TEXTURE_COORDINATES, 0);

  // allocate the vertex buffer
  vertexData->vertexCount = this->points.size() * 2;
  Ogre::HardwareVertexBufferSharedPtr vBuf =
    Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
        vertexDecl->getVertexSize(0), vertexData->vertexCount,
        Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY, false);

  Ogre::VertexBufferBinding *binding = vertexData->vertexBufferBinding;
  binding->setBinding(0, vBuf);
  float *vertices = static_cast<float*>(
      vBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));

  // index buffer
  subMesh->indexData->indexCount = vertexData->vertexCount;
  subMesh->indexData->indexBuffer =
      Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(
      Ogre::HardwareIndexBuffer::IT_32BIT,
      subMesh->indexData->indexCount,
      Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
      false);
  Ogre::HardwareIndexBufferSharedPtr iBuf = subMesh->indexData->indexBuffer;
  uint32_t *indices = static_cast<uint32_t *>(
      iBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));

  ignition::math::Vector3d pA, pB, tangent;

  ignition::math::AxisAlignedBox bounds;
  bounds.Min().Set(ignition::math::MAX_D, ignition::math::MAX_D,
                   ignition::math::MAX_D);
  bounds.Max().Set(ignition::math::MIN_D, ignition::math::MIN_D,
                   ignition::math::MIN_D);

  // length for each texture tile, same as road width as texture is square
  // (if texture size should change or made custom in a future version
  // there needs to be code to handle this)
  double texMaxLen = this->width;

  // current road length
  double curLen = 0.0;

  // Generate the triangles for the road
  for (unsigned int i = 0; i < this->points.size(); ++i)
  {
    double factor = 1.0;

    // update current road length
    if (i > 0)
    {
      curLen += this->points[i].Distance(this->points[i-1]);
    }

    // assign texture coordinate as percentage of texture tile size
    // and let ogre/opengl handle the texture wrapping
    float texCoord = curLen/texMaxLen;

    // Start point is a special case
    if (i == 0)
    {
      tangent = (this->points[i+1] - this->points[i]).Normalize();
    }
    // End point is a special case
    else if (i == this->points.size() - 1)
    {
      tangent = (this->points[i] - this->points[i-1]).Normalize();
    }
    // Every other point in the road
    else
    {
      auto v1 = (this->points[i+1] - this->points[i]).Normalize();
      auto v0 = (this->points[i] - this->points[i-1]).Normalize();
      double dot = v0.Dot(v1 * -1);
      tangent = (v1+v0).Normalize();

      // Check to see if the points are not colinear
      // If not colinear, then the road needs to be widended for the turns
      if (!ignition::math::equal(fabs(dot), 1.0))
      {
        factor = 1.0 / sin(acos(dot) * 0.5);
      }
    }

    // The tangent is used to calculate the two verteces to either side of
    // the point. The vertices define the triangle mesh of the road
    double theta = atan2(tangent.X(), -tangent.Y());

    pA = pB = this->points[i];
    double w = (this->width * factor) * 0.5;

    pA.X() += cos(theta) * w;
    pA.Y() += sin(theta) * w;

    pB.X() -= cos(theta) * w;
    pB.Y() -= sin(theta) * w;

    bounds.Min().Min(pA);
    bounds.Min().Min(pB);

    bounds.Max().Max(pA);
    bounds.Max().Max(pB);

    // Position
    *vertices++ = pA.X();
    *vertices++ = pA.Y();
    *vertices++ = pA.Z();

    // Normal
    *vertices++ = 0;
    *vertices++ = 0;
    *vertices++ = 1;

    // UV Coord
    *vertices++ = 0;
    *vertices++ = texCoord;

    // Position
    *vertices++ = pB.X();
    *vertices++ = pB.Y();
    *vertices++ = pB.Z();

    // Normal
    *vertices++ = 0;
    *vertices++ = 0;
    *vertices++ = 1;

    // UV Coord
    *vertices++ = 1;
    *vertices++ = texCoord;
  }

  // Add all the indices
  for (unsigned int i = 0; i < subMesh->indexData->indexCount; ++i)
    *indices++ = i;

  // unlock
  vBuf->unlock();
  iBuf->unlock();

  mesh->_setBounds(Ogre::AxisAlignedBox(Conversions::Convert(bounds.Min()),
                   Conversions::Convert(bounds.Max())));

  if (_msg.has_material())
  {
    if (_msg.material().has_script())
    {
      for (int i = 0; i < _msg.material().script().uri_size(); ++i)
      {
        std::string matUri = _msg.material().script().uri(i);
        if (!matUri.empty())
          RenderEngine::Instance()->AddResourcePath(matUri);
      }

      std::string matName = _msg.material().script().name();

      if (!matName.empty())
        subMesh->setMaterialName(matName);
     }
  }
  else
  {
    subMesh->setMaterialName("Gazebo/Road");
  }

  mesh->load();
}
