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

/**
 * @file
 * Functions used for Dubins motion primitive calculations.
 * To fully support code generation, note that this file needs to be fully
 * compliant with the C++98 standard.
 */

#ifndef AUTONOMOUSCODEGEN_DUBINS_FUNCTORS_HPP_
#define AUTONOMOUSCODEGEN_DUBINS_FUNCTORS_HPP_

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

namespace autonomous {
namespace dubins {

/**
 * @brief Add extracted (path cost, path segments, path types) to output vector.
 */
template <typename T>
void addCostSegLensSegTypes(T* distances,
                            T* segmentLen,
                            T* segmentType,
                            std::vector<DubinsPath> path,
                            const uint32_T m,
                            const uint32_T numPaths,
                            const T turningRadius,
                            const uint32_T numInputs) {
    // Add cost
    for (uint32_T n = 0; n < numPaths; ++n) {
        distances[n + m * numPaths] = path[n].length() * turningRadius;
    }

    // Add segment lengths
    if (numInputs == 2) {
        for (uint32_T n = 0; n < numPaths; ++n) {
            const T* segLens = path[n].getSegmentLengths();
            segmentLen[0 + n * 3 + m * 3 * numPaths] = segLens[0] * turningRadius;
            segmentLen[1 + n * 3 + m * 3 * numPaths] = segLens[1] * turningRadius;
            segmentLen[2 + n * 3 + m * 3 * numPaths] = segLens[2] * turningRadius;
        }
    }

    // Add segment lengths & segment type
    if (numInputs == 3) {
        for (uint32_T n = 0; n < numPaths; ++n) {
            const T* segLens = path[n].getSegmentLengths();
            for (uint32_T k = 0; k < static_cast<uint32_T>(3); ++k) {
                segmentLen[k + n * 3 + m * 3 * numPaths] = segLens[k] * turningRadius;
                segmentType[k + n * 3 + m * 3 * numPaths] =
                    static_cast<int32_T>(pathToSegment[path[n].getPathType()][k]);
            }
        }
    }
}

/**
 * @brief Functor for evaluating Dubins motion primitive segments
 *
 * This function is used to allow both parallel (with TBB) and serial execution.
 * @see autonomousDubinsSegmentsCodegen_real64, autonomousDubinsSegmentsCodegen_tbb_real64
 */
template <typename T>
class DubinsSegmentsFunctor {
  private:
    const T* m_startPose;
    const uint32_T m_maxNumPoses;
    const uint32_T m_numStartPoses;
    const uint32_T m_numGoalPoses;
    const T* m_goalPose;
    const T m_turningRadius;
    const boolean_T* m_allPathTypes;
    const uint32_T m_numPaths;
    const boolean_T m_isOptimal;
    const uint32_T m_nlhs;
    T* m_distance;
    T* m_segmentLen;
    T* m_segmentType;

  public:
    DubinsSegmentsFunctor(const T* startPose,
                          const uint32_T numStartPoses,
                          const T* goalPose,
                          const uint32_T numGoalPoses,
                          const T turningRadius,
                          const boolean_T* allPathTypes,
                          const boolean_T isOptimal,
                          const uint32_T nlhs,
                          T* distance,
                          T* segmentLen,
                          T* segmentType)
        : m_startPose(startPose)
        , m_maxNumPoses(numStartPoses > numGoalPoses ? numStartPoses : numGoalPoses)
        , m_numStartPoses(numStartPoses)
        , m_numGoalPoses(numGoalPoses)
        , m_goalPose(goalPose)
        , m_turningRadius(turningRadius)
        , m_allPathTypes(allPathTypes)
        , m_numPaths(isOptimal ? 1 : static_cast<const uint32_T>(TotalNumPaths))
        , m_isOptimal(isOptimal)
        , m_nlhs(nlhs)
        , m_distance(distance)
        , m_segmentLen(segmentLen)
        , m_segmentType(segmentType)

    {
    }

