# Copyright 2014, 2020, Florent Lamiraux, Guilhem Saurel, CNRS
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
#   list of conditions and the following disclaimer.
#
# 1. Redistributions in binary form must reproduce the above copyright notice,
#   this list of conditions and the following disclaimer in the documentation
#   and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

cmake_minimum_required(VERSION 3.22)

set(PROJECT_NAME hpp-constraints)
set(PROJECT_DESCRIPTION
    "Definition of basic geometric constraints for motion planning"
)

set(CXX_DISABLE_WERROR TRUE)
set(PROJECT_USE_CMAKE_EXPORT TRUE)

# Check if the submodule cmake have been initialized
set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake")
    message(STATUS "JRL cmakemodules found in 'cmake/' git submodule")
else()
    find_package(jrl-cmakemodules QUIET CONFIG)
    if(jrl-cmakemodules_FOUND)
        get_property(
            JRL_CMAKE_MODULES
            TARGET jrl-cmakemodules::jrl-cmakemodules
            PROPERTY INTERFACE_INCLUDE_DIRECTORIES
        )
        message(
            STATUS
            "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}"
        )
    else()
        message(STATUS "JRL cmakemodules not found. Let's fetch it.")
        include(FetchContent)
        FetchContent_Declare(
            "jrl-cmakemodules"
            GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git"
        )
        FetchContent_MakeAvailable("jrl-cmakemodules")
        FetchContent_GetProperties(
            "jrl-cmakemodules"
            SOURCE_DIR JRL_CMAKE_MODULES
        )
    endif()
endif()

include("${JRL_CMAKE_MODULES}/hpp.cmake")
include("${JRL_CMAKE_MODULES}/boost.cmake")

# Activate hpp-util logging if requested
set(HPP_CONSTRAINTS_NUMERIC_DEBUG
    ${HPP_DEBUG}
    CACHE BOOL
    "trigger hpp-util debug output"
)
if(HPP_CONSTRAINTS_NUMERIC_DEBUG)
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_CONSTRAINTS_NUMERIC_DEBUG")
endif()

option(CHECK_JACOBIANS "Check jacobians against finite difference." OFF)
if(CHECK_JACOBIANS)
    add_definitions(-DCHECK_JACOBIANS)
endif(CHECK_JACOBIANS)

compute_project_args(PROJECT_ARGS LANGUAGES CXX)
project(${PROJECT_NAME} ${PROJECT_ARGS})

add_project_dependency(hpp-pinocchio REQUIRED)
add_project_dependency(qpOASES QUIET FIND_EXTERNAL qpOASES)

if(qpOASES_FOUND)
    message(STATUS "Building with qpOASES")
else()
    message(STATUS "Building without qpOASES")
endif()

set(${PROJECT_NAME}_HEADERS
    include/hpp/constraints/differentiable-function.hh
    include/hpp/constraints/differentiable-function-set.hh
    include/hpp/constraints/active-set-differentiable-function.hh
    include/hpp/constraints/affine-function.hh
    include/hpp/constraints/comparison-types.hh
    include/hpp/constraints/distance-between-bodies.hh
    include/hpp/constraints/fwd.hh
    include/hpp/constraints/svd.hh
    include/hpp/constraints/tools.hh
    include/hpp/constraints/matrix-view.hh
    include/hpp/constraints/generic-transformation.hh
    include/hpp/constraints/macros.hh
    include/hpp/constraints/manipulability.hh
    include/hpp/constraints/min-manipulability.hh
    include/hpp/constraints/convex-shape.hh
    include/hpp/constraints/convex-shape-contact.hh
    include/hpp/constraints/static-stability.hh
    include/hpp/constraints/symbolic-calculus.hh
    include/hpp/constraints/symbolic-function.hh
    include/hpp/constraints/relative-com.hh
    include/hpp/constraints/com-between-feet.hh
    include/hpp/constraints/configuration-constraint.hh
    include/hpp/constraints/explicit-constraint-set.hh
    include/hpp/constraints/explicit.hh
    include/hpp/constraints/explicit/implicit-function.hh
    include/hpp/constraints/explicit/relative-pose.hh
    include/hpp/constraints/explicit/convex-shape-contact.hh
    include/hpp/constraints/explicit/relative-transformation.hh
    include/hpp/constraints/implicit.hh
    include/hpp/constraints/implicit-constraint-set.hh
    include/hpp/constraints/locked-joint.hh
    include/hpp/constraints/serialization.hh
    include/hpp/constraints/solver/hierarchical-iterative.hh
    include/hpp/constraints/solver/by-substitution.hh
    include/hpp/constraints/function/of-parameter-subset.hh
    include/hpp/constraints/function/difference.hh
    include/hpp/constraints/solver/impl/by-substitution.hh
    include/hpp/constraints/solver/impl/hierarchical-iterative.hh
    include/hpp/constraints/impl/matrix-view.hh
    include/hpp/constraints/impl/matrix-view-operation.hh
)

set(${PROJECT_NAME}_SOURCES
    src/affine-function.cc
    src/differentiable-function.cc
    src/differentiable-function-set.cc
    src/generic-transformation.cc
    src/relative-com.cc
    src/com-between-feet.cc
    src/distance-between-bodies.cc
    src/distance-between-points-in-bodies.cc
    src/configuration-constraint.cc
    src/convex-shape-contact.cc
    src/extracted-function.hh
    src/matrix-view.cc
    src/manipulability.cc
    src/min-manipulability.cc
    src/static-stability.cc
    src/explicit-constraint-set.cc
    src/implicit.cc
    src/explicit.cc
    src/explicit/convex-shape-contact.cc
    src/explicit/implicit-function.cc
    src/explicit/input-configurations.hh
    src/explicit/relative-transformation.cc
    src/explicit/relative-pose.cc
    src/function/of-parameter-subset.cc
    src/function/difference.cc
    src/locked-joint.cc
    src/solver/by-substitution.cc
    src/solver/hierarchical-iterative.cc
)

if(qpOASES_FOUND)
    set(${PROJECT_NAME}_HEADERS
        ${${PROJECT_NAME}_HEADERS}
        include/hpp/constraints/qp-static-stability.hh
    )
    set(${PROJECT_NAME}_SOURCES
        ${${PROJECT_NAME}_SOURCES}
        src/qp-static-stability.cc
    )
endif()

add_library(
    ${PROJECT_NAME}
    SHARED
    ${${PROJECT_NAME}_SOURCES}
    ${${PROJECT_NAME}_HEADERS}
)
target_include_directories(
    ${PROJECT_NAME}
    PUBLIC $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
)
target_link_libraries(${PROJECT_NAME} PUBLIC hpp-pinocchio::hpp-pinocchio)

if(qpOASES_FOUND)
    target_link_libraries(${PROJECT_NAME} PUBLIC qpOASES::qpOASES)
    target_compile_definitions(
        ${PROJECT_NAME}
        PUBLIC HPP_CONSTRAINTS_USE_QPOASES
    )
endif()

install(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)

if(BUILD_TESTING)
    find_package(Boost REQUIRED COMPONENTS unit_test_framework)
    find_package(example-robot-data REQUIRED)
    add_subdirectory(tests)
endif()
