/*
 * Copyright (C) 2021 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 <gz/common/Util.hh>
#include <gz/common/Console.hh>
#include "gz/sim/Primitives.hh"

using namespace gz;
using namespace sim;

/////////////////////////////////////////////////
constexpr const char * kBoxSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <model name="box">
    <pose>0 0 0.5 0 0 0</pose>
    <link name="box_link">
      <inertial>
        <inertia>
          <ixx>0.16666</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.16666</iyy>
          <iyz>0</iyz>
          <izz>0.16666</izz>
        </inertia>
        <mass>1.0</mass>
      </inertial>
      <collision name="box_collision">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <visual name="box_visual">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
        <material>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>1 1 1 1</specular>
        </material>
     </visual>
    </link>
  </model>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char * kSphereSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <model name="sphere">
    <pose>0 0 0.5 0 0 0</pose>
    <link name="sphere_link">
      <inertial>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
        <mass>1.0</mass>
      </inertial>
      <collision name="sphere_collision">
        <geometry>
          <sphere>
            <radius>0.5</radius>
          </sphere>
        </geometry>
      </collision>
      <visual name="sphere_visual">
        <geometry>
          <sphere>
            <radius>0.5</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>1 1 1 1</specular>
        </material>
     </visual>
    </link>
  </model>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char * kCylinderSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <model name="cylinder">
    <pose>0 0 0.5 0 0 0</pose>
    <link name="cylinder_link">
      <inertial>
        <inertia>
          <ixx>0.1458</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1458</iyy>
          <iyz>0</iyz>
          <izz>0.125</izz>
        </inertia>
        <mass>1.0</mass>
      </inertial>
      <collision name="cylinder_collision">
        <geometry>
          <cylinder>
            <radius>0.5</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="cylinder_visual">
        <geometry>
          <cylinder>
            <radius>0.5</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>1 1 1 1</specular>
        </material>
     </visual>
    </link>
  </model>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char * kCapsuleSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <model name="capsule">
    <pose>0 0 0.5 0 0 0</pose>
    <link name="capsule_link">
      <inertial>
        <inertia>
          <ixx>0.074154</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.074154</iyy>
          <iyz>0</iyz>
          <izz>0.018769</izz>
        </inertia>
        <mass>1.0</mass>
      </inertial>
      <collision name="capsule_collision">
        <geometry>
          <capsule>
            <radius>0.2</radius>
            <length>0.6</length>
          </capsule>
        </geometry>
      </collision>
      <visual name="capsule_visual">
        <geometry>
          <capsule>
            <radius>0.2</radius>
            <length>0.6</length>
          </capsule>
        </geometry>
        <material>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>1 1 1 1</specular>
        </material>
     </visual>
    </link>
  </model>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char *kEllipsoidSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <model name="ellipsoid">
    <pose>0 0 0.5 0 0 0</pose>
    <link name="ellipsoid_link">
      <inertial>
        <inertia>
          <ixx>0.068</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.058</iyy>
          <iyz>0</iyz>
          <izz>0.026</izz>
        </inertia>
        <mass>1.0</mass>
      </inertial>
      <collision name="ellipsoid_collision">
        <geometry>
          <ellipsoid>
            <radii>0.2 0.3 0.5</radii>
          </ellipsoid>
        </geometry>
      </collision>
      <visual name="ellipsoid_visual">
        <geometry>
          <ellipsoid>
            <radii>0.2 0.3 0.5</radii>
          </ellipsoid>
        </geometry>
        <material>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>1 1 1 1</specular>
        </material>
     </visual>
    </link>
  </model>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char *kDirectionalSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <light type='directional' name='directionallight'>
    <pose>0 0 2 0 0 0</pose>
    <cast_shadows>true</cast_shadows>
    <diffuse>0.8 0.8 0.8 1</diffuse>
    <specular>0.2 0.2 0.2 1</specular>
    <attenuation>
      <range>1000</range>
      <constant>0.9</constant>
      <linear>0.01</linear>
      <quadratic>0.001</quadratic>
    </attenuation>
    <direction>0 0 -1</direction>
  </light>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char *kPointSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <light type='point' name='pointlight'>
    <pose>0 0 2 0 0 0</pose>
    <cast_shadows>false</cast_shadows>
    <diffuse>0.5 0.5 0.5 1</diffuse>
    <specular>0.5 0.5 0.5 1</specular>
    <attenuation>
      <range>4</range>
      <constant>0.2</constant>
      <linear>0.5</linear>
      <quadratic>0.01</quadratic>
    </attenuation>
  </light>
</sdf>
)";

/////////////////////////////////////////////////
constexpr const char *kSpotSdf = R"(<?xml version="1.0"?>
<sdf version="1.9">
  <light type='spot' name='spotlight'>
    <pose>0 0 2 0 0 0</pose>
    <cast_shadows>true</cast_shadows>
    <diffuse>0.5 0.5 0.5 1</diffuse>
    <specular>0.5 0.5 0.5 1</specular>
    <attenuation>
      <range>4</range>
      <constant>0.2</constant>
      <linear>0.5</linear>
      <quadratic>0.01</quadratic>
    </attenuation>
    <direction>0 0 -1</direction>
    <spot>
      <inner_angle>0.1</inner_angle>
      <outer_angle>0.5</outer_angle>
      <falloff>0.8</falloff>
    </spot>
  </light>
</sdf>
)";

/////////////////////////////////////////////////
std::string gz::sim::getPrimitiveShape(const PrimitiveShape &_type)
{
  switch(_type)
  {
    case PrimitiveShape::kBox:
      return kBoxSdf;
    case PrimitiveShape::kSphere:
      return kSphereSdf;
    case PrimitiveShape::kCylinder:
      return kCylinderSdf;
    case PrimitiveShape::kCapsule:
      return kCapsuleSdf;
    case PrimitiveShape::kEllipsoid:
      return kEllipsoidSdf;
    default:
      return "";
  }
}

/////////////////////////////////////////////////
std::string gz::sim::getPrimitiveLight(const PrimitiveLight &_type)
{
  switch(_type)
  {
    case PrimitiveLight::kDirectional:
      return kDirectionalSdf;
    case PrimitiveLight::kPoint:
      return kPointSdf;
    case PrimitiveLight::kSpot:
      return kSpotSdf;
    default:
      return "";
  }
}

/////////////////////////////////////////////////
std::string gz::sim::getPrimitive(const std::string &_typeName)
{
  std::string type = common::lowercase(_typeName);

  if (type == "box")
    return getPrimitiveShape(PrimitiveShape::kBox);
  else if (type == "sphere")
    return getPrimitiveShape(PrimitiveShape::kSphere);
  else if (type == "cylinder")
    return getPrimitiveShape(PrimitiveShape::kCylinder);
  else if (type == "capsule")
    return getPrimitiveShape(PrimitiveShape::kCapsule);
  else if (type == "ellipsoid")
    return getPrimitiveShape(PrimitiveShape::kEllipsoid);
  else if (type == "point")
    return getPrimitiveLight(PrimitiveLight::kPoint);
  else if (type == "directional")
    return getPrimitiveLight(PrimitiveLight::kDirectional);
  else if (type == "spot")
    return getPrimitiveLight(PrimitiveLight::kSpot);

  gzwarn << "Invalid model string " << type << "\n";
  gzwarn << "The valid options are:\n";
  gzwarn << " - box\n";
  gzwarn << " - sphere\n";
  gzwarn << " - cylinder\n";
  gzwarn << " - capsule\n";
  gzwarn << " - ellipsoid\n";
  gzwarn << " - point\n";
  gzwarn << " - directional\n";
  gzwarn << " - spot\n";
  return "";
}