    /**
     * Functor to calculate the Dubins segment information for a
     * given range of start and goal points.
     *
     * This functor can be executed as-is (for serial execution) or
     * be part of parallel execution, e.g. through parallel_for
     *
     * @param range The range information for how many start and
     * goal locations should be processed.
     */
    void operator()(const autonomous::ParallelRange<uint32_T>& range) const {
        // The conditional statements are constant, so should be
        // compiler-optimized.
        if (m_numStartPoses >= m_numGoalPoses && m_numGoalPoses == 1) {
            // Single startPose, single goalPose
            // Multiple startPoses, single goalPose
            T a[3];
            for (uint32_T i = range.begin(); i < range.end(); ++i) {
                a[0] = m_startPose[i];
                a[1] = m_startPose[i + m_maxNumPoses];
                a[2] = m_startPose[i + 2 * m_maxNumPoses];

                std::vector<DubinsPath> path =
                    computeAllDubinsPaths(&a[0], m_goalPose, m_turningRadius, m_isOptimal,
                                          &m_allPathTypes[0]);

                // Add cost, segment lengths & segment types
                addCostSegLensSegTypes(m_distance, m_segmentLen, m_segmentType, path, i, m_numPaths,
                                       m_turningRadius, m_nlhs);
            }

        } else if (m_numStartPoses < m_numGoalPoses && m_numStartPoses == 1) {
            // Single startPose, multiple goalPoses
            T b[3];
            for (uint32_T i = range.begin(); i < range.end(); ++i) {
                b[0] = m_goalPose[i];
                b[1] = m_goalPose[i + m_maxNumPoses];
                b[2] = m_goalPose[i + 2 * m_maxNumPoses];

                std::vector<DubinsPath> path =
                    computeAllDubinsPaths(m_startPose, &b[0], m_turningRadius, m_isOptimal,
                                          &m_allPathTypes[0]);

                // Add cost, segment lengths & segment types
                addCostSegLensSegTypes(m_distance, m_segmentLen, m_segmentType, path, i, m_numPaths,
                                       m_turningRadius, m_nlhs);
            }

        } else {
            // Multiple startPoses, multiple goalPoses
            T a[3];
            T b[3];
            for (uint32_T i = range.begin(); i < range.end(); ++i) {
                a[0] = m_startPose[i];
                a[1] = m_startPose[i + m_maxNumPoses];
                a[2] = m_startPose[i + 2 * m_maxNumPoses];
                b[0] = m_goalPose[i];
                b[1] = m_goalPose[i + m_maxNumPoses];
                b[2] = m_goalPose[i + 2 * m_maxNumPoses];

                std::vector<DubinsPath> path =
                    computeAllDubinsPaths(&a[0], &b[0], m_turningRadius, m_isOptimal,
                                          &m_allPathTypes[0]);

                // Add cost, segment lengths & segment types
                addCostSegLensSegTypes(m_distance, m_segmentLen, m_segmentType, path, i, m_numPaths,
                                       m_turningRadius, m_nlhs);
            }
        }
    }
};

/**
 * @brief Functor for calculating the distance along Dubins motion primitive segments
 *
 * This function is used to allow both parallel (with TBB) and serial execution.
 * @see autonomousDubinsDistanceCodegen_real64, autonomousDubinsDistanceCodegen_tbb_real64
 */
template <typename T>
class DubinsDistanceFunctor {
  private:
    const T* m_startPose;
    const uint32_T m_maxNumPoses;
    const uint32_T m_numStartPoses;
    const uint32_T m_numGoalPoses;
    const T* m_goalPose;
    const T m_turningRadius;
    T* m_distance;

  public:
    DubinsDistanceFunctor(const T* startPose,
                          const uint32_T numStartPoses,
                          const T* goalPose,
                          const uint32_T numGoalPoses,
                          const T turningRadius,
                          T* distance)
        : m_startPose(startPose)
        , m_maxNumPoses(numStartPoses > numGoalPoses ? numStartPoses : numGoalPoses)
        , m_numStartPoses(numStartPoses)
        , m_numGoalPoses(numGoalPoses)
        , m_goalPose(goalPose)
        , m_turningRadius(turningRadius)
        , m_distance(distance) {
    }

    /**
     * Functor to calculate the distance along Dubins segments for a
     * given range of start and goal points.
     *
     * This functor can be executed as-is (for serial execution) or
     * be part of parallel execution, e.g. through parallel_for
     *
     * @param range The range information for how many start and
     * goal locations should be processed.
     */
    void operator()(const autonomous::ParallelRange<uint32_T>& range) const {
        // The conditional statements are constant, so should be
        // compiler-optimized.

        if (m_numStartPoses >= m_numGoalPoses) {
            // Single startPose, single goalPose
            // Multiple startPoses, single goalPose
            T a[3];
            for (uint32_T i = range.begin(); i < range.end(); ++i) {
                a[0] = m_startPose[i];
                a[1] = m_startPose[i + m_maxNumPoses];
                a[2] = m_startPose[i + 2 * m_maxNumPoses];

                DubinsPath path = computeDubinsPath(&a[0], m_goalPose, m_turningRadius);
                m_distance[i] = path.length() * m_turningRadius;
            }

        } else {
            // Single startPose, multiple goalPoses
            T b[3];
            for (uint32_T i = range.begin(); i < range.end(); ++i) {
                b[0] = m_goalPose[i];
                b[1] = m_goalPose[i + m_maxNumPoses];
                b[2] = m_goalPose[i + 2 * m_maxNumPoses];

                DubinsPath path = computeDubinsPath(m_startPose, &b[0], m_turningRadius);
                m_distance[i] = path.length() * m_turningRadius;
            }
        }
    }
};

} // namespace dubins
} // namespace autonomous

#endif /* AUTONOMOUSCODEGEN_DUBINS_FUNCTORS_HPP_ */