#pragma once

#if defined(_MSC_VER)
/* Disable some warnings on MSVC++ */
#pragma warning(disable : 4127 4702 4100 4515 4800 4146 4512 4819 4334)
#define WIN32_LEAN_AND_MEAN     /* Don't ever include MFC on Windows */
#define NOMINMAX                /* Don't override Min/max */
#endif

#include <string>
#include <iostream>
#include <algorithm>
#include <vector>
#include <memory>
#include <map>
#include <cstdint>
#include <thread>
#include <stdexcept>
#include <limits>
#include <iomanip>
#include <time.h>

#include <Eigen\Core>
#include <Eigen\Geometry>
#include <Eigen\Lu>
#include <glog\logging.h>
#include <tinyformat.h>
#include <ImathPlatform.h>
#include <filesystem\resolver.h>

#define NAMESPACE_BEGIN namespace Hikari {
#define NAMESPACE_END }

#if defined(MACOS)
	#define __PLATFORM_MACOS__
#elif defined(LINUX)
	#define __PLATFORM_LINUX__
#elif defined(WIN32)
	#define __PLATFORM_WINDOWS__
#endif

/* "Ray epsilon": relative error threshold for ray intersection computations */
#define Epsilon 1e-4

/* Used for BSDF::Eval() or BSDF::PDF() for the delta distribution. */
#define DeltaEpsilon 1e-3

/* To avoid the numerical error in computation */
#define MIN_ALPHA 5e-4
#define MAX_ALPHA 1.0

/* A few useful constants */
#undef M_PI

#define M_PI         3.14159265358979323846
#define INV_PI       0.31830988618379067154
#define INV_TWOPI    0.15915494309189533577
#define INV_FOURPI   0.07957747154594766788
#define SQRT_TWO     1.41421356237309504880
#define INV_SQRT_TWO 0.70710678118654752440

/* XML fields name */
#define XML_TYPE(Field)     Field##_XmlType
#define XML_VALUE(Field)    Field##_XmlValue

#define XML_TRANSFORM_TRANSLATE                  "translate"
#define XML_TRANSFORM_MATRIX                     "matrix"
#define XML_TRANSFORM_ROTATE                     "rotate"
#define XML_TRANSFORM_ANGLE                      "angle"
#define XML_TRANSFORM_AXIS                       "axis"
#define XML_TRANSFORM_SCALE                      "scale"
#define XML_TRANSFORM_LOOKAT                     "lookat"
#define XML_TRANSFORM_ORIGIN                     "origin"
#define XML_TRANSFORM_TARGET                     "target"
#define XML_TRANSFORM_UP                         "up"

#define XML_INTEGRATOR                           "integrator"
#define XML_INTEGRATOR_NORMAL                    "normals"
#define XML_INTEGRATOR_SIMPLE                    "simple"
#define XML_INTEGRATOR_SIMPLE_POSITION           "position"
#define XML_INTEGRATOR_SIMPLE_POWER              "power"
#define XML_INTEGRATOR_AO                        "ao"
#define XML_INTEGRATOR_AO_ALPHA                  "alpha"
#define XML_INTEGRATOR_AO_SAMPLE_COUNT           "sampleCount"
#define XML_INTEGRATOR_WHITTED                   "whitted"
#define XML_INTEGRATOR_WHITTED_DEPTH             "depth"
#define XML_INTEGRATOR_PATH_EMS                  "pathEMS"
#define XML_INTEGRATOR_PATH_EMS_DEPTH            "depth"
#define XML_INTEGRATOR_PATH_MATS                 "pathMATS"
#define XML_INTEGRATOR_PATH_MATS_DEPTH           "depth"
#define XML_INTEGRATOR_PATH_MIS                  "pathMIS"
#define XML_INTEGRATOR_PATH_MIS_DEPTH            "depth"

#define XML_EMITTER                              "emitter"
#define XML_EMITTER_AREA_LIGHT                   "area"
#define XML_EMITTER_AREA_LIGHT_RADIANCE          "radiance"
#define XML_EMITTER_POINT_LIGHT                  "point"
#define XML_EMITTER_POINT_LIGHT_POSITION         "position"
#define XML_EMITTER_POINT_LIGHT_POWER            "power"
#define XML_EMITTER_ENVIRONMENT_LIGHT            "env"
#define XML_EMITTER_ENVIRONMENT_LIGHT_FILENAME   "filename"
#define XML_EMITTER_ENVIRONMENT_LIGHT_SCALE      "scale"
#define XML_EMITTER_ENVIRONMENT_LIGHT_TO_WORLD   "toWorld"
#define XML_EMITTER_DIRECTIONAL_LIGHT            "directional"
#define XML_EMITTER_DIRECTIONAL_LIGHT_IRRADIANCE "irradiance"
#define XML_EMITTER_DIRECTIONAL_LIGHT_DIRECTION  "direction"
#define XML_EMITTER_CONSTANT_LIGHT               "constant"
#define XML_EMITTER_CONSTANT_LIGHT_RADIANCE      "radiance"

#define XML_TEXTURE                              "texture"
#define XML_TEXTURE_BITMAP                       "bitmap"
#define XML_TEXTURE_BITMAP_FILENAME              "filename"
#define XML_TEXTURE_BITMAP_GAMMA                 "gamma"
#define XML_TEXTURE_BITMAP_WRAP_MODE             "wrapMode"
#define XML_TEXTURE_BITMAP_WRAP_MODE_U           "uWrapMode"
#define XML_TEXTURE_BITMAP_WRAP_MODE_V           "vWrapMode"
#define XML_TEXTURE_BITMAP_WRAP_MODE_REPEAT      "repeat"
#define XML_TEXTURE_BITMAP_WRAP_MODE_CLAMP       "clamp"
#define XML_TEXTURE_BITMAP_WRAP_MODE_BLACK       "black"
#define XML_TEXTURE_BITMAP_FILTER_TYPE           "filterType"
#define XML_TEXTURE_BITMAP_FILTER_TYPE_NEAREST   "nearest"
#define XML_TEXTURE_BITMAP_FILTER_TYPE_BILINEAR  "bilinear"
#define XML_TEXTURE_BITMAP_FILTER_TYPE_TRILINEAR "trilinear"
#define XML_TEXTURE_BITMAP_FILTER_TYPE_EWA       "ewa"
#define XML_TEXTURE_BITMAP_MAX_ANISOTROPY        "maxAnisotropy"
#define XML_TEXTURE_BITMAP_OFFSE_U               "uOffset"
#define XML_TEXTURE_BITMAP_OFFSE_V               "vOffset"
#define XML_TEXTURE_BITMAP_SCALE_U               "uScale"
#define XML_TEXTURE_BITMAP_SCALE_V               "vScale"
#define XML_TEXTURE_BITMAP_CHANNEL               "channel"
#define XML_TEXTURE_BITMAP_CHANNEL_R             "r"
#define XML_TEXTURE_BITMAP_CHANNEL_RGB           "rgb"
#define XML_TEXTURE_CHECKERBOARD                 "checkerboard"
#define XML_TEXTURE_CHECKERBOARD_BLOCKS          "blocks"
#define XML_TEXTURE_CHECKERBOARD_COLOR_A         "colorA"
#define XML_TEXTURE_CHECKERBOARD_COLOR_B         "colorB"
#define XML_TEXTURE_CHECKERBOARD_OFFSE_U         "uOffset"
#define XML_TEXTURE_CHECKERBOARD_OFFSE_V         "vOffset"
#define XML_TEXTURE_CHECKERBOARD_SCALE_U         "uScale"
#define XML_TEXTURE_CHECKERBOARD_SCALE_V         "vScale"
#define XML_TEXTURE_WIREFRAME                    "wireframe"
#define XML_TEXTURE_WIREFRAME_INTERIOR_COLOR     "interiorColor"
#define XML_TEXTURE_WIREFRAME_EDGE_COLOR         "edgeColor"
#define XML_TEXTURE_WIREFRAME_EDGE_WIDTH         "edgeWidth"
#define XML_TEXTURE_WIREFRAME_TRANSITION_WIDTH   "transitionWidth"
#define XML_TEXTURE_WIREFRAME_OFFSE_U            "uOffset"
#define XML_TEXTURE_WIREFRAME_OFFSE_V            "vOffset"
#define XML_TEXTURE_WIREFRAME_SCALE_U            "uScale"
#define XML_TEXTURE_WIREFRAME_SCALE_V            "vScale"
#define XML_TEXTURE_GRID                         "grid"
#define XML_TEXTURE_GRID_COLOR_BACKGROUND        "backgroundColor"
#define XML_TEXTURE_GRID_COLOR_LINE              "lineColor"
#define XML_TEXTURE_GRID_LINE_WIDTH              "lineWidth"
#define XML_TEXTURE_GRID_LINES                   "lines"
#define XML_TEXTURE_GRID_OFFSE_U                 "uOffset"
#define XML_TEXTURE_GRID_OFFSE_V                 "vOffset"
#define XML_TEXTURE_GRID_SCALE_U                 "uScale"
#define XML_TEXTURE_GRID_SCALE_V                 "vScale"
#define XML_TEXTURE_CURVATURE                    "curvature"
#define XML_TEXTURE_CURVATURE_POSITIVE_COLOR     "positiveColor"
#define XML_TEXTURE_CURVATURE_NEGATIVE_COLOR     "negativeColor"
#define XML_TEXTURE_CURVATURE_SCALE              "scale"
#define XML_TEXTURE_CURVATURE_TYPE               "curvatureType"
#define XML_TEXTURE_CURVATURE_OFFSE_U            "uOffset"
#define XML_TEXTURE_CURVATURE_OFFSE_V            "vOffset"
#define XML_TEXTURE_CURVATURE_SCALE_U            "uScale"
#define XML_TEXTURE_CURVATURE_SCALE_V            "vScale"
#define XML_TEXTURE_SCALE                        "scale"
#define XML_TEXTURE_SCALE_SCALE                  "scale"

#define XML_ACCELERATION                         "acceleration"
#define XML_ACCELERATION_BRUTO_LOOP              "bruto"
#define XML_ACCELERATION_BVH                     "bvh"
#define XML_ACCELERATION_BVH_LEAF_SIZE           "leafSize"
#define XML_ACCELERATION_BVH_SPLIT_METHOD        "splitMethod"
#define XML_ACCELERATION_BVH_SPLIT_METHOD_CENTER "center"
#define XML_ACCELERATION_BVH_SPLIT_METHOD_SAH    "sah"
#define XML_ACCELERATION_HLBVH                   "hlbvh"
#define XML_ACCELERATION_HLBVH_LEAF_SIZE         "leafSize"

#define XML_SCENE                                "scene"
#define XML_SCENE_BACKGROUND                     "background"
#define XML_SCENE_FORCE_BACKGROUND               "forceBackground"

#define XML_MESH                                 "mesh"
#define XML_MESH_WAVEFRONG_OBJ                   "obj"
#define XML_MESH_WAVEFRONG_OBJ_FILENAME          "filename"
#define XML_MESH_WAVEFRONG_OBJ_TO_WORLD          "toWorld"

#define XML_BSDF                                 "bsdf"
#define XML_BSDF_GGX                             "ggx"
#define XML_BSDF_BECKMANN                        "beckmann"
#define XML_BSDF_DIELECTRIC                      "dielectric"
#define XML_BSDF_DIELECTRIC_INT_IOR              "intIOR"
#define XML_BSDF_DIELECTRIC_EXT_IOR              "extIOR"
#define XML_BSDF_DIELECTRIC_KS_REFLECT           "ksReflect"
#define XML_BSDF_DIELECTRIC_KS_REFRACT           "ksRefract"
#define XML_BSDF_DIFFUSE                         "diffuse"
#define XML_BSDF_DIFFUSE_ALBEDO                  "albedo"
#define XML_BSDF_MIRROR                          "mirror"
#define XML_BSDF_MICROFACET                      "microfacet"
#define XML_BSDF_MICROFACET_ALPHA                "alpha"
#define XML_BSDF_MICROFACET_INT_IOR              "intIOR"
#define XML_BSDF_MICROFACET_EXT_IOR              "extIOR"
#define XML_BSDF_MICROFACET_KD                   "kd"
#define XML_BSDF_CONDUCTOR                       "conductor"
#define XML_BSDF_CONDUCTOR_INT_IOR               "intIOR"
#define XML_BSDF_CONDUCTOR_EXT_IOR               "extIOR"
#define XML_BSDF_CONDUCTOR_K                     "k"
#define XML_BSDF_CONDUCTOR_KS                    "ks"
#define XML_BSDF_PLASTIC                         "plastic"
#define XML_BSDF_PLASTIC_INT_IOR                 "intIOR"
#define XML_BSDF_PLASTIC_EXT_IOR                 "extIOR"
#define XML_BSDF_PLASTIC_KS                      "ks"
#define XML_BSDF_PLASTIC_KD                      "kd"
#define XML_BSDF_PLASTIC_NONLINEAR               "nonlinear"
#define XML_BSDF_ROUGH_CONDUCTOR                 "roughConductor"
#define XML_BSDF_ROUGH_CONDUCTOR_INT_IOR         "intIOR"
#define XML_BSDF_ROUGH_CONDUCTOR_EXT_IOR         "extIOR"
#define XML_BSDF_ROUGH_CONDUCTOR_K               "k"
#define XML_BSDF_ROUGH_CONDUCTOR_KS              "ks"
#define XML_BSDF_ROUGH_CONDUCTOR_ALPHA           "alpha"
#define XML_BSDF_ROUGH_CONDUCTOR_ALPHA_U         "alphaU"
#define XML_BSDF_ROUGH_CONDUCTOR_ALPHA_V         "alphaV"
#define XML_BSDF_ROUGH_CONDUCTOR_TYPE            "type"
#define XML_BSDF_ROUGH_CONDUCTOR_AS              "as"
#define XML_BSDF_ROUGH_DIELECTRIC                "roughDielectric"
#define XML_BSDF_ROUGH_DIELECTRIC_INT_IOR        "intIOR"
#define XML_BSDF_ROUGH_DIELECTRIC_EXT_IOR        "extIOR"
#define XML_BSDF_ROUGH_DIELECTRIC_KS_REFLECT     "ksReflect"
#define XML_BSDF_ROUGH_DIELECTRIC_KS_REFRACT     "ksRefract"
#define XML_BSDF_ROUGH_DIELECTRIC_ALPHA          "alpha"
#define XML_BSDF_ROUGH_DIELECTRIC_ALPHA_U        "alphaU"
#define XML_BSDF_ROUGH_DIELECTRIC_ALPHA_V        "alphaV"
#define XML_BSDF_ROUGH_DIELECTRIC_TYPE           "type"
#define XML_BSDF_ROUGH_DIELECTRIC_AS             "as"
#define XML_BSDF_ROUGH_DIFFUSE                   "roughDiffuse"
#define XML_BSDF_ROUGH_DIFFUSE_ALBEDO            "albedo"
#define XML_BSDF_ROUGH_DIFFUSE_ALPHA             "alpha"
#define XML_BSDF_ROUGH_DIFFUSE_FAST_APPROX       "fastApprox"
#define XML_BSDF_ROUGH_PLASTIC                   "roughPlastic"
#define XML_BSDF_ROUGH_PLASTIC_INT_IOR           "intIOR"
#define XML_BSDF_ROUGH_PLASTIC_EXT_IOR           "extIOR"
#define XML_BSDF_ROUGH_PLASTIC_KS                "ks"
#define XML_BSDF_ROUGH_PLASTIC_KD                "kd"
#define XML_BSDF_ROUGH_PLASTIC_NONLINEAR         "nonlinear"
#define XML_BSDF_ROUGH_PLASTIC_TYPE              "type"
#define XML_BSDF_ROUGH_PLASTIC_ALPHA             "alpha"
#define XML_BSDF_ROUGH_PLASTIC_BECKMANN_RFT_DATA "data\\BeckmannRFTData.bin"
#define XML_BSDF_ROUGH_PLASTIC_GGX_RFT_DATA      "data\\GGXRFTData.bin"
#define XML_BSDF_COATING                         "coating"
#define XML_BSDF_COATING_INT_IOR                 "intIOR"
#define XML_BSDF_COATING_EXT_IOR                 "extIOR"
#define XML_BSDF_COATING_THICKNESS               "thickness"
#define XML_BSDF_COATING_SIGMA_A                 "sigmaA"
#define XML_BSDF_COATING_KS                      "ks"
#define XML_BSDF_ROUGH_COATING                   "roughCoating"
#define XML_BSDF_ROUGH_COATING_INT_IOR           "intIOR"
#define XML_BSDF_ROUGH_COATING_EXT_IOR           "extIOR"
#define XML_BSDF_ROUGH_COATING_THICKNESS         "thickness"
#define XML_BSDF_ROUGH_COATING_SIGMA_A           "sigmaA"
#define XML_BSDF_ROUGH_COATING_KS                "ks"
#define XML_BSDF_ROUGH_COATING_ALPHA             "alpha"
#define XML_BSDF_ROUGH_COATING_TYPE              "type"
#define XML_BSDF_ROUGH_COATING_BECKMANN_RFT_DATA "data\\BeckmannRFTData.bin"
#define XML_BSDF_ROUGH_COATING_GGX_RFT_DATA      "data\\GGXRFTData.bin"
#define XML_BSDF_TWO_SIDED                       "twoSided"
#define XML_BSDF_BUMP_MAP                        "bumpMap"
#define XML_BSDF_NORMAL_MAP                      "normalMap"

#define XML_MEDIUM                               "medium"

#define XML_PHASE                                "phase"

#define XML_CAMERA                               "camera"
#define XML_CAMERA_PERSPECTIVE                   "perspective"
#define XML_CAMERA_PERSPECTIVE_WIDTH             "width"
#define XML_CAMERA_PERSPECTIVE_HEIGHT            "height"
#define XML_CAMERA_PERSPECTIVE_TO_WORLD          "toWorld"
#define XML_CAMERA_PERSPECTIVE_FOV               "fov"
#define XML_CAMERA_PERSPECTIVE_NEAR_CLIP         "nearClip"
#define XML_CAMERA_PERSPECTIVE_FAR_CLIP          "farClip"

#define XML_TEST                                 "test"
#define XML_TEST_STUDENT_T                       "ttest"
#define XML_TEST_STUDENT_T_SIGNIFICANCE_LEVEL    "significanceLevel"
#define XML_TEST_STUDENT_T_ANGLES                "angles"
#define XML_TEST_STUDENT_T_REFERENCES            "references"
#define XML_TEST_STUDENT_T_SAMPLE_COUNT          "sampleCount"
#define XML_TEST_CHI2                            "chi2test"
#define XML_TEST_CHI2_SIGNIFICANCE_LEVEL         "significanceLevel"
#define XML_TEST_CHI2_RESOLUTION                 "resolution"
#define XML_TEST_CHI2_MIN_EXP_FREQUENCY          "minExpFrequency"
#define XML_TEST_CHI2_SAMPLE_COUNT               "sampleCount"
#define XML_TEST_CHI2_TEST_COUNT                 "testCount"

#define XML_FILTER                               "rfilter"
#define XML_FILTER_BOX                           "box"
#define XML_FILTER_GAUSSION                      "gaussian"
#define XML_FILTER_GAUSSION_RADIUS               "radius"
#define XML_FILTER_GAUSSION_STDDEV               "stddev"
#define XML_FILTER_MITCHELL_NETRAVALI            "mitchell"
#define XML_FILTER_MITCHELL_NETRAVALI_RADIUS     "radius"
#define XML_FILTER_MITCHELL_NETRAVALI_B          "B"
#define XML_FILTER_MITCHELL_NETRAVALI_C          "C"
#define XML_FILTER_TENT                          "tent"

#define XML_SAMPLER                              "sampler"
#define XML_SAMPLER_INDEPENDENT                  "independent"
#define XML_SAMPLER_INDEPENDENT_SAMPLE_COUNT     "sampleCount"

#define XML_SHAPE                                "shape"

/* Default setting */
#define DEFAULT_ACCELERATION_BVH_LEAF_SIZE         10
#define DEFAULT_ACCELERATION_BVH_SPLIT_METHOD      XML_ACCELERATION_BVH_SPLIT_METHOD_SAH

#define DEFAULT_ACCELERATION_HLBVH_LEAF_SIZE       10

#define DEFAULT_SCENE_ACCELERATION                 XML_ACCELERATION_BRUTO_LOOP

#define DEFAULT_SCENE_SAMPLER                      XML_SAMPLER_INDEPENDENT

#define DEFAULT_CAMERA_OUTPUTSIZE_X                1280
#define DEFAULT_CAMERA_OUTPUTSIZE_Y                720
#define DEFAULT_CAMERA_CAMERA_TO_WORLD             Transform()
#define DEFAULT_CAMERA_FOV                         30.0f
#define DEFAULT_CAMERA_NEAR_CLIP                   1e-4f
#define DEFAULT_CAMERA_FAR_CLIP                    1e4f
#define DEFAULT_CAMERA_FAR_CLIP                    1e4f
#define DEFAULT_CAMERA_RFILTER                     XML_FILTER_GAUSSION

#define DEFAULT_BSDF_DIELECTRIC_INT_IOR            1.5046f
#define DEFAULT_BSDF_DIELECTRIC_EXT_IOR            1.000277f /* Air */
#define DEFAULT_BSDF_DIELECTRIC_KS_REFLECT         Color3f(1.0f)
#define DEFAULT_BSDF_DIELECTRIC_KS_REFRACT         Color3f(1.0f)
#define DEFAULT_BSDF_DIFFUSE_ALBEDO                Color3f(0.5f)
#define DEFAULT_BSDF_MICROFACET_ALPHA              0.1f
#define DEFAULT_BSDF_MICROFACET_INT_IOR            1.5046f
#define DEFAULT_BSDF_MICROFACET_EXT_IOR            1.000277f /* Air */
#define DEFAULT_BSDF_MICROFACET_ALBEDO             Color3f(0.5f)
#define DEFAULT_BSDF_CONDUCTOR_INT_IOR             1.5046f
#define DEFAULT_BSDF_CONDUCTOR_EXT_IOR             1.000277f /* Air */
#define DEFAULT_BSDF_CONDUCTOR_K                   Color3f(1.0f)
#define DEFAULT_BSDF_CONDUCTOR_KS                  Color3f(1.0f)
#define DEFAULT_BSDF_PLASTIC_INT_IOR               1.5046f
#define DEFAULT_BSDF_PLASTIC_EXT_IOR               1.000277f /* Air */
#define DEFAULT_BSDF_PLASTIC_KS                    Color3f(1.0f)
#define DEFAULT_BSDF_PLASTIC_KD                    Color3f(0.5f)
#define DEFAULT_BSDF_PLASTIC_NONLINEAR             false
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_INT_IOR       1.5046f
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_EXT_IOR       1.000277f /* Air */
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_K             Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_KS            Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_ALPHA         0.1f
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_ALPHA_U       0.1f
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_ALPHA_V       0.1f
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_TYPE          XML_BSDF_BECKMANN
#define DEFAULT_BSDF_ROUGH_CONDUCTOR_AS            false
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_INT_IOR      1.5046f
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_EXT_IOR      1.000277f /* Air */
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_KS_REFLECT   Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_KS_REFRACT   Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_ALPHA        0.1f
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_ALPHA_U      0.1f
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_ALPHA_V      0.1f
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_TYPE         XML_BSDF_BECKMANN
#define DEFAULT_BSDF_ROUGH_DIELECTRIC_AS           false
#define DEFAULT_BSDF_ROUGH_DIFFUSE_ALBEDO          Color3f(0.5f)
#define DEFAULT_BSDF_ROUGH_DIFFUSE_ALPHA           0.2f
#define DEFAULT_BSDF_ROUGH_DIFFUSE_FAST_APPROX     false
#define DEFAULT_BSDF_ROUGH_PLASTIC_INT_IOR         1.49f
#define DEFAULT_BSDF_ROUGH_PLASTIC_EXT_IOR         1.000277f /* Air */
#define DEFAULT_BSDF_ROUGH_PLASTIC_KS              Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_PLASTIC_KD              Color3f(0.5f)
#define DEFAULT_BSDF_ROUGH_PLASTIC_NONLINEAR       false
#define DEFAULT_BSDF_ROUGH_PLASTIC_TYPE            XML_BSDF_BECKMANN
#define DEFAULT_BSDF_ROUGH_PLASTIC_ALPHA           0.1f
#define DEFAULT_BSDF_COATING_INT_IOR               1.49f
#define DEFAULT_BSDF_COATING_EXT_IOR               1.000277f /* Air */
#define DEFAULT_BSDF_COATING_THICKNESS             1.0f
#define DEFAULT_BSDF_COATING_SIGMA_A               Color3f(0.0f)
#define DEFAULT_BSDF_COATING_KS                    Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_COATING_INT_IOR         1.49f
#define DEFAULT_BSDF_ROUGH_COATING_EXT_IOR         1.000277f /* Air */
#define DEFAULT_BSDF_ROUGH_COATING_THICKNESS       1.0f
#define DEFAULT_BSDF_ROUGH_COATING_SIGMA_A         Color3f(0.0f)
#define DEFAULT_BSDF_ROUGH_COATING_KS              Color3f(1.0f)
#define DEFAULT_BSDF_ROUGH_COATING_ALPHA           0.1f
#define DEFAULT_BSDF_ROUGH_COATING_TYPE            XML_BSDF_BECKMANN

#define DEFAULT_FILTER_GAUSSIAN_RADIUS             2.0f
#define DEFAULT_FILTER_GAUSSIAN_STDDEV             0.5f
#define DEFAULT_FILTER_MITCHELL_RADIUS             2.0f
#define DEFAULT_FILTER_MITCHELL_B                  (1.0f / 3.0f)
#define DEFAULT_FILTER_MITCHELL_C                  (1.0f / 3.0f)

#define DEFAULT_MESH_TO_WORLD                      Transform()

#define DEFAULT_INTEGRATOR_AO_ALPHA                1e6f
#define DEFAULT_INTEGRATOR_AO_SAMPLE_COUNT         16
#define DEFAULT_INTEGRATOR_WHITTED_DEPTH           -1

#define DEFAULT_SAMPLER_INDEPENDENT_SAMPLE_COUNT   1

#define DEFAULT_MESH_BSDF                          XML_BSDF_DIFFUSE

#define DEFAULT_EMITTER_ENVIRONMENT_SCALE          1.0f
#define DEFAULT_EMITTER_ENVIRONMENT_TO_WORLD       Transform()

#define DEFAULT_TEST_STUDENT_T_SIGNIFICANCE_LEVEL  0.01f
#define DEFAULT_TEST_STUDENT_T_ANGLES              ""
#define DEFAULT_TEST_STUDENT_T_REFERENCES          ""
#define DEFAULT_TEST_STUDENT_T_SAMPLE_COUNT        100000

#define DEFAULT_TEST_CHI2_SIGNIFICANCE_LEVEL       0.01f
#define DEFAULT_TEST_CHI2_RESOLUTION               10
#define DEFAULT_TEST_CHI2_MIN_EXP_FREQUENCY        5
#define DEFAULT_TEST_CHI2_SAMPLE_COUNT             -1
#define DEFAULT_TEST_CHI2_TEST_COUNT               5

#define DEFAULT_SCENE_BACKGROUND                   Color3f(0.0f)
#define DEFAULT_SCENE_FORCE_BACKGROUND             false

#define DEFAULT_TEXTURE_BITMAP_GAMMA               1.0f
#define DEFAULT_TEXTURE_BITMAP_WRAP_MODE           XML_TEXTURE_BITMAP_WRAP_MODE_REPEAT
#define DEFAULT_TEXTURE_BITMAP_WRAP_MODE_U         XML_TEXTURE_BITMAP_WRAP_MODE_REPEAT
#define DEFAULT_TEXTURE_BITMAP_WRAP_MODE_V         XML_TEXTURE_BITMAP_WRAP_MODE_REPEAT
#define DEFAULT_TEXTURE_BITMAP_FILTER_TYPE         XML_TEXTURE_BITMAP_FILTER_TYPE_EWA
#define DEFAULT_TEXTURE_BITMAP_MAX_ANISOTROPY      20.0f
#define DEFAULT_TEXTURE_BITMAP_OFFSET_U            0.0f
#define DEFAULT_TEXTURE_BITMAP_OFFSET_V            0.0f
#define DEFAULT_TEXTURE_BITMAP_SCALE_U             1.0f
#define DEFAULT_TEXTURE_BITMAP_SCALE_V             1.0f
#define DEFAULT_TEXTURE_BITMAP_CHANNEL             XML_TEXTURE_BITMAP_CHANNEL_RGB
#define DEFAULT_TEXTURE_CHECKERBOARD_BLOCKS        10
#define DEFAULT_TEXTURE_CHECKERBOARD_COLOR_A       Color3f(0.4f)
#define DEFAULT_TEXTURE_CHECKERBOARD_COLOR_B       Color3f(0.2f)
#define DEFAULT_TEXTURE_CHECKERBOARD_OFFSET_U      0.0f
#define DEFAULT_TEXTURE_CHECKERBOARD_OFFSET_V      0.0f
#define DEFAULT_TEXTURE_CHECKERBOARD_SCALE_U       1.0f
#define DEFAULT_TEXTURE_CHECKERBOARD_SCALE_V       1.0f
#define DEFAULT_TEXTURE_WIREFRAME_INTERIOR_COLOR   Color3f(0.5f)
#define DEFAULT_TEXTURE_WIREFRAME_EDGE_COLOR       Color3f(0.1f)
#define DEFAULT_TEXTURE_WIREFRAME_EDGE_WIDTH       0.0f
#define DEFAULT_TEXTURE_WIREFRAME_TRANSITION_WIDTH 0.5f
#define DEFAULT_TEXTURE_WIREFRAME_OFFSET_U         0.0f
#define DEFAULT_TEXTURE_WIREFRAME_OFFSET_V         0.0f
#define DEFAULT_TEXTURE_WIREFRAME_SCALE_U          1.0f
#define DEFAULT_TEXTURE_WIREFRAME_SCALE_V          1.0f
#define DEFAULT_TEXTURE_GRID_COLOR_BACKGROUND      Color3f(0.2f)
#define DEFAULT_TEXTURE_GRID_COLOR_LINE            Color3f(0.4f)
#define DEFAULT_TEXTURE_GRID_LINE_WIDTH            0.01f
#define DEFAULT_TEXTURE_GRID_LINES                 10
#define DEFAULT_TEXTURE_GRID_OFFSET_U              0.0f
#define DEFAULT_TEXTURE_GRID_OFFSET_V              0.0f
#define DEFAULT_TEXTURE_GRID_SCALE_U               1.0f
#define DEFAULT_TEXTURE_GRID_SCALE_V               1.0f
#define DEFAULT_TEXTURE_CURVATURE_SCALE            1.0f
#define DEFAULT_TEXTURE_CURVATURE_POSITIVE_COLOR   Color3f(1.0f, 0.0f, 0.0f)
#define DEFAULT_TEXTURE_CURVATURE_NEGATIVE_COLOR   Color3f(0.0f, 0.0f, 1.0f)
#define DEFAULT_TEXTURE_CURVATURE_OFFSET_U         0.0f
#define DEFAULT_TEXTURE_CURVATURE_OFFSET_V         0.0f
#define DEFAULT_TEXTURE_CURVATURE_SCALE_U          1.0f
#define DEFAULT_TEXTURE_CURVATURE_SCALE_V          1.0f

NAMESPACE_BEGIN

/* Counting the time between TIMER_START and TIMER_END */
#define TIMER_START(Var) clock_t __Start##Var, __Finish##Var; double Var; __Start##Var = clock();
#define TIMER_END(Var) __Finish##Var = clock();  Var = double(__Finish##Var - __Start##Var) / (CLOCKS_PER_SEC);

/* Import cout, cerr, endl for debugging purposes */
using std::cout;
using std::cerr;
using std::endl;

/* Forward declarations */
template <typename TScalar, int TDimension>  struct TVector;
template <typename TScalar, int TDimension>  struct TPoint;
template <typename TPoint, typename TVector> struct TRay;
template <typename TPoint>                   struct TBoundingBox;
template <typename T>                        class MipMap;

class Acceleration;
class Bitmap;
class ImageBlock;
class BlockGenerator;
class BSDF;
struct BSDFQueryRecord;
class Camera;
struct Color3f;
struct Color4f;
struct DiscretePDF1D;
struct DiscretePDF2D;
class Emitter;
struct Frame;
class Integrator;
class Mesh;
struct Intersection;
class Object;
class ObjectFactory;
class PropertyList;
class ReconstructionFilter;
class Sampler;
class Sampling;
class Scene;
class Timer;
class Shape;
class MicrofacetDistribution;
class Texture;
class Texture2D;
class ConstantColor3fTexture;
class ConstantFloatTexture;
class Color3fAdditionTexture;
class Color3fSubtractionTexture;
class Color3fProductTexture;
struct RoughTransmittance;

/* Basic data structures (vectors, points, rays, bounding boxes,
kd-trees) are oblivious to the underlying data type and dimension.
The following list of typedefs establishes some convenient aliases
for specific types. */
using Vector1f      = TVector<float, 1>;
using Vector2f      = TVector<float, 2>;
using Vector3f      = TVector<float, 3>;
using Vector4f      = TVector<float, 4>;
using Vector1d      = TVector<double, 1>;
using Vector2d      = TVector<double, 2>;
using Vector3d      = TVector<double, 3>;
using Vector4d      = TVector<double, 4>;
using Vector1i      = TVector<int, 1>;
using Vector2i      = TVector<int, 2>;
using Vector3i      = TVector<int, 3>;
using Vector4i      = TVector<int, 4>;
using Point1f       = TPoint<float, 1>;
using Point2f       = TPoint<float, 2>;
using Point3f       = TPoint<float, 3>;
using Point4f       = TPoint<float, 4>;
using Point1d       = TPoint<double, 1>;
using Point2d       = TPoint<double, 2>;
using Point3d       = TPoint<double, 3>;
using Point4d       = TPoint<double, 4>;
using Point1i       = TPoint<int, 1>;
using Point2i       = TPoint<int, 2>;
using Point3i       = TPoint<int, 3>;
using Point4i       = TPoint<int, 4>;
using BoundingBox1f = TBoundingBox<Point1f>;
using BoundingBox2f = TBoundingBox<Point2f>;
using BoundingBox3f = TBoundingBox<Point3f>;
using BoundingBox4f = TBoundingBox<Point4f>;
using BoundingBox1d = TBoundingBox<Point1d>;
using BoundingBox2d = TBoundingBox<Point2d>;
using BoundingBox3d = TBoundingBox<Point3d>;
using BoundingBox4d = TBoundingBox<Point4d>;
using BoundingBox1i = TBoundingBox<Point1i>;
using BoundingBox2i = TBoundingBox<Point2i>;
using BoundingBox3i = TBoundingBox<Point3i>;
using BoundingBox4i = TBoundingBox<Point4i>;
using Ray2f         = TRay<Point2f, Vector2f>;
using Ray3f         = TRay<Point3f, Vector3f>;
using MatrixXf      = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>;
using MatrixXu      = Eigen::Matrix<uint32_t, Eigen::Dynamic, Eigen::Dynamic>;
using MipMap3f      = MipMap<Color3f>;
using MipMap1f      = MipMap<float>;

/* Simple exception class, which stores a human-readable error description */
class HikariException : public std::runtime_error
{
public:
	// Variadic template constructor to support printf-style arguments
	template <typename... Args>
	HikariException(const char * pFmt, const Args &... Arg) : std::runtime_error(tfm::format(pFmt, Arg...)) { }
};

/* Check whether "Value" is in the range (Min, Max). Exception will be throw
with info "Msg" if it is not in the range. */
template <typename T, typename... Args>
void CheckOO(
	T Value,
	T Min,
	T Max,
	const char * pFmt,
	const Args &... Arg
)
{
	if (Value <= Min || Value >= Max)
	{
		throw HikariException(pFmt, Arg...);
	}
}

/* Check whether "Value" is in the range (Min, Max]. Exception will be throw
with info "Msg" if it is not in the range. */
template <typename T, typename... Args>
void CheckOC(
	T Value,
	T Min,
	T Max,
	const char * pFmt,
	const Args &... Arg
)
{
	if (Value <= Min || Value > Max)
	{
		throw HikariException(pFmt, Arg...);
	}
}

/* Check whether "Value" is in the range [Min, Max). Exception will be throw
with info "Msg" if it is not in the range. */
template <typename T, typename... Args>
void CheckCO(
	T Value,
	T Min,
	T Max,
	const char * pFmt,
	const Args &... Arg
)
{
	if (Value < Min || Value >= Max)
	{
		throw HikariException(pFmt, Arg...);
	}
}

/* Check whether "Value" is in the range [Min, Max]. Exception will be throw
with info "Msg" if it is not in the range. */
template <typename T, typename... Args>
void CheckCC(
	T Value,
	T Min,
	T Max,
	const char * pFmt,
	const Args &... Arg
)
{
	if (Value < Min || Value > Max)
	{
		throw HikariException(pFmt, Arg...);
	}
}

/// Return the number of cores (real and virtual)
int GetCoreCount();

/// Indent a string by the specified number of spaces
std::string Indent(const std::string & String, int Amount = 2);

/// Convert a string to lower case
std::string ToLower(const std::string & String);

/// Convert a string into an boolean value
bool ToBool(const std::string & String);

/// Convert a string into a signed integer value
int ToInt(const std::string & String);

/// Convert a string into an unsigned integer value
unsigned int ToUInt(const std::string & String);

/// Convert a string into a floating point value
float ToFloat(const std::string & String);

/// Convert a string into a 3D vector
Eigen::Vector3f ToVector3f(const std::string & String);

/// Tokenize a string into a list by splitting at 'delim'
std::vector<std::string> Tokenize(const std::string & String, const std::string & Delim = ", ", bool bIncludeEmpty = false);

/// Check if a string ends with another string
bool EndsWith(const std::string & String, const std::string & Ending);

/// Convert a time value in milliseconds into a human-readable string
std::string TimeString(double Time, bool bPrecise = false);

/// Convert a memory amount in bytes into a human-readable string
std::string MemString(size_t Size, bool bPrecise = false);

/// Measures associated with probability distributions
enum class EMeasure
{
	EUnknownMeasure = 0,
	ESolidAngle     = 1,
	EDiscrete       = 2
};

/// Type of the emitter
enum class EEmitterType
{
	EUnknown = 0,
	EPoint = 1,
	EArea = 2,
	EEnvironment = 3,
	EDirectional = 4
};

/// Type of the light transport. Currently not used, so set it to ERadiance now.
enum class ETransportMode
{
	EUnknown = 0,
	ERadiance = 1,
	EImportance = 2
};

/// Wrap mode of the texture, which decides the behavior when u > 1 or v > 1.
enum class EWrapMode
{
	ERepeat = 0,
	EBlack = 1,
	EClamp = 2
};

/// Filter type of the sampler
enum class EFilterType
{
	ENearest = 0,
	EBilinear = 1,
	ETrilinear = 2,
	EEWA = 3
};

/// Lobe type of the BSDF
enum EBSDFType
{
	ENull                = 0x001,
	EDiffuseReflection   = 0x002,
	EDiffuseTransmission = 0x004,
	EGlossyReflection    = 0x008,
	EGlossyTransmission  = 0x010,
	EDeltaReflection     = 0x020,
	EDeltaTransmission   = 0x040,

	EExtraSampling       = 0x080,
	EUVDependent         = 0x100,
};

/// Convert radians to degrees
inline float RadToDeg(float Value) { return Value * (180.0f / float(M_PI)); }

/// Convert degrees to radians
inline float DegToRad(float Value) { return Value * (float(M_PI) / 180.0f); }

/// Emulate sincosf using sinf() and cosf()
inline void SinCos(float Theta, float * pSin, float * pCos)
{
	*pSin = sinf(Theta);
	*pCos = cosf(Theta);
}

/// Simple floating point clamping function
inline float Clamp(float Value, float Min, float Max)
{
	if (Value < Min)
	{
		return Min;
	}
	else if (Value > Max)
	{
		return Max;
	}
	else
	{
		return Value;
	}
}

/// Simple integer clamping function
inline int Clamp(int Value, int Min, int Max)
{
	if (Value < Min)
	{
		return Min;
	}
	else if (Value > Max)
	{
		return Max;
	}
	else
	{
		return Value;
	}
}

Color3f Clamp(Color3f Value, Color3f Min, Color3f Max);
Color4f Clamp(Color4f Value, Color4f Min, Color4f Max);

/// Linearly interpolate between two values
inline float Lerp(float T, float V1, float V2)
{
	return (1.0f - T) * V1 + T * V2;
}

/// Linearly interpolate between two Color
Color3f Lerp(float T, const Color3f & V1, const Color3f & V2);
Color4f Lerp(float T, const Color4f & V1, const Color4f & V2);

/// Always-positive modulo operation
inline int Mod(int A, int B)
{
	int R = A % B;
	return (R < 0) ? R + B : R;
}

/// Arcsine variant that gracefully handles arguments > 1 that are due to roundoff errors
inline float SafeAsin(float Value)
{
	return std::asin(std::min(1.0f, std::max(-1.0f, Value)));
}

/// Arcsine variant that gracefully handles arguments > 1 that are due to roundoff errors
inline double SafeAsin(double Value)
{
	return std::asin(std::min(1.0, std::max(-1.0, Value)));
}

/// Arccosine variant that gracefully handles arguments > 1 that are due to roundoff errors
inline float SafeAcos(float Value)
{
	return std::acos(std::min(1.0f, std::max(-1.0f, Value)));
}

/// Arccosine variant that gracefully handles arguments > 1 that are due to roundoff errors
inline double SafeAcos(double Value)
{
	return std::acos(std::min(1.0, std::max(-1.0, Value)));
}

/// Square root variant that gracefully handles arguments < 0 that are due to roundoff errors
inline float SafeSqrt(float Value)
{
	return std::sqrt(std::max(0.0f, Value));
}

/// Square root variant that gracefully handles arguments < 0 that are due to roundoff errors
inline double SafeSqrt(double Value)
{
	return std::sqrt(std::max(0.0, Value));
}

inline float Hypot2(float A, float B)
{
	float Result;
	if (std::abs(A) > std::abs(B))
	{
		Result = B / A;
		Result = std::abs(A) * std::sqrt(1.0f + Result * Result);
	}
	else if (B != 0.0f)
	{
		Result = A / B;
		Result = std::abs(B) * std::sqrt(1.0f + Result * Result);
	}
	else
	{
		Result = 0.0f;
	}
	return Result;
}

inline double Hypot2(double A, double B)
{
	double Result;
	if (std::abs(A) > std::abs(B))
	{
		Result = B / A;
		Result = std::abs(A) * std::sqrt(1.0 + Result * Result);
	}
	else if (B != 0.0)
	{
		Result = A / B;
		Result = std::abs(B) * std::sqrt(1.0 + Result * Result);
	}
	else
	{
		Result = 0.0;
	}
	return Result;
}

inline bool SolveLinearSystem2x2(const float A[2][2], const float B[2], float X[2])
{
	float Det = A[0][0] * A[1][1] - A[0][1] * A[1][0];

	constexpr float InvOverflow = 1.0f / std::numeric_limits<float>::max();

	if (std::abs(Det) <= InvOverflow)
	{
		return false;
	}

	float InvDet = 1.0f / Det;

	X[0] = (A[1][1] * B[0] - A[0][1] * B[1]) * InvDet;
	X[1] = (A[0][0] * B[1] - A[1][0] * B[0]) * InvDet;

	return true;
}

/// Simple signum function -- note that it returns the FP sign of the input (and never zero)
inline float Signum(float Value)
{
#if defined(__PLATFORM_WINDOWS__)
	return float(_copysign(1.0f, Value));
#elif
	return float(copysign(1.0f, Value));
#endif
}

template <typename T>
inline constexpr bool IsPowerOf2(T Value)
{
	return Value && !(Value & (Value - 1));
}

inline int32_t RoundUpPow2(int32_t Value)
{
	Value--;
	Value |= Value >> 1;
	Value |= Value >> 2;
	Value |= Value >> 4;
	Value |= Value >> 8;
	Value |= Value >> 16;
	return Value + 1;
}

inline int64_t RoundUpPow2(int64_t Value)
{
	Value--;
	Value |= Value >> 1;
	Value |= Value >> 2;
	Value |= Value >> 4;
	Value |= Value >> 8;
	Value |= Value >> 16;
	Value |= Value >> 32;
	return Value + 1;
}

inline int Log2Int(uint32_t V)
{
#if defined(__PLATFORM_WINDOWS__)
	unsigned long LZ = 0;
	if (_BitScanReverse(&LZ, V)) { return LZ; }
	return 0;
#else
	return 31 - __builtin_clz(V);
#endif
}

inline int Log2Int(int32_t V)
{
	return Log2Int(uint32_t(V));
}

inline int Log2Int(uint64_t V)
{
#if defined(__PLATFORM_WINDOWS__)
	unsigned long LZ = 0;
#if defined(_WIN64)
	_BitScanReverse64(&LZ, V);
#else
	if (_BitScanReverse(&LZ, V >> 32))
	{
		LZ += 32;
	}
	else
	{
		_BitScanReverse(&LZ, V & 0xffffffff);
	}
#endif
	return LZ;
#else
	return 63 - __builtin_clzll(V);
#endif
}

inline int Log2Int(int64_t V)
{
	return Log2Int(uint64_t(V));
}

inline float GammaCorrect(float Value, float InvGamma)
{
	return InvGamma == 1.0f ? Value : std::pow(Value, InvGamma);
}

inline int ModPositive(int A, int B)
{
	int R = A % B;
	return (R < 0) ? R + B : R;
}

/// S-shape interpolation between two values
inline float SmoothStep(float Min, float Max, float Value)
{
	float V = Clamp((Value - Min) / (Max - Min), 0.0f, 1.0f);
	return V * V * (-2.0f * V + 3.0f);
}

/**
 * \brief Evaluate a cubic spline interpolant of a uniformly sampled 1D function
 *
 * This implementation relies on Catmull-Rom splines, i.e. it uses finite
 * differences to approximate the derivatives at the endpoints of each spline
 * segment.
 *
 * \param X
 *      Evaluation point
 * \param pValues
 *      Floating point array containing size regularly spaced evaluations
 *      in the range [Min, Max] of the function to be approximated.
 * \param Size
 *      Denotes the size of the pValues array
 * \param Min
 *      Position of the first knot
 * \param Max
 *      Position of the last knot
 * \param bExtrapolate
 *      Extrapolate values when X is out of range?
 * \return
 *      The interpolated value or zero when bExtrapolate = false
 *      and X lies outside of [Min, Max]
 */
float EvalCubicInterpolate1D(
	float X,
	const float * pValues,
	int Size,
	float Min,
	float Max,
	bool bExtrapolate = false
);

/**
 * \brief Evaluate a cubic spline interpolant of a uniformly sampled 2D function
 *
 * This implementation relies on a tensor product of Catmull-Rom splines, i.e. it uses
 * finite differences to approximate the derivatives at the endpoints of each spline
 * patch.
 *
 * \param P
 *      Evaluation point
 * \param pValues
 *      A 2D floating point array of Size.x * Size.y cells containing regularly
 *      spaced evaluations of the function to be interpolated on the domain [Min, Max].
 *      Consecutive entries of this array correspond to increments in the 'x' coordinate.
 * \param Size
 *      Denotes the size of the pValues array (along each dimension)
 * \param Min
 *      Position of the first knot on each dimension
 * \param Max
 *      Position of the last knot on each dimension
 * \param bExtrapolate
 *      Extrapolate values when P is out of range?
 * \return
 *      The interpolated value or zero when bExtrapolate=false and
 *      P lies outside of the knot range
 */
float EvalCubicInterpolate2D(
	const Point2f & P,
	const float * pValues,
	const Point2i & Size,
	const Point2f & Min,
	const Point2f & Max,
	bool bExtrapolate = false
);

/**
 * \brief Evaluate a cubic spline interpolant of a uniformly sampled 3D function
 *
 * This implementation relies on a tensor product of Catmull-Rom splines, i.e. it uses
 * finite differences to approximate the derivatives at the endpoints of each spline
 * region.
 *
 * \param P
 *      Evaluation point of the interpolant
 * \param pValues
 *      A 3D floating point array of Size.x * Size.y * Size.z cells containing regularly
 *      spaced evaluations of the function to be interpolated on the domain [Min, Max].
 *      Consecutive entries of this array correspond to increments in the 'x' coordinate,
 *      then 'y', and finally 'z' increments.
 * \param Size
 *      Denotes the size of the pValues array (along each dimension)
 * \param Min
 *      Position of the first knot on each dimension
 * \param Max
 *      Position of the last knot on each dimension
 * \param bExtrapolate
 *      Extrapolate values when P is out of range?
 * \return
 *      The interpolated value or zero when bExtrapolate=false and
 *      P lies outside of the knot range
 */
float EvalCubicInterpolate3D(
	const Point3f & P,
	const float * pValues,
	const Point3i & Size,
	const Point3f & Min,
	const Point3f & Max,
	bool bExtrapolate = false
);

/// Compute a direction for the given coordinates in spherical coordinates
Vector3f SphericalDirection(float Theta, float Phi);

/// Compute a direction for the given coordinates in spherical coordinates
Point2f SphericalCoordinates(const Vector3f & Dir);

/// Fresnel coefficient for dielectric material. Eta = IntIOR / ExtIOR
float FresnelDielectric(float CosThetaI, float Eta, float InvEta, float & CosThetaT);

/// Fresnel coefficient for conductor material. Eta = IntIOR / ExtIOR, EtaK = K / ExtIOR
Color3f FresnelConductor(float CosThetaI, const Color3f & Eta, const Color3f & EtaK);

/**
* Approximating the diffuse Frensel reflectance
* for the Eta < 1.0 and Eta > 1.0 cases.
*/
float ApproxFresnelDiffuseReflectance(float Eta);

/// Complete the set {a} to an orthonormal base
void CoordinateSystem(const Vector3f & Va, Vector3f & Vb, Vector3f & Vc);

/// Reflection in local coordinates
Vector3f Reflect(const Vector3f & Wi);

/// Refraction in local coordinates
Vector3f Refract(const Vector3f & Wi, float CosThetaT, float Eta, float InvEta);

/// Reflection in global coordinates
Vector3f Reflect(const Vector3f & Wi, const Vector3f & M);

/// Refraction in global coordinates
Vector3f Refract(const Vector3f & Wi, const Vector3f & M, float CosThetaT, float Eta, float InvEta);

/**
* \brief Return the global file resolver instance
*
* This class is used to locate resource files (e.g. mesh or
* texture files) referenced by a scene being loaded
*/
filesystem::resolver * GetFileResolver();

NAMESPACE_END


#ifndef VEXCL_SPARSE_ELL_HPP
#define VEXCL_SPARSE_ELL_HPP

/*
The MIT License

Copyright (c) 2012-2018 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
 * \file   vexcl/sparse/ell.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Sparse matrix in ELL format.
 */

#include <vector>
#include <type_traits>
#include <utility>

#include <boost/foreach.hpp>
#include <boost/range.hpp>

#include <vexcl/util.hpp>
#include <vexcl/operations.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/sparse/product.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/tagged_terminal.hpp>
#include <vexcl/reductor.hpp>

#include <vexcl/reductor.hpp>
#include <vexcl/vector_view.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/function.hpp>
#include <vexcl/eval.hpp>
#include <vexcl/vector_pointer.hpp>
#include <vexcl/scan.hpp>
#include <vexcl/sparse/spmv_ops.hpp>

namespace vex {
namespace sparse {

template <typename Val, typename Col = int, typename Ptr = Col>
class ell {
    public:
        typedef Val value_type;

        typedef Val val_type;
        typedef Col col_type;
        typedef Ptr ptr_type;

        template <class PtrRange, class ColRange, class ValRange>
        ell(
                const std::vector<backend::command_queue> &q,
                size_t nrows, size_t ncols,
                const PtrRange &ptr,
                const ColRange &col,
                const ValRange &val,
                bool fast = true
           ) :
            q(q[0]), n(nrows), m(ncols), nnz(boost::size(val)),
            ell_pitch(alignup(nrows, 16U)), csr_nnz(0)
        {
            precondition(q.size() == 1,
                    "sparse::ell is only supported for single-device contexts");

            if (fast) {
                convert(ptr, col, val);
                return;
            }

            /* 1. Get optimal ELL widths for local and remote parts. */
            // Speed of ELL relative to CSR:
            const double ell_vs_csr = 3.0;

            // Find maximum widths for local and remote parts:
            int max_width = 0;
            for(size_t i = 0; i < n; ++i)
                max_width = std::max(max_width, static_cast<int>(ptr[i+1] - ptr[i]));

            // Build width distribution histogram.
            std::vector<size_t> hist(max_width + 1, 0);
            for(size_t i = 0; i < n; ++i)
                ++hist[ptr[i+1] - ptr[i]];

            // Estimate optimal width for ELL part of the matrix.
            ell_width = max_width;
            {
                size_t rows = n;
                for(int i = 0; i < max_width; ++i) {
                    rows -= hist[i]; // Number of rows wider than i.
                    if (ell_vs_csr * rows < n) {
                        ell_width = i;
                        break;
                    }
                }
            }

            if (ell_width == 0) {
                csr_nnz = nnz;

                csr_ptr = backend::device_vector<Col>(q[0], n + 1,   &ptr[0]);
                csr_col = backend::device_vector<Col>(q[0], csr_nnz, &col[0]);
                csr_val = backend::device_vector<Val>(q[0], csr_nnz, &val[0]);

                return;
            }

            // Count nonzeros in CSR part of the matrix.
            for(int i = ell_width + 1; i <= max_width; ++i)
                csr_nnz += hist[i] * (i - ell_width);

            /* 3. Split the input matrix into ELL and CSR submatrices. */
            std::vector<Col> _ell_col(ell_pitch * ell_width, static_cast<Col>(-1));
            std::vector<Val> _ell_val(ell_pitch * ell_width);
            std::vector<Ptr> _csr_ptr;
            std::vector<Col> _csr_col;
            std::vector<Val> _csr_val;

            if (csr_nnz) {
                _csr_ptr.resize(n + 1);
                _csr_col.resize(csr_nnz);
                _csr_val.resize(csr_nnz);

                _csr_ptr[0] = 0;
                for(size_t i = 0; i < n; ++i) {
                    Ptr w = ptr[i+1] - ptr[i];
                    _csr_ptr[i+1] = _csr_ptr[i] + static_cast<Ptr>(w > ell_width ? w - ell_width : 0);
                }
            }

            for(size_t i = 0; i < n; ++i) {
                int w = 0;
                Ptr csr_head = csr_nnz ? _csr_ptr[i] : 0;
                for(Ptr j = ptr[i], e = ptr[i+1]; j < e; ++j, ++w) {
                    Col c = col[j];
                    Val v = val[j];

                    if (w < ell_width) {
                        _ell_col[i + w * ell_pitch] = c;
                        _ell_val[i + w * ell_pitch] = v;
                    } else {
                        _csr_col[csr_head] = c;
                        _csr_val[csr_head] = v;
                        ++csr_head;
                    }
                }
            }

            ell_col = backend::device_vector<Col>(q[0], ell_pitch * ell_width, _ell_col.data());
            ell_val = backend::device_vector<Val>(q[0], ell_pitch * ell_width, _ell_val.data());

            if (csr_nnz) {
                csr_ptr = backend::device_vector<Col>(q[0], n + 1,   _csr_ptr.data());
                csr_col = backend::device_vector<Col>(q[0], csr_nnz, _csr_col.data());
                csr_val = backend::device_vector<Val>(q[0], csr_nnz, _csr_val.data());
            }
        }

