/* Copyright 2018-2019 The MathWorks, Inc. */

/**
 * @file
 * External C-API interfaces for Reeds-Shepp motion primitive calculations.
 * To fully support code generation, note that this file needs to be fully
 * compliant with the C89/C90 (ANSI) standard.
 */

#ifndef AUTONOMOUSCODEGEN_REEDS_SHEPP_API_H_
#define AUTONOMOUSCODEGEN_REEDS_SHEPP_API_H_

#ifdef BUILDING_LIBMWAUTONOMOUSCODEGEN
#include "autonomouscodegen/autonomouscodegen_util.hpp"
#else
/* To deal with the fact that PackNGo has no include file hierarchy during test */
#include "autonomouscodegen_util.hpp"
#endif


/**
 * @brief Compute segments along ReedsShepp curve between two specified poses
 *
 * @param[in] startPose Starting pose, specified as real64_T precision
 *    matrices. Either one of @p startPose or @p goalPose can be M-by-3, while the other
 *    must be 1-by-3 or both should be M-by-3.
 * @param[in] numStartPoses Number of rows in starting pose @p startPose
 * @param[in] goalPose Ending pose. See notes for starting pose @p startPose.
 * @param[in] numGoalPoses Number of rows in ending pose @p goalPose
 * @param[in] turningRadius Turning radius (in radians)
 * @param[in] forwardCost Cost of forward motion.
 * @param[in] reverseCost Cost of reverse motion.
 * @param[in] allPathTypes Boolean array indicating if path types are enabled
 *    (true) or disabled (false)
 * @param[in] numDisabledPathTypes Number of path types that are disabled.
 * @param[in] numPaths Number of total path types minus @p numDisabledPathTypes
 * @param[in] isOptimal TRUE if user only wants optimal path, FALSE otherwise.
 * @param[in] nlhs Number of outputs requested. The user can request 1 - 3 outputs
 * @param[out] distance Distance between @p startPose and @p goalPose, along ReedsShepp curve.
 *    If @p startPose and @p goalPose are both 1-by-3 & paths is optimal, @p distance is a
 *    scalar. If @p startPose or @p goalPose is M-by-3, @p distance is an N-by-M
 *    matrix. where N = (44 - @p numDisabledPathTypes)--> (all),
 *    1-->(optimal)
 * @param[out] segmentLen Segments lengths of the path between @p startPose and @p goalPose.
 *    If @p startPose or @p goalPose is M-by-3, @p segmentLen is an 5-by-N-by-M matrix.
 *    where N = (44 - @p numDisabledPathTypes)--> (all),
 *    1-->(optimal)
 * @param[out] segmentType Segments types of the path between @p startPose and @p goalPose, size is
 * same as
 *    @p segmentLen output.
 */
EXTERN_C AUTONOMOUS_CODEGEN_API void autonomousReedsSheppSegmentsCodegen_real64(
    const real64_T* startPose,
    const uint32_T numStartPoses,
    const real64_T* goalPose,
    const uint32_T numGoalPoses,
    const real64_T turningRadius,
    const real64_T forwardCost,
    const real64_T reverseCost,
    const boolean_T* allPathTypes,
    const uint32_T numDisabledPathTypes,
    const uint32_T numPaths,
    const boolean_T isOptimal,
    const uint32_T nlhs,
    real64_T* distance,
    real64_T* segmentLen,
    real64_T* segmentType);

/**
 * @brief Interpolate along shortest Reeds-Shepp curve between states
 *
 * @param[in] startPose Starting pose, specified as real64_T precision 3-vector.
 * @param[in] goalPose Ending pose, specified as real64_T precision 3-vector.
 * @param[in] maxDistance Maximum distance to interpolate to, typically the range
 *   of the planner, specified as a real, real64_T, scalar.
 * @param[in] numSteps Number of interpolation steps.
 * @param[in] turningRadius Radius used to compute left and right turns along
 *   Reeds-Shepp curve (in radians)
 * @param[in] reverseCost Cost of reverse motion.
 * @param[out] interpPoses Interpolated poses along the ReedsShepp curve, returned as a
 *   (4 + @p numSteps)-by-3 real64_T-precision matrix. In
 *   addition to the @p numSteps queries, 4 transition poses are
 *   returned. The transition poses saturate at the ending
 *   pose determined by @p maxDistance. These are the first 4
 *   poses returned.
 */
