# Generates ACADO MPC solver code from a given code generator
# Generated files are compiled into a library with target GENERATED_LIB
# :option ENABLE_WARNINGS: If active, enable warnings on autogenerated and external code
# :param GENERATOR: Source file for an ACADO code generator
# :type GENERATOR: A string/path to a c++ file
# :param OUTPUT_PATH: The directory the code generator writes to, which is set via the
#                     exportCode() function call at the end of code generation
# :type OUTPUT_PATH: A string/directory name
# :param NAME: Name of the code generator target, defaults to "code_generator". Intended
#              for when a user is trying to generate multiple solvers in the same project
# :type NAME: A string
# :param QP_SOLVER: The QP solver backend (chosen by code generator)
# :type QP_SOLVER: A string, One of [QPOASES,]
# :param LIB_NAME: Name of the generated library
# :type LIB_NAME: String
# :param GENERATED_PATH: A variable which holds the resulting path of generated files
# :param SNIPPET_PATHS: list of include directories for code generator
# :type SNIPPET_PATHS: List of strings
macro(ament_acado_generate GENERATED_PATH)
  # Input handling
  set(_ARG_NAMES "GENERATOR;OUTPUT_PATH;NAME;LIB_NAME;QP_SOLVER")
  set(_ARG_OPTIONS "ENABLE_WARNINGS")
  set(_ARG_ARGN "SNIPPET_PATHS")

  cmake_parse_arguments(ARG
    "${_ARG_OPTIONS}" # Options
    "${_ARG_NAMES}" # single value keywords
    "${_ARG_ARGN}" # ARGN / multi_value_keywords
    ${ARGN})

  if(ARG_UNPARSED_ARGUMENTS)
    message(FATAL_ERROR
      "ament_acado_generate called with unused arguments: " ${ARG_UNPARSED_ARGUMENTS})
  endif()

  if(NOT ARG_GENERATOR)
    message(FATAL_ERROR "Argument GENERATOR not provided to ament_acado_generate()!")
  endif()

  if(NOT ARG_OUTPUT_PATH)
    message(FATAL_ERROR "Argument OUTPUT_PATH not provided to ament_acado_generate()!")
  endif()

  if(NOT ARG_LIB_NAME)
    message(FATAL_ERROR "Argument LIB_NAME not provided to ament_acado_generate()!")
  endif()

  if(NOT ARG_QP_SOLVER)
    set(ARG_QP_SOLVER "QPOASES")
    message("Argument QP_SOLVER not set, using default of ${ARG_QP_SOLVER}")
  endif()
  string(TOUPPER ${ARG_QP_SOLVER} QP_SOLVER)

  if(NOT ARG_NAME)
    set(ARG_NAME "code_generator")
    message("Argument NAME not set, using default of ${ARG_NAME}")
  endif()

  # Build code generator
  add_executable(${ARG_NAME}
    ${ARG_GENERATOR})
  ament_target_dependencies(${ARG_NAME} "ament_acado")
  set_property(TARGET ${ARG_NAME} PROPERTY CXX_CLANG_TIDY "")
  target_compile_options(${ARG_NAME} PRIVATE -w)
  target_include_directories(${ARG_NAME} PRIVATE ${ARG_SNIPPET_PATHS})

  # Populate output variables
  set(_GENERATED_PATH "${CMAKE_BINARY_DIR}/${ARG_OUTPUT_PATH}")
  # Run code generator
  set(GENERATED_DIRECTORY single_track_dynamics)
  add_custom_command(
    DEPENDS ${ARG_NAME}
    COMMAND ${ARG_NAME}
    OUTPUT
      # Common
      ${_GENERATED_PATH}/acado_common.h
      ${_GENERATED_PATH}/acado_integrator.c
      ${_GENERATED_PATH}/acado_solver.c
      # Interface specific
      ${_GENERATED_PATH}/acado_qpoases_interface.hpp
      ${_GENERATED_PATH}/acado_qpoases_interface.cpp
      # Interface specific
      ${_GENERATED_PATH}/acado_qpoases3_interface.h
      ${_GENERATED_PATH}/acado_qpoases3_interface.c
      # Interface specific
      ${_GENERATED_PATH}/acado_hpmpc_interface.c
  COMMENT "Running code generator..")
  add_custom_target(_GENERATE_FILES_TARGET DEPENDS ${_GENERATED_PATH}/acado_common.h)
  # Check that generated files exist
  add_custom_command(
    TARGET _GENERATE_FILES_TARGET POST_BUILD
    COMMAND ${CMAKE_COMMAND}
      -D_OUTPUT_PATH=${ARG_OUTPUT_PATH}
      -P ${ament_acado_DIR}/ament_acado_check_generated.cmake
    COMMENT "Checking code generator output...")

  # Get files related to embedded sources
  set(EMBEDDED_SRCS "")
  set(EMBEDDED_INCLUDES "")
  set(GENERATED_FILES
      ${_GENERATED_PATH}/acado_common.h
      ${_GENERATED_PATH}/acado_integrator.c
      ${_GENERATED_PATH}/acado_solver.c)
  if(${QP_SOLVER} STREQUAL "QPOASES")
    message("QP solver ${QP_SOLVER} is not recommended!")
    set(QPOASES_DIR ${ament_acado_DIR}/../../acado/external_packages/qpoases)
    set(EMBEDDED_SRCS
          ${QPOASES_DIR}/SRC/Bounds.cpp
          ${QPOASES_DIR}/SRC/CyclingManager.cpp
          ${QPOASES_DIR}/SRC/MessageHandling.cpp
          ${QPOASES_DIR}/SRC/QProblem.cpp
          ${QPOASES_DIR}/SRC/Utils.cpp
          ${QPOASES_DIR}/SRC/Constraints.cpp
          ${QPOASES_DIR}/SRC/Indexlist.cpp
          ${QPOASES_DIR}/SRC/QProblemB.cpp
          ${QPOASES_DIR}/SRC/SubjectTo.cpp
          ${QPOASES_DIR}/SRC/EXTRAS/SolutionAnalysis.cpp)
    set(EMBEDDED_INCLUDES
      "${QPOASES_DIR}/INCLUDE/;"
      "${QPOASES_DIR}/SRC/;"
      "${QPOASES_DIR}/")
    set(GENERATED_FILES ${GENERATED_FILES}
      ${_GENERATED_PATH}/acado_qpoases_interface.hpp
      ${_GENERATED_PATH}/acado_qpoases_interface.cpp)
  elseif(${QP_SOLVER} STREQUAL "QPOASES3")
    set(QPOASES3_DIR ${ament_acado_DIR}/../../acado/external_packages/qpoases3)
    set(EMBEDDED_SRCS
          ${QPOASES3_DIR}/src/Bounds.c
          ${QPOASES3_DIR}/src/Constraints.c
          ${QPOASES3_DIR}/src/Flipper.c
          ${QPOASES3_DIR}/src/Indexlist.c
          ${QPOASES3_DIR}/src/Matrices.c
          ${QPOASES3_DIR}/src/MessageHandling.c
          ${QPOASES3_DIR}/src/Options.c
          ${QPOASES3_DIR}/src/OQPinterface.c
          ${QPOASES3_DIR}/src/QProblemB.c
          ${QPOASES3_DIR}/src/QProblem.c
          ${QPOASES3_DIR}/src/Utils.c)
    set(EMBEDDED_INCLUDES
      "${QPOASES3_DIR}/include/;"
      "${QPOASES3_DIR}/src/;"
      "${QPOASES3_DIR}/")
    set(GENERATED_FILES ${GENERATED_FILES}
      ${_GENERATED_PATH}/acado_qpoases3_interface.h
      ${_GENERATED_PATH}/acado_qpoases3_interface.c)
  elseif(${QP_SOLVER} STREQUAL "HPMPC")
    set(HPMPC_DIR ${ament_acado_DIR}/../../../)
    set(EMBEDDED_INCLUDES ${HPMPC_DIR}/include)
    set(GENERATED_FILES ${GENERATED_FILES}
      ${_GENERATED_PATH}/acado_hpmpc_interface.c)
  else()
    message(FATAL_ERROR "Unsupported QP solver backend: ${QP_SOLVER}")
  endif()

  # Build autogenerated code as a shared library
  add_library(${ARG_LIB_NAME} SHARED
    ${GENERATED_FILES}
    ${EMBEDDED_SRCS})
  target_include_directories(${ARG_LIB_NAME} PRIVATE ${EMBEDDED_INCLUDES} ${_GENERATED_PATH})
  add_dependencies(${ARG_LIB_NAME} ${ARG_NAME} _GENERATE_FILES_TARGET)
  if(NOT ARG_ENABLE_WARNINGS)
    # Don't generate warnings: not helpful (because autogenerated)
    set_property(TARGET ${ARG_LIB_NAME} PROPERTY CXX_CLANG_TIDY "")
    target_compile_options(${ARG_LIB_NAME} PRIVATE -w)
  endif()
  if(${QP_SOLVER} STREQUAL "HPMPC")
    target_link_libraries(${ARG_LIB_NAME} ${ament_acado_DIR}/../../../lib/libhpmpc.so)
  endif()

  # Install and export
  install(TARGETS ${ARG_LIB_NAME}
    LIBRARY DESTINATION lib
    ARCHIVE DESTINATION lib
    RUNTIME DESTINATION bin)
  ament_export_libraries(${ARG_LIB_NAME})
  if(NOT EXISTS ${_GENERATED_PATH})
    file(MAKE_DIRECTORY ${_GENERATED_PATH})
  endif()
  ament_export_include_directories(${_GENERATED_PATH})
  # Populate output variables
  set(GENERATED_PATH ${_GENERATED_PATH})
endmacro()