        // Dummy matrix; used internally to pass empty parameters to kernels.
        ell(const backend::command_queue &q)
            : q(q), n(0), m(0), nnz(0), ell_pitch(0), csr_nnz(0), ell_width(0)
        {}

        template <class Expr>
        friend
        typename std::enable_if<
            boost::proto::matches<
                typename boost::proto::result_of::as_expr<Expr>::type,
                vector_expr_grammar
            >::value,
            matrix_vector_product<ell, Expr>
        >::type
        operator*(const ell &A, const Expr &x) {
            return matrix_vector_product<ell, Expr>(A, x);
        }

        template <class Vector>
        static void terminal_preamble(const Vector &x, backend::source_generator &src,
            const backend::command_queue &q, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
        {
            detail::output_terminal_preamble tp(src, q, prm_name + "_x", state);
            boost::proto::eval(boost::proto::as_child(x), tp);
        }

        template <class Vector>
        static void local_terminal_init(const Vector &x, backend::source_generator &src,
            const backend::command_queue &q, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
        {
            typedef typename detail::return_type<Vector>::type x_type;
            typedef spmv_ops_impl<Val, x_type> spmv_ops;

            spmv_ops::decl_accum_var(src, prm_name + "_sum");
            src.open("{");

            // ELL part
            src.new_line() << "for(size_t j = 0; j < " << prm_name << "_ell_width; ++j)";
            src.open("{");
            src.new_line() << type_name<Col>() << " nnz_idx = idx + j * " << prm_name << "_ell_pitch;";
            src.new_line() << type_name<Col>() << " c = " << prm_name << "_ell_col[nnz_idx];";
            src.new_line() << "if (c != (" << type_name<Col>() << ")(-1))";
            src.open("{");

            src.new_line() << type_name<Col>() << " idx = c;";

            {
                detail::output_local_preamble init_x(src, q, prm_name + "_x", state);
                boost::proto::eval(boost::proto::as_child(x), init_x);

                backend::source_generator vec_value;
                detail::vector_expr_context expr_x(vec_value, q, prm_name + "_x", state);
                boost::proto::eval(boost::proto::as_child(x), expr_x);

                spmv_ops::append_product(src, prm_name + "_sum", prm_name + "_ell_val[nnz_idx]", vec_value.str());
            }

            src.close("} else break;");
            src.close("}");

            // CSR part
            src.new_line() << "if (" << prm_name << "_csr_ptr)";
            src.open("{");
            src.new_line() << type_name<Ptr>() << " csr_beg = " << prm_name << "_csr_ptr[idx];";
            src.new_line() << type_name<Ptr>() << " csr_end = " << prm_name << "_csr_ptr[idx+1];";
            src.new_line() << "for(" << type_name<Ptr>() << " j = csr_beg; j < csr_end; ++j)";
            src.open("{");

            src.new_line() << type_name<Col>() << " idx = " << prm_name << "_csr_col[j];";

            {
                detail::output_local_preamble init_x(src, q, prm_name + "_x", state);
                boost::proto::eval(boost::proto::as_child(x), init_x);

                backend::source_generator vec_value;
                detail::vector_expr_context expr_x(vec_value, q, prm_name + "_x", state);
                boost::proto::eval(boost::proto::as_child(x), expr_x);

                spmv_ops::append_product(src, prm_name + "_sum", prm_name + "_csr_val[j]", vec_value.str());
            }

            src.close("}");
            src.close("}");
            src.close("}");
        }

        template <class Vector>
        static void kernel_param_declaration(const Vector &x, backend::source_generator &src,
            const backend::command_queue &q, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
        {
            src.parameter< int >(prm_name + "_ell_width");
            src.parameter< size_t >(prm_name + "_ell_pitch");

            src.parameter< global_ptr<Col> >(prm_name + "_ell_col");
            src.parameter< global_ptr<Val> >(prm_name + "_ell_val");
            src.parameter< global_ptr<Ptr> >(prm_name + "_csr_ptr");
            src.parameter< global_ptr<Col> >(prm_name + "_csr_col");
            src.parameter< global_ptr<Val> >(prm_name + "_csr_val");

            detail::declare_expression_parameter decl_x(src, q, prm_name + "_x", state);
            detail::extract_terminals()(boost::proto::as_child(x), decl_x);
        }

        template <class Vector>
        static void partial_vector_expr(const Vector &, backend::source_generator &src,
            const backend::command_queue&, const std::string &prm_name,
            detail::kernel_generator_state_ptr)
        {
            src << prm_name << "_sum";
        }

        template <class Vector>
        void kernel_arg_setter(const Vector &x,
            backend::kernel &kernel, unsigned part, size_t index_offset,
            detail::kernel_generator_state_ptr state) const
        {
            kernel.push_arg(ell_width);
            kernel.push_arg(ell_pitch);
            if (ell_width) {
                kernel.push_arg(ell_col);
                kernel.push_arg(ell_val);
            } else {
                kernel.push_arg(static_cast<size_t>(0));
                kernel.push_arg(static_cast<size_t>(0));
            }
            if (csr_nnz) {
                kernel.push_arg(csr_ptr);
                kernel.push_arg(csr_col);
                kernel.push_arg(csr_val);
            } else {
                kernel.push_arg(static_cast<size_t>(0));
                kernel.push_arg(static_cast<size_t>(0));
                kernel.push_arg(static_cast<size_t>(0));
            }

            detail::set_expression_argument x_args(kernel, part, index_offset, state);
            detail::extract_terminals()( boost::proto::as_child(x), x_args);
        }

        template <class Vector>
        void expression_properties(const Vector &x,
            std::vector<backend::command_queue> &queue_list,
            std::vector<size_t> &partition,
            size_t &size) const
        {
            queue_list = std::vector<backend::command_queue>(1, q);
            partition  = std::vector<size_t>(2, 0);
            partition.back() = size = n;
        }

        size_t rows()     const { return n; }
        size_t cols()     const { return m; }
        size_t nonzeros() const { return nnz; }
    private:
        backend::command_queue q;

        size_t n, m, nnz, ell_pitch, csr_nnz;
        int ell_width;

        backend::device_vector<Col> ell_col;
        backend::device_vector<Val> ell_val;

        backend::device_vector<Ptr> csr_ptr;
        backend::device_vector<Col> csr_col;
        backend::device_vector<Val> csr_val;

        backend::kernel& csr2ell_kernel() const {
            using namespace vex::detail;
            static kernel_cache cache;

            auto kernel = cache.find(q);
            if (kernel == cache.end()) {
                backend::source_generator src(q);

                src.begin_kernel("convert_csr2ell");
                src.begin_kernel_parameters();
                src.template parameter<size_t>("n");
                src.template parameter<int>("ell_width");
                src.template parameter<size_t>("ell_pitch");
                src.template parameter< global_ptr<const ptr_type> >("ptr");
                src.template parameter< global_ptr<const col_type> >("col");
                src.template parameter< global_ptr<const val_type> >("val");
                src.template parameter< global_ptr<col_type> >("ell_col");
                src.template parameter< global_ptr<val_type> >("ell_val");
                src.template parameter< global_ptr<const ptr_type> >("csr_ptr");
                src.template parameter< global_ptr<col_type> >("csr_col");
                src.template parameter< global_ptr<val_type> >("csr_val");
                src.end_kernel_parameters();
                src.grid_stride_loop().open("{");

                src.new_line() << type_name<int>() << " w = 0;";
                src.new_line() << type_name<ptr_type>() << " csr_head = 0;";
                src.new_line() << "if (csr_ptr) csr_head = csr_ptr[idx];";
                src.new_line() << "for(" << type_name<ptr_type>() << " j = ptr[idx], e = ptr[idx+1]; j < e; ++j, ++w)";
                src.open("{");
                src.new_line() << type_name<col_type>() << " c = col[j];";
                src.new_line() << type_name<val_type>() << " v = val[j];";
                src.new_line() << "if (w < ell_width) {";
                src.new_line() << "  ell_col[idx + w * ell_pitch] = c;";
                src.new_line() << "  ell_val[idx + w * ell_pitch] = v;";
                src.new_line() << "} else {";
                src.new_line() << "  csr_col[csr_head] = c;";
                src.new_line() << "  csr_val[csr_head] = v;";
                src.new_line() << "  ++csr_head;";
                src.new_line() << "}";
                src.close("}");
                //src.new_line() << "for(; w < ell_width; ++w)";
                //src.new_line() << "  ell_col[idx + w * ell_pitch] = (" << type_name<col_type>() << ")(-1);";
                src.close("}");
                src.end_kernel();

                kernel = cache.insert(q, backend::kernel(q, src.str(), "convert_csr2ell"));
            }

            return kernel->second;
        }

        template <class PtrRange, class ColRange, class ValRange>
        void convert(
                const PtrRange &host_ptr,
                const ColRange &host_col,
                const ValRange &host_val
                )
        {
            size_t nnz = host_ptr[n];

            backend::device_vector<Ptr> Aptr(q, n+1, &host_ptr[0]);
            backend::device_vector<Col> Acol(q, nnz, &host_col[0]);
            backend::device_vector<Val> Aval(q, nnz, &host_val[0]);

            /* 1. Get optimal ELL widths for local and remote parts. */
            // Speed of ELL relative to CSR:
            const double ell_vs_csr = 3.0;

            // Find maximum widths for local and remote parts:
            std::vector<backend::command_queue> ctx(1, q);
            Reductor<int, MAX> max(ctx);

            vex::vector<Ptr> ptr(q, Aptr);

            VEX_FUNCTION(ptr_type, row_width, (size_t, i)(const ptr_type*, ptr),
                    return ptr[i+1] - ptr[i];
                    );

            int max_width = max(row_width(element_index(0, n), raw_pointer(ptr)));

            // Build width distribution histogram.
            vex::vector<int> hist(ctx, max_width + 1);
            hist = 0;
            eval(atomic_add(&permutation(row_width(element_index(0, n), raw_pointer(ptr)))(hist), 1));

            // Estimate optimal width for ELL part of the matrix,
            // count nonzeros in CSR part of the matrix
            ell_width = max_width;
            {
                auto h = hist.map(0);

                for(int i = 0, rows = static_cast<int>(n); i < max_width; ++i) {
                    rows -= h[i]; // Number of rows wider than i.
                    if (ell_vs_csr * rows < n) {
                        ell_width = i;
                        break;
                    }
                }

                for(int i = ell_width + 1; i <= max_width; ++i)
                    csr_nnz += h[i] * (i - ell_width);
            }

            if (ell_width == 0) {
                assert(csr_nnz == nnz);

                csr_ptr = Aptr;
                csr_col = Acol;
                csr_val = Aval;

                return;
            }

            if (csr_nnz) {
                VEX_FUNCTION(int, csr_width, (int, ell_width)(size_t, i)(const ptr_type*, ptr),
                        if (i == 0) return 0;
                        int w = ptr[i] - ptr[i-1];
                        return (w > ell_width) ? (w - ell_width) : 0;
                        );

                vex::vector<ptr_type> csr_w(ctx, n+1);

                csr_ptr = backend::device_vector<Ptr>(q, n + 1);
                csr_col = backend::device_vector<Col>(q, csr_nnz);
                csr_val = backend::device_vector<Val>(q, csr_nnz);

                csr_w = csr_width(ell_width, element_index(), raw_pointer(ptr));
                vector<ptr_type> csr_p(q, csr_ptr);
                inclusive_scan(csr_w, csr_p);
            }

            /* 3. Split the input matrix into ELL and CSR submatrices. */
            ell_col = backend::device_vector<Col>(q, ell_pitch * ell_width);
            ell_val = backend::device_vector<Val>(q, ell_pitch * ell_width);

            vex::vector<Col>(q, ell_col) = -1;

            auto &convert = csr2ell_kernel();

            convert.push_arg(n);
            convert.push_arg(ell_width);
            convert.push_arg(ell_pitch);
            convert.push_arg(Aptr);
            convert.push_arg(Acol);
            convert.push_arg(Aval);
            convert.push_arg(ell_col);
            convert.push_arg(ell_val);
            if (csr_nnz) {
                convert.push_arg(csr_ptr);
                convert.push_arg(csr_col);
                convert.push_arg(csr_val);
            } else {
                convert.push_arg(static_cast<size_t>(0));
                convert.push_arg(static_cast<size_t>(0));
                convert.push_arg(static_cast<size_t>(0));
            }
            convert(q);
        }

};

} // namespace sparse
} // namespace vex

#endif


// Copyright (c) 2016-2017, Philipp Sebastian Ruppel
//
// 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.
//
//    * Neither the name of the Universität Hamburg nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// 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.

#include "bio_ik/ik_gradient.hpp"

#include <Eigen/Core>          // For NumTraits
#include <bio_ik/ik_base.hpp>  // for IKSolver
#include <bio_ik/problem.hpp>  // for Problem, Problem::GoalInfo
#include <bio_ik/utils.hpp>    // for FNPROFILER
#include <cmath>               // for isfinite
#include <cstddef>             // for size_t
#include <ext/alloc_traits.h>  // for __alloc_traits<>::value_type
#include <kdl/frames.hpp>      // for Twist, Vector
#include <memory>
#include <optional>
#include <vector>  // for vector, allocator

#include "bio_ik/frame.hpp"       // for Frame, frameTwist
#include "bio_ik/robot_info.hpp"  // for RobotInfo

namespace bio_ik {

// simple gradient descent
template <int IF_STRUCK, size_t N_THREADS>
void IKGradientDescent<IF_STRUCK, N_THREADS>::initialize(
    const Problem& problem) {
  IKSolver::initialize(problem);
  solution_ = problem_.initial_guess;
  if (thread_index_ > 0)
    for (auto& vi : problem_.active_variables)
      solution_[vi] = random(modelInfo_.getMin(vi), modelInfo_.getMax(vi));
  best_solution_ = solution_;
  reset_ = false;
}

template <int IF_STRUCK, size_t N_THREADS>
void IKGradientDescent<IF_STRUCK, N_THREADS>::step() {
  // random reset if stuck
  if (reset_) {
    reset_ = false;
    for (auto& vi : problem_.active_variables)
      solution_[vi] = random(modelInfo_.getMin(vi), modelInfo_.getMax(vi));
  }

  // compute gradient_ direction
  temp_ = solution_;
  double jd = 0.0001;
  gradient_.resize(solution_.size(), 0);
  for (auto ivar : problem_.active_variables) {
    temp_[ivar] = solution_[ivar] - jd;
    double p1 = computeFitness(temp_);

    temp_[ivar] = solution_[ivar] + jd;
    double p3 = computeFitness(temp_);

    temp_[ivar] = solution_[ivar];

    gradient_[ivar] = p3 - p1;
  }

  // normalize gradient direction
  double sum = 0.0001;
  for (auto ivar : problem_.active_variables) sum += fabs(gradient_[ivar]);
  double f = 1.0 / sum * jd;
  for (auto ivar : problem_.active_variables) gradient_[ivar] *= f;

  // initialize line search
  temp_ = solution_;

  for (auto ivar : problem_.active_variables)
    temp_[ivar] = solution_[ivar] - gradient_[ivar];
  double p1 = computeFitness(temp_);

  // for(auto ivar : problem_.active_variables) temp_[ivar] = solution_[ivar];
  // double p2 = computeFitness(temp_);

  for (auto ivar : problem_.active_variables)
    temp_[ivar] = solution_[ivar] + gradient_[ivar];
  double p3 = computeFitness(temp_);

  double p2 = (p1 + p3) * 0.5;

  // linear step size estimation
  double cost_diff = (p3 - p1) * 0.5;
  double joint_diff = p2 / cost_diff;

  // in case cost_diff is 0
  if (!std::isfinite(joint_diff)) joint_diff = 0.0;

  // apply optimization step
  // (move along gradient direction by estimated step size)
  for (auto ivar : problem_.active_variables)
    temp_[ivar] =
        modelInfo_.clip(solution_[ivar] - gradient_[ivar] * joint_diff, ivar);

  if (IF_STRUCK == 'c') {
    // always accept solution and continue
    solution_ = temp_;
  } else {
    // has solution improved?
    if (computeFitness(temp_) < computeFitness(solution_)) {
      // solution improved -> accept solution
      solution_ = temp_;
    } else {
      if (IF_STRUCK == 'r') {
        // reset if stuck
        reset_ = true;
      }
    }
  }

  // update best solution
  if (computeFitness(solution_) < computeFitness(best_solution_))
    best_solution_ = solution_;
}

// pseudoinverse jacobian solver
template <class BASE>
void IKJacobianBase<BASE>::optimizeJacobian(std::vector<double>& solution) {
  FNPROFILER();

  Eigen::Index tip_count =
      static_cast<Eigen::Index>(problem_.tip_link_indices.size());
  tip_diffs_.resize(tip_count * 6);
  joint_diffs_.resize(
      static_cast<Eigen::Index>(problem_.active_variables.size()));

  // compute fk
  model_.applyConfiguration(solution);

  double translational_scale = 1;
  double rotational_scale = 1;

  // compute goal diffs
  tip_frames_temp_ = model_.getTipFrames();
  for (Eigen::Index itip = 0; itip < tip_count; ++itip) {
    auto twist = frameTwist(tip_frames_temp_[static_cast<size_t>(itip)],
                            tipObjectives_[static_cast<size_t>(itip)]);
    tip_diffs_(itip * 6 + 0) = twist.vel.x() * translational_scale;
    tip_diffs_(itip * 6 + 1) = twist.vel.y() * translational_scale;
    tip_diffs_(itip * 6 + 2) = twist.vel.z() * translational_scale;
    tip_diffs_(itip * 6 + 3) = twist.rot.x() * rotational_scale;
    tip_diffs_(itip * 6 + 4) = twist.rot.y() * rotational_scale;
    tip_diffs_(itip * 6 + 5) = twist.rot.z() * rotational_scale;
  }

  // compute jacobian
  {
    model_.computeJacobian(problem_.active_variables, jacobian_);
    Eigen::Index icol = 0;
    for (auto __attribute__((unused)) _ : problem_.active_variables) {
      for (Eigen::Index itip = 0; itip < tip_count; ++itip) {
        jacobian_(itip * 6 + 0, icol) *= translational_scale;
        jacobian_(itip * 6 + 1, icol) *= translational_scale;
        jacobian_(itip * 6 + 2, icol) *= translational_scale;
        jacobian_(itip * 6 + 3, icol) *= rotational_scale;
        jacobian_(itip * 6 + 4, icol) *= rotational_scale;
        jacobian_(itip * 6 + 5, icol) *= rotational_scale;
      }
      icol++;
    }
  }

  // get pseudoinverse and multiply
  joint_diffs_ = jacobian_.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV)
                     .solve(tip_diffs_);
  // joint_diffs_ = (jacobian_.transpose() *
  // jacobian_).ldlt().solve(jacobian_.transpose() * tip_diffs_);