EXTERN_C AUTONOMOUS_CODEGEN_API void autonomousReedsSheppInterpolateCodegen_real64(
    const real64_T* startPose,
    const real64_T* goalPose,
    const real64_T maxDistance,
    const uint32_T numSteps,
    const real64_T turningRadius,
    const real64_T reverseCost,
    real64_T* interpPoses);

/**
 * @brief Interpolate along any Reeds-Shepp curve between states
 *
 * @param[in] startPose Starting pose, specified as real64_T precision 3-vector.
 * @param[in] goalPose Ending pose, specified as real64_T precision 3-vector.
 * @param[in] samples Set of lengths at which user want to interpolate the
 *   poses of the planner, specified as a real, vector, scalar.
 * @param[in] numSamples Number of samples, specified as a real, double and scalar.
 * @param[in] turningRadius Turning radius (in radians)
 * @param[in] segmentLengths Length of the Reeds-Shepp's 5 segments (in meters),
 *   specified as real64_T precision 5-vector.
 * @param[in] segmentDirections Direction of the Reeds-Shepp's 5 segments, specified as
 *   int32_T precision 5-vector.
 * @param[in] segmentTypes Types of the Reeds-Shepp 5 segments, specified as
 *   real64_T precision 5-vector.
 * @param[out] interpPoses Interpolated poses along the Reeds-Shepp curve,
 *   returned as a (@p numSteps)-by-3 double-precision matrix.
 * @param[out] interpDirections Interpolated poses's directions along the
 *   Reeds-Shepp curve, returned as a (@p numSteps)-by-1 double-precision vector.
 */
EXTERN_C AUTONOMOUS_CODEGEN_API void autonomousReedsSheppInterpolateSegmentsCodegen_real64(
    const real64_T* startPose,
    const real64_T* goalPose,
    const real64_T* samples,
    const uint32_T numSamples,
    const real64_T turningRadius,
    const real64_T* segmentLengths,
    const int32_T*  segmentDirections,
    const uint32_T* segmentTypes,
    real64_T* interpPoses,
    real64_T* interpDirections);


/**
 * @brief Compute distance along ReedsShepp curve between two specified poses
 *
 * @param[in] startPose Starting pose, specified as real64_T precision
 *    matrices. Either one of @p startPose or @p goalPose can be M-by-3, while the other
 *    must be 1-by-3 or both should be M-by-3.
 * @param[in] numStartPoses Number of rows in starting pose @p startPose
 * @param[in] goalPose Ending pose. See notes for starting pose @p startPose.
 * @param[in] numGoalPoses Number of rows in ending pose @p goalPose
 * @param[in] turningRadius Turning radius (in radians)
 * @param[in] reverseCost Cost of reverse motion.
 * @param[out] distance Distance between @p startPose and @p goalPose, along ReedsShepp curve.
 *   If @p startPose and @p goalPose are both 1-by-3, @p distance is a scalar.
 *   If @p startPose or @p goalPose is M-by-3, @p distance is an M-by-1 column vector.
 */
EXTERN_C AUTONOMOUS_CODEGEN_API void autonomousReedsSheppDistanceCodegen_real64(
    const real64_T* startPose,
    const uint32_T numStartPoses,
    const real64_T* goalPose,
    const uint32_T numGoalPoses,
    const real64_T turningRadius,
    const real64_T reverseCost,
    real64_T* distance);

#endif /* AUTONOMOUSCODEGEN_REEDS_SHEPP_API_H_ */