/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
#define DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_

#include <cmath>
#include <assimp/scene.h>

#include "dart/collision/fcl/BackwardCompatibility.hpp"
#include "dart/math/Constants.hpp"

namespace dart {
namespace collision {

template <class BV>
::fcl::BVHModel<BV>* createMesh(
    float _scaleX,
    float _scaleY,
    float _scaleZ,
    const aiScene* _mesh,
    const dart::collision::fcl::Transform3& _transform)
{
  assert(_mesh);
  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
  model->beginModel();

  for (unsigned int i = 0; i < _mesh->mNumMeshes; i++)
  {
    for (unsigned int j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++)
    {
      dart::collision::fcl::Vector3 vertices[3];
      for (unsigned int k = 0; k < 3; k++)
      {
        const aiVector3D& vertex
            = _mesh->mMeshes[i]
                  ->mVertices[_mesh->mMeshes[i]->mFaces[j].mIndices[k]];
        vertices[k] = dart::collision::fcl::Vector3(
            vertex.x * _scaleX, vertex.y * _scaleY, vertex.z * _scaleZ);
        vertices[k] = dart::collision::fcl::transform(_transform, vertices[k]);
      }
      model->addTriangle(vertices[0], vertices[1], vertices[2]);
    }
  }

  model->endModel();
  return model;
}

template <class BV>
::fcl::BVHModel<BV>* createEllipsoid(
    float _sizeX,
    float _sizeY,
    float _sizeZ,
    const dart::collision::fcl::Transform3& _transform)
{
  float v[59][3] = {{0, 0, 0},
                    {0.135299, -0.461940, -0.135299},
                    {0.000000, -0.461940, -0.191342},
                    {-0.135299, -0.461940, -0.135299},
                    {-0.191342, -0.461940, 0.000000},
                    {-0.135299, -0.461940, 0.135299},
                    {0.000000, -0.461940, 0.191342},
                    {0.135299, -0.461940, 0.135299},
                    {0.191342, -0.461940, 0.000000},
                    {0.250000, -0.353553, -0.250000},
                    {0.000000, -0.353553, -0.353553},
                    {-0.250000, -0.353553, -0.250000},
                    {-0.353553, -0.353553, 0.000000},
                    {-0.250000, -0.353553, 0.250000},
                    {0.000000, -0.353553, 0.353553},
                    {0.250000, -0.353553, 0.250000},
                    {0.353553, -0.353553, 0.000000},
                    {0.326641, -0.191342, -0.326641},
                    {0.000000, -0.191342, -0.461940},
                    {-0.326641, -0.191342, -0.326641},
                    {-0.461940, -0.191342, 0.000000},
                    {-0.326641, -0.191342, 0.326641},
                    {0.000000, -0.191342, 0.461940},
                    {0.326641, -0.191342, 0.326641},
                    {0.461940, -0.191342, 0.000000},
                    {0.353553, 0.000000, -0.353553},
                    {0.000000, 0.000000, -0.500000},
                    {-0.353553, 0.000000, -0.353553},
                    {-0.500000, 0.000000, 0.000000},
                    {-0.353553, 0.000000, 0.353553},
                    {0.000000, 0.000000, 0.500000},
                    {0.353553, 0.000000, 0.353553},
                    {0.500000, 0.000000, 0.000000},
                    {0.326641, 0.191342, -0.326641},
                    {0.000000, 0.191342, -0.461940},
                    {-0.326641, 0.191342, -0.326641},
                    {-0.461940, 0.191342, 0.000000},
                    {-0.326641, 0.191342, 0.326641},
                    {0.000000, 0.191342, 0.461940},
                    {0.326641, 0.191342, 0.326641},
                    {0.461940, 0.191342, 0.000000},
                    {0.250000, 0.353553, -0.250000},
                    {0.000000, 0.353553, -0.353553},
                    {-0.250000, 0.353553, -0.250000},
                    {-0.353553, 0.353553, 0.000000},
                    {-0.250000, 0.353553, 0.250000},
                    {0.000000, 0.353553, 0.353553},
                    {0.250000, 0.353553, 0.250000},
                    {0.353553, 0.353553, 0.000000},
                    {0.135299, 0.461940, -0.135299},
                    {0.000000, 0.461940, -0.191342},
                    {-0.135299, 0.461940, -0.135299},
                    {-0.191342, 0.461940, 0.000000},
                    {-0.135299, 0.461940, 0.135299},
                    {0.000000, 0.461940, 0.191342},
                    {0.135299, 0.461940, 0.135299},
                    {0.191342, 0.461940, 0.000000},
                    {0.000000, -0.500000, 0.000000},
                    {0.000000, 0.500000, 0.000000}};
  int f[112][3]
      = {{1, 2, 9},    {9, 2, 10},   {2, 3, 10},   {10, 3, 11},  {3, 4, 11},
         {11, 4, 12},  {4, 5, 12},   {12, 5, 13},  {5, 6, 13},   {13, 6, 14},
         {6, 7, 14},   {14, 7, 15},  {7, 8, 15},   {15, 8, 16},  {8, 1, 16},
         {16, 1, 9},   {9, 10, 17},  {17, 10, 18}, {10, 11, 18}, {18, 11, 19},
         {11, 12, 19}, {19, 12, 20}, {12, 13, 20}, {20, 13, 21}, {13, 14, 21},
         {21, 14, 22}, {14, 15, 22}, {22, 15, 23}, {15, 16, 23}, {23, 16, 24},
         {16, 9, 24},  {24, 9, 17},  {17, 18, 25}, {25, 18, 26}, {18, 19, 26},
         {26, 19, 27}, {19, 20, 27}, {27, 20, 28}, {20, 21, 28}, {28, 21, 29},
         {21, 22, 29}, {29, 22, 30}, {22, 23, 30}, {30, 23, 31}, {23, 24, 31},
         {31, 24, 32}, {24, 17, 32}, {32, 17, 25}, {25, 26, 33}, {33, 26, 34},
         {26, 27, 34}, {34, 27, 35}, {27, 28, 35}, {35, 28, 36}, {28, 29, 36},
         {36, 29, 37}, {29, 30, 37}, {37, 30, 38}, {30, 31, 38}, {38, 31, 39},
         {31, 32, 39}, {39, 32, 40}, {32, 25, 40}, {40, 25, 33}, {33, 34, 41},
         {41, 34, 42}, {34, 35, 42}, {42, 35, 43}, {35, 36, 43}, {43, 36, 44},
         {36, 37, 44}, {44, 37, 45}, {37, 38, 45}, {45, 38, 46}, {38, 39, 46},
         {46, 39, 47}, {39, 40, 47}, {47, 40, 48}, {40, 33, 48}, {48, 33, 41},
         {41, 42, 49}, {49, 42, 50}, {42, 43, 50}, {50, 43, 51}, {43, 44, 51},
         {51, 44, 52}, {44, 45, 52}, {52, 45, 53}, {45, 46, 53}, {53, 46, 54},
         {46, 47, 54}, {54, 47, 55}, {47, 48, 55}, {55, 48, 56}, {48, 41, 56},
         {56, 41, 49}, {2, 1, 57},   {3, 2, 57},   {4, 3, 57},   {5, 4, 57},
         {6, 5, 57},   {7, 6, 57},   {8, 7, 57},   {1, 8, 57},   {49, 50, 58},
         {50, 51, 58}, {51, 52, 58}, {52, 53, 58}, {53, 54, 58}, {54, 55, 58},
         {55, 56, 58}, {56, 49, 58}};

  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
  dart::collision::fcl::Vector3 p1, p2, p3;
  model->beginModel();

  for (int i = 0; i < 112; i++)
  {
    p1 = dart::collision::fcl::Vector3(
        v[f[i][0]][0] * _sizeX, v[f[i][0]][1] * _sizeY, v[f[i][0]][2] * _sizeZ);
    p2 = dart::collision::fcl::Vector3(
        v[f[i][1]][0] * _sizeX, v[f[i][1]][1] * _sizeY, v[f[i][1]][2] * _sizeZ);
    p3 = dart::collision::fcl::Vector3(
        v[f[i][2]][0] * _sizeX, v[f[i][2]][1] * _sizeY, v[f[i][2]][2] * _sizeZ);
    p1 = dart::collision::fcl::transform(_transform, p1);
    p2 = dart::collision::fcl::transform(_transform, p2);
    p3 = dart::collision::fcl::transform(_transform, p3);
    model->addTriangle(p1, p2, p3);
  }
  model->endModel();
  return model;
}

// Create a cube mesh for collision detection
template <class BV>
::fcl::BVHModel<BV>* createCube(
    float _sizeX,
    float _sizeY,
    float _sizeZ,
    const dart::collision::fcl::Transform3& _transform)
{
  //  float n[6][3] = {
  //    {-1.0, 0.0, 0.0},
  //    {0.0, 1.0, 0.0},
  //    {1.0, 0.0, 0.0},
  //    {0.0, -1.0, 0.0},
  //    {0.0, 0.0, 1.0},
  //    {0.0, 0.0, -1.0}
  //  };
  int faces[6][4] = {{0, 1, 2, 3},
                     {3, 2, 6, 7},
                     {7, 6, 5, 4},
                     {4, 5, 1, 0},
                     {5, 6, 2, 1},
                     {7, 4, 0, 3}};
  float v[8][3];

  v[0][0] = v[1][0] = v[2][0] = v[3][0] = -_sizeX / 2;
  v[4][0] = v[5][0] = v[6][0] = v[7][0] = _sizeX / 2;
  v[0][1] = v[1][1] = v[4][1] = v[5][1] = -_sizeY / 2;
  v[2][1] = v[3][1] = v[6][1] = v[7][1] = _sizeY / 2;
  v[0][2] = v[3][2] = v[4][2] = v[7][2] = -_sizeZ / 2;
  v[1][2] = v[2][2] = v[5][2] = v[6][2] = _sizeZ / 2;

  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
  dart::collision::fcl::Vector3 p1, p2, p3;
  model->beginModel();

  for (int i = 0; i < 6; i++)
  {
    p1 = dart::collision::fcl::Vector3(
        v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);
    p2 = dart::collision::fcl::Vector3(
        v[faces[i][1]][0], v[faces[i][1]][1], v[faces[i][1]][2]);
    p3 = dart::collision::fcl::Vector3(
        v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);
    p1 = dart::collision::fcl::transform(_transform, p1);
    p2 = dart::collision::fcl::transform(_transform, p2);
    p3 = dart::collision::fcl::transform(_transform, p3);
    model->addTriangle(p1, p2, p3);
    p1 = dart::collision::fcl::Vector3(
        v[faces[i][0]][0], v[faces[i][0]][1], v[faces[i][0]][2]);
    p2 = dart::collision::fcl::Vector3(
        v[faces[i][2]][0], v[faces[i][2]][1], v[faces[i][2]][2]);
    p3 = dart::collision::fcl::Vector3(
        v[faces[i][3]][0], v[faces[i][3]][1], v[faces[i][3]][2]);
    p1 = dart::collision::fcl::transform(_transform, p1);
    p2 = dart::collision::fcl::transform(_transform, p2);
    p3 = dart::collision::fcl::transform(_transform, p3);
    model->addTriangle(p1, p2, p3);
  }
  model->endModel();
  return model;
}

template <class BV>
::fcl::BVHModel<BV>* createCylinder(
    double _baseRadius,
    double _topRadius,
    double _height,
    int _slices,
    int _stacks,
    const dart::collision::fcl::Transform3& _transform)
{
  const int CACHE_SIZE = 240;

  int i, j;
  float sinCache[CACHE_SIZE];
  float cosCache[CACHE_SIZE];
  float angle;
  float zBase;
  float zLow, zHigh;
  float sintemp, costemp;
  float deltaRadius;
  float radiusLow, radiusHigh;

  if (_slices >= CACHE_SIZE)
    _slices = CACHE_SIZE - 1;

  if (_slices < 2 || _stacks < 1 || _baseRadius < 0.0 || _topRadius < 0.0
      || _height < 0.0)
  {
    return nullptr;
  }

  /* Center at CoM */
  zBase = -_height / 2;

  /* Compute delta */
  deltaRadius = _baseRadius - _topRadius;

  /* Cache is the vertex locations cache */
  for (i = 0; i < _slices; i++)
  {
    angle = 2 * math::constantsd::pi() * i / _slices;
    sinCache[i] = sin(angle);
    cosCache[i] = cos(angle);
  }

  sinCache[_slices] = sinCache[0];
  cosCache[_slices] = cosCache[0];

  ::fcl::BVHModel<BV>* model = new ::fcl::BVHModel<BV>;
  dart::collision::fcl::Vector3 p1, p2, p3, p4;

  model->beginModel();

  /* Base of cylinder */
  sintemp = sinCache[0];
  costemp = cosCache[0];
  radiusLow = _baseRadius;
  zLow = zBase;
  p1 = dart::collision::fcl::Vector3(
      radiusLow * sintemp, radiusLow * costemp, zLow);
  p1 = dart::collision::fcl::transform(_transform, p1);
  for (i = 1; i < _slices; i++)
  {
    p2 = dart::collision::fcl::Vector3(
        radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
    p3 = dart::collision::fcl::Vector3(
        radiusLow * sinCache[i + 1], radiusLow * cosCache[i + 1], zLow);
    p2 = dart::collision::fcl::transform(_transform, p2);
    p3 = dart::collision::fcl::transform(_transform, p3);
    model->addTriangle(p1, p2, p3);

    Eigen::Vector3d v(radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
  }

  /* Body of cylinder */
  for (i = 0; i < _slices; i++)
  {
    for (j = 0; j < _stacks; j++)
    {
      zLow = j * _height / _stacks + zBase;
      zHigh = (j + 1) * _height / _stacks + zBase;
      radiusLow = _baseRadius - deltaRadius * (static_cast<float>(j) / _stacks);
      radiusHigh
          = _baseRadius - deltaRadius * (static_cast<float>(j + 1) / _stacks);

      p1 = dart::collision::fcl::Vector3(
          radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
      p2 = dart::collision::fcl::Vector3(
          radiusLow * sinCache[i + 1], radiusLow * cosCache[i + 1], zLow);
      p3 = dart::collision::fcl::Vector3(
          radiusHigh * sinCache[i], radiusHigh * cosCache[i], zHigh);
      p4 = dart::collision::fcl::Vector3(
          radiusHigh * sinCache[i + 1], radiusHigh * cosCache[i + 1], zHigh);
      p1 = dart::collision::fcl::transform(_transform, p1);
      p2 = dart::collision::fcl::transform(_transform, p2);
      p3 = dart::collision::fcl::transform(_transform, p3);
      p4 = dart::collision::fcl::transform(_transform, p4);

      model->addTriangle(p1, p2, p3);
      model->addTriangle(p2, p3, p4);
    }
  }

  /* Top of cylinder */
  sintemp = sinCache[0];
  costemp = cosCache[0];
  radiusLow = _topRadius;
  zLow = zBase + _height;
  p1 = dart::collision::fcl::Vector3(
      radiusLow * sintemp, radiusLow * costemp, zLow);
  p1 = dart::collision::fcl::transform(_transform, p1);
  for (i = 1; i < _slices; i++)
  {
    p2 = dart::collision::fcl::Vector3(
        radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
    p3 = dart::collision::fcl::Vector3(
        radiusLow * sinCache[i + 1], radiusLow * cosCache[i + 1], zLow);
    p2 = dart::collision::fcl::transform(_transform, p2);
    p3 = dart::collision::fcl::transform(_transform, p3);
    model->addTriangle(p1, p2, p3);
  }

  model->endModel();
  return model;
}

} // namespace collision
} // namespace dart

#endif // DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