  // apply joint deltas and clip
  {
    int icol = 0;
    for (auto ivar : problem_.active_variables) {
      auto v = solution[ivar] + joint_diffs_(icol);
      if (!std::isfinite(v)) continue;
      v = modelInfo_.clip(v, ivar);
      solution[ivar] = v;
      icol++;
    }
  }
}

// pseudoinverse jacobian only
template <size_t N_THREADS>
void IKJacobian<N_THREADS>::initialize(const Problem& problem) {
  IKJacobianBase<IKSolver>::initialize(problem);
  solution_ = problem_.initial_guess;
  if (thread_index_ > 0)
    for (auto& vi : problem_.active_variables)
      solution_[vi] = random(modelInfo_.getMin(vi), modelInfo_.getMax(vi));
}

std::optional<std::unique_ptr<IKSolver>> makeGradientDecentSolver(
    const IKParams& params) {
  const auto& name = params.ros_params.mode;
  if (name == "gd")
    return std::make_unique<IKGradientDescent<' ', 1>>(params);
  else if (name == "gd_2")
    return std::make_unique<IKGradientDescent<' ', 2>>(params);
  else if (name == "gd_4")
    return std::make_unique<IKGradientDescent<' ', 4>>(params);
  else if (name == "gd_8")
    return std::make_unique<IKGradientDescent<' ', 8>>(params);
  else if (name == "gd_r")
    return std::make_unique<IKGradientDescent<'r', 1>>(params);
  else if (name == "gd_r_2")
    return std::make_unique<IKGradientDescent<'r', 2>>(params);
  else if (name == "gd_r_4")
    return std::make_unique<IKGradientDescent<'r', 4>>(params);
  else if (name == "gd_r_8")
    return std::make_unique<IKGradientDescent<'r', 8>>(params);
  else if (name == "gd_c")
    return std::make_unique<IKGradientDescent<'c', 1>>(params);
  else if (name == "gd_c_2")
    return std::make_unique<IKGradientDescent<'c', 2>>(params);
  else if (name == "gd_c_4")
    return std::make_unique<IKGradientDescent<'c', 4>>(params);
  else if (name == "gd_c_8")
    return std::make_unique<IKGradientDescent<'c', 8>>(params);
  else if (name == "jac")
    return std::make_unique<IKJacobian<1>>(params);
  else if (name == "jac_2")
    return std::make_unique<IKJacobian<2>>(params);
  else if (name == "jac_4")
    return std::make_unique<IKJacobian<4>>(params);
  else if (name == "jac_8")
    return std::make_unique<IKJacobian<8>>(params);
  else
    return std::nullopt;
}

}  // namespace bio_ik


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  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.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  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 OWNER 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.
 */

/** \author Jia Pan */

#include <hpp/fcl/narrowphase/narrowphase.h>

#include <vector>
#include <boost/math/constants/constants.hpp>

#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/internal/intersect.h>
#include "details.h"

namespace hpp
{
namespace fcl
{
// Shape intersect algorithms based on:
// - built-in function: 0
// - GJK:               1
//
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | box        |  0  |   0    |    1    |   1  |    1     |   0   |      0     |    1     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   0    |    0    |   1  |    1     |   0   |      0     |    0     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|    1    |   1  |    1     |   0   |      0     |    1     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|/////////|   1  |    1     |   0   |      0     |    1     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|/////////|//////|    1     |   0   |      0     |    1     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|/////////|//////|//////////|   0   |      0     |    0     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|/////////|//////|//////////|///////|      0     |    0     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|    1     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+

template<>
bool GJKSolver::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
                                                      const Capsule &s2, const Transform3f& tf2,
                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}

template<>
bool GJKSolver::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
                                                      const Sphere &s2, const Transform3f& tf2,
                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
  if (normal) (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
                                                     const Sphere& s2, const Transform3f& tf2,
                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}

template<>
bool GJKSolver::shapeIntersect<Box, Sphere>(const Box   & s1, const Transform3f& tf1,
                                                  const Sphere& s2, const Transform3f& tf2,
                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL dist;
  Vec3f ps, pb, n;
  bool intersect = details::boxSphereDistance (s1, tf1, s2, tf2, dist, ps, pb, n);
  if (!intersect) return false;
  if (penetration_depth) *penetration_depth = dist;
  if (normal)            *normal = n;
  if (contact_points)    *contact_points = pb;
  return true;
}

template<>
bool GJKSolver::shapeIntersect<Sphere, Box>(const Sphere& s1, const Transform3f& tf1,
                                                  const Box   & s2, const Transform3f& tf2,
                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL dist;
  Vec3f ps, pb, n;
  bool intersect = details::boxSphereDistance (s2, tf2, s1, tf1, dist, ps, pb, n);
  if (!intersect) return false;
  if (penetration_depth) *penetration_depth = dist;
  if (normal)            *normal = -n;
  if (contact_points)    *contact_points = pb;
  return true;
}

template<>
bool GJKSolver::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
                                               const Box& s2, const Transform3f& tf2,
                                               Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}

template<>
bool GJKSolver::shapeIntersect<Sphere, Halfspace>
(const Sphere& s1, const Transform3f& tf1,
 const Halfspace& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
                                               p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
                                                        const Sphere& s2, const Transform3f& tf2,
                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, distance, p1,
                                               p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Box, Halfspace>
(const Box& s1, const Transform3f& tf1,
 const Halfspace& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
                                            p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Box>
(const Halfspace& s1, const Transform3f& tf1,
 const Box& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, distance, p1,
                                            p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Capsule, Halfspace>
(const Capsule& s1, const Transform3f& tf1,
 const Halfspace& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res =  details::capsuleHalfspaceIntersect
    (s1, tf1, s2, tf2, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Capsule>
(const Halfspace& s1, const Transform3f& tf1,
 const Capsule& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res =  details::capsuleHalfspaceIntersect
    (s2, tf2, s1, tf1, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Cylinder, Halfspace>
(const Cylinder& s1, const Transform3f& tf1,
 const Halfspace& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res =  details::cylinderHalfspaceIntersect
    (s1, tf1, s2, tf2, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Cylinder>
(const Halfspace& s1, const Transform3f& tf1,
 const Cylinder& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res =  details::cylinderHalfspaceIntersect
    (s2, tf2, s1, tf1, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Cone, Halfspace>
(const Cone& s1, const Transform3f& tf1,
 const Halfspace& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res =  details::coneHalfspaceIntersect
    (s1, tf1, s2, tf2, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Cone>
(const Halfspace& s1, const Transform3f& tf1,
 const Cone& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res =  details::coneHalfspaceIntersect
    (s2, tf2, s1, tf1, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
                                                           const Halfspace& s2, const Transform3f& tf2,
                                                           Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
  Halfspace s;
  Vec3f p, d;
  FCL_REAL depth;
  int ret;
  return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
}

template<>
bool GJKSolver::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
                                                       Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
  Plane pl;
  Vec3f p, d;
  FCL_REAL depth;
  int ret;
  return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
}

template<>
bool GJKSolver::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
                                                       const Plane& s2, const Transform3f& tf2,
                                                       Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
  Plane pl;
  Vec3f p, d;
  FCL_REAL depth;
  int ret;
  return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
}

template<>
bool GJKSolver::shapeIntersect<Sphere, Plane>
(const Sphere& s1, const Transform3f& tf1,
 const Plane& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1,
                                           p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Plane, Sphere>
(const Plane& s1, const Transform3f& tf1,
 const Sphere& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, distance, p1,
                                           p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Box, Plane>
(const Box& s1, const Transform3f& tf1,
 const Plane& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::boxPlaneIntersect
    (s1, tf1, s2, tf2, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Plane, Box>
(const Plane& s1, const Transform3f& tf1,
 const Box& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::boxPlaneIntersect
    (s2, tf2, s1, tf1, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Capsule, Plane>
(const Capsule& s1, const Transform3f& tf1,
 const Plane& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1,
                                            p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Plane, Capsule>
(const Plane& s1, const Transform3f& tf1,
 const Capsule& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, distance, p1,
                                            p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Cylinder, Plane>
(const Cylinder& s1, const Transform3f& tf1,
 const Plane& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::cylinderPlaneIntersect
    (s1, tf1, s2, tf2, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Plane, Cylinder>
(const Plane& s1, const Transform3f& tf1,
 const Cylinder& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::cylinderPlaneIntersect
    (s2, tf2, s1, tf1, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Cone, Plane>
(const Cone& s1, const Transform3f& tf1,
 const Plane& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::conePlaneIntersect
    (s1, tf1, s2, tf2, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Plane, Cone>
(const Plane& s1, const Transform3f& tf1,
 const Cone& s2, const Transform3f& tf2,
 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  FCL_REAL distance;
  Vec3f p1, p2;
  bool res = details::conePlaneIntersect
    (s2, tf2, s1, tf1, distance, p1, p2, *normal);
  *contact_points = p1;
  *penetration_depth = -distance;
  (*normal) *= -1.0;
  return res;
}

template<>
bool GJKSolver::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
  return details::planeIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}

template<>
bool GJKSolver::shapeTriangleInteraction
(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
 Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::sphereTriangleIntersect
    (s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3),
     distance, p1, p2, normal);
}

template<>
bool GJKSolver::shapeTriangleInteraction
(const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
 Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::halfspaceTriangleIntersect
    (s, tf1, P1, P2, P3, tf2, distance, p1, p2, normal);
}

template<>
bool GJKSolver::shapeTriangleInteraction
(const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
 Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::planeTriangleIntersect
    (s, tf1, P1, P2, P3, tf2, distance, p1, p2, normal);
}

// Shape distance algorithms not using built-in GJK algorithm
//
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | box        |     |   O    |         |      |          |       |            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   O    |    O    |      |          |       |            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|    O    |      |          |       |            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|/////////|      |          |       |            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|/////////|//////|          |       |            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|/////////|//////|//////////|       |            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|/////////|//////|//////////|///////|            |          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+

template<>
bool GJKSolver::shapeDistance<Sphere, Capsule>
(const Sphere& s1, const Transform3f& tf1,
 const Capsule& s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template<>
bool GJKSolver::shapeDistance<Capsule, Sphere>
(const Capsule& s1, const Transform3f& tf1,
 const Sphere& s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
}

template<>
bool GJKSolver::shapeDistance<Sphere, Cylinder>
(const Sphere& s1, const Transform3f& tf1,
 const Cylinder& s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::sphereCylinderDistance
    (s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template<>
bool GJKSolver::shapeDistance<Box, Sphere>
(const Box   & s1, const Transform3f& tf1,
 const Sphere& s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return !details::boxSphereDistance (s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template<>
bool GJKSolver::shapeDistance<Sphere, Box>
(const Sphere& s1, const Transform3f& tf1,
 const Box   & s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  bool collide = details::boxSphereDistance (s2, tf2, s1, tf1, dist, p2, p1, normal);
  normal *= -1;
  return !collide;
}

template<>
bool GJKSolver::shapeDistance<Cylinder, Sphere>
(const Cylinder& s1, const Transform3f& tf1,
 const Sphere& s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::sphereCylinderDistance
    (s2, tf2, s1, tf1, dist, p2, p1, normal);
}

template<>
bool GJKSolver::shapeDistance<Sphere, Sphere>
(const Sphere& s1, const Transform3f& tf1,
 const Sphere& s2, const Transform3f& tf2,
 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
{
  return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template<>
bool GJKSolver::shapeDistance<Capsule, Capsule>
(const Capsule& /*s1*/, const Transform3f& /*tf1*/,
 const Capsule& /*s2*/, const Transform3f& /*tf2*/,
 FCL_REAL& /*dist*/, Vec3f& /*p1*/, Vec3f& /*p2*/, Vec3f& /*normal*/) const
{
  abort ();
}

  template<>
    bool GJKSolver::shapeDistance<TriangleP, TriangleP>
    (const TriangleP& s1, const Transform3f& tf1,
     const TriangleP& s2, const Transform3f& tf2,
     FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
  {
    const TriangleP
      t1 (tf1.transform(s1.a), tf1.transform(s1.b), tf1.transform(s1.c)),
      t2 (tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));

    Vec3f guess;
    if(enable_cached_guess) guess = cached_guess;
    else guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
    bool enable_penetration (true);

    details::MinkowskiDiff shape;
    shape.set (&t1, &t2);

    details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
    details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
    if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();

    details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);

    if((gjk_status == details::GJK::Valid) ||
       (gjk_status == details::GJK::Failed))
    {
      // TODO On degenerated case, the closest point may be wrong
      // (i.e. an object face normal is colinear to gjk.ray
      // assert (dist == (w0 - w1).norm());
      dist = gjk.distance;

      return true;
    }
    else if (gjk_status == details::GJK::Inside)
    {
      if (enable_penetration) {
        FCL_REAL penetrationDepth = details::computePenetration
          (t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
        dist = -penetrationDepth;
        assert (dist <= 1e-6);
        // GJK says Inside when below GJK.tolerance. So non intersecting
        // triangle may trigger "Inside" and have no penetration.
        return penetrationDepth < 0;
      }
      dist = 0;
      return false;
    }
    assert (false && "should not reach this point");
    return false;
  }
} // fcl

} // namespace hpp