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

/**
 * @file
 * Main interfaces 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_HPP_
#define AUTONOMOUSCODEGEN_DUBINS_HPP_

#include <math.h>    // for sqrt, trigonometric functions
#include <algorithm> // for max
#include <string>
#include <vector>

#ifdef BUILDING_LIBMWAUTONOMOUSCODEGEN
#include "autonomouscodegen/autonomouscodegen_constants.hpp"        // for inf, tooSmall
#include "autonomouscodegen/autonomouscodegen_dubins_constants.hpp" // for TotalNumPaths
#include "autonomouscodegen/autonomouscodegen_trig.hpp"             // for wrapToTwoPi
#include "autonomouscodegen/autonomouscodegen_util.hpp"
#include "autonomouscodegen/autonomouscodegen_queries.hpp"          // for isNaN
#else
// To deal with the fact that PackNGo has no include file hierarchy during test
#include "autonomouscodegen_constants.hpp"
#include "autonomouscodegen_dubins_constants.hpp"
#include "autonomouscodegen_trig.hpp"
#include "autonomouscodegen_util.hpp"
#include "autonomouscodegen_queries.hpp" 
#endif

namespace autonomous {
namespace dubins {

/*
 * DubinsPathType - Enumeration for holding path type.
 */
enum DubinsPathType { LSL = 0, LSR, RSL, RSR, RLR, LRL };

/*
 * DubinsSegmentType - Enumeration for holding segment type.
 */
enum DubinsSegmentType { Left, Right, Straight };

const DubinsSegmentType pathToSegment[dubins::TotalNumPaths][3] = {
    {autonomous::dubins::Left, autonomous::dubins::Straight, autonomous::dubins::Left},
    {autonomous::dubins::Left, autonomous::dubins::Straight, autonomous::dubins::Right},
    {autonomous::dubins::Right, autonomous::dubins::Straight, autonomous::dubins::Left},
    {autonomous::dubins::Right, autonomous::dubins::Straight, autonomous::dubins::Right},
    {autonomous::dubins::Right, autonomous::dubins::Left, autonomous::dubins::Right},
    {autonomous::dubins::Left, autonomous::dubins::Right, autonomous::dubins::Left}};

/*
 * DubinsPath - Encapsulate normalized Dubins path.
 */
class DubinsPath {

  public:
    DubinsPath(const real64_T firstSegment = 0.,
               const real64_T secondSegment = 0.,
               const real64_T thirdSegment = 0.,
               const DubinsPathType pathType = autonomous::dubins::LSL)
        : pathType_(pathType) {

        segmentLengths_[0] = firstSegment;
        segmentLengths_[1] = secondSegment;
        segmentLengths_[2] = thirdSegment;
    }

    real64_T length() const {
        return segmentLengths_[0] + segmentLengths_[1] + segmentLengths_[2];
    }

    DubinsPathType getPathType() const {
        return pathType_;
    }

    const real64_T* getSegmentLengths() const {

        return &segmentLengths_[0];
    }

  private:
    DubinsPathType pathType_;
    real64_T segmentLengths_[3];
};

struct FindShortestDubinsPath {
    bool operator()(DubinsPath p1, DubinsPath p2) {
          // Return true if p1's length is not NaN and either p2's length 
          // is NaN or p1's length is less than p2's length.
          return (!autonomous::queries::isNaN(p1.length()) && 
                    (autonomous::queries::isNaN(p2.length()) || 
                    p1.length() < p2.length()));
    }
};

/*
 * dubinsLSL - compute DubinsPath for LSL path.
 */
inline DubinsPath dubinsLSL(const real64_T d, const real64_T alpha, const real64_T beta) {
    real64_T ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
    real64_T tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
    if (tmp >= -tooSmall) {
        real64_T theta = atan2(cb - ca, d + sa - sb);
        real64_T t = wrapToTwoPi(-alpha + theta);
        real64_T p = sqrt(std::max(tmp, 0.));
        real64_T q = wrapToTwoPi(beta - theta);
        return DubinsPath(t, p, q, autonomous::dubins::LSL);
    }
    return DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::LSL);
}

/*
 * dubinsLSR - compute DubinsPath for LSR path.
 */
inline DubinsPath dubinsLSR(const real64_T d, const real64_T alpha, const real64_T beta) {
    real64_T ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
    real64_T tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
    if (tmp >= -tooSmall) {
        real64_T p = sqrt(std::max(tmp, 0.));
        real64_T theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
        real64_T t = wrapToTwoPi(-alpha + theta);
        real64_T q = wrapToTwoPi(-beta + theta);
        return DubinsPath(t, p, q, autonomous::dubins::LSR);
    }
    return DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::LSR);
}

/*
 * dubinsRSL - compute DubinsPath for RSL path.
 */
inline DubinsPath dubinsRSL(const real64_T d, const real64_T alpha, const real64_T beta) {
    real64_T ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
    real64_T tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
    if (tmp >= -tooSmall) {
        real64_T p = sqrt(std::max(tmp, 0.));
        real64_T theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
        real64_T t = wrapToTwoPi(alpha - theta);
        real64_T q = wrapToTwoPi(beta - theta);
        return DubinsPath(t, p, q, autonomous::dubins::RSL);
    }
    return DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::RSL);
}

/*
 * dubinsRSR - compute DubinsPath for RSR path.
 */
inline DubinsPath dubinsRSR(const real64_T d, const real64_T alpha, const real64_T beta) {
    real64_T ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
    real64_T tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
    if (tmp >= -tooSmall) {
        real64_T theta = atan2(ca - cb, d - sa + sb);
        real64_T t = wrapToTwoPi(alpha - theta);
        real64_T p = sqrt(std::max(tmp, 0.));
        real64_T q = wrapToTwoPi(-beta + theta);
        return DubinsPath(t, p, q, autonomous::dubins::RSR);
    }
    return DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::RSR);
}

/*
 * dubinsRLR - compute DubinsPath for RLR path.
 */
inline DubinsPath dubinsRLR(const real64_T d, const real64_T alpha, const real64_T beta) {
    real64_T ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
    real64_T tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
    if (fabs(tmp) < 1.) {
        real64_T p = twoPi - acos(tmp);
        real64_T theta = atan2(ca - cb, d - sa + sb);
        real64_T t = wrapToTwoPi(alpha - theta + .5 * p);
        real64_T q = wrapToTwoPi(alpha - beta - t + p);
        return DubinsPath(t, p, q, autonomous::dubins::RLR);
    }
    return DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::RLR);
}


/*
 * dubinsLRL - compute DubinsPath for LRL path.
 */
inline DubinsPath dubinsLRL(const real64_T d, const real64_T alpha, const real64_T beta) {
    real64_T ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
    real64_T tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
    if (fabs(tmp) < 1.) {
        real64_T p = twoPi - acos(tmp);
        real64_T theta = atan2(-ca + cb, d + sa - sb);
        real64_T t = wrapToTwoPi(-alpha + theta + .5 * p);
        real64_T q = wrapToTwoPi(beta - alpha - t + p);
        return DubinsPath(t, p, q, autonomous::dubins::LRL);
    }
    return DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::LRL);
}

/*
 * shortestDubins return shortest normalized Dubins path.
 */
inline DubinsPath shortestDubins(const real64_T d, const real64_T alpha, const real64_T beta) {
    DubinsPath path;

    if (d < tooSmall && fabs(alpha - beta) < tooSmall) {
        return DubinsPath(0, d, 0);
    }

    DubinsPath minPath = dubinsLSL(d, alpha, beta);
    real64_T minLength = minPath.length();

    DubinsPath tempPath = dubinsLSR(d, alpha, beta);
    real64_T tempLength = tempPath.length();

    if (!autonomous::queries::isNaN(tempLength) && tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    tempPath = dubinsRSL(d, alpha, beta);
    tempLength = tempPath.length();

    if (!autonomous::queries::isNaN(tempLength) && tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    tempPath = dubinsRSR(d, alpha, beta);
    tempLength = tempPath.length();

    if (!autonomous::queries::isNaN(tempLength) && tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    tempPath = dubinsRLR(d, alpha, beta);
    tempLength = tempPath.length();

    if (!autonomous::queries::isNaN(tempLength) && tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    tempPath = dubinsLRL(d, alpha, beta);
    tempLength = tempPath.length();

    if (!autonomous::queries::isNaN(tempLength) && tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    return minPath;
}

/*
 * computeDubinsPath - compute Dubins shortest/all path.
 */

inline DubinsPath computeDubinsPath(const real64_T startPose[3],
                                    const real64_T goalPose[3],
                                    const real64_T turningRadius) {
    const real64_T dx = goalPose[0] - startPose[0];
    const real64_T dy = goalPose[1] - startPose[1];
    const real64_T d = sqrt(dx * dx + dy * dy) / turningRadius;

    const real64_T theta = atan2(dy, dx);

    const real64_T alpha = wrapToTwoPi(startPose[2] - theta);
    const real64_T beta = wrapToTwoPi(goalPose[2] - theta);

    return shortestDubins(d, alpha, beta);
}

/*
 * allDubins return shortest/all normalized Dubins path.
 */
inline std::vector<DubinsPath> allDubins(const real64_T d,
                                         const real64_T alpha,
                                         const real64_T beta,
                                         const boolean_T isOptimal,
                                         const boolean_T allPathTypes[dubins::TotalNumPaths]) {
    
    std::vector<DubinsPath> allPaths(autonomous::dubins::TotalNumPaths);
    uint32_T i = 0;

    // Handling empty paths
    if (d < tooSmall && fabs(alpha - beta) < tooSmall) {
        static const real64_T arr[] = {d, d, d, d, autonomous::nan, autonomous::nan};
        std::vector<real64_T> cost(arr, arr + sizeof(arr) / sizeof(arr[0]));
        DubinsPathType allDubinsPathType[dubins::TotalNumPaths] = {
            autonomous::dubins::LSL, autonomous::dubins::LSR, autonomous::dubins::RSL,
            autonomous::dubins::RSR, autonomous::dubins::RLR, autonomous::dubins::LRL};

        for (uint32_T j = 0; j < dubins::TotalNumPaths; ++j) {
            if (allPathTypes[j]) {
                allPaths[i++] = DubinsPath(0, cost[j], 0, allDubinsPathType[j]);
            }
            else{
                allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, allDubinsPathType[j]);
            }
        }
    }
    // For enabled paths, compute the segments lengths, create DubinsPath objects
    // for computed segments lengths and for disabled paths create DubinsPath 
    // object with segments lengths [nan, nan, nan].
    else {
        if (allPathTypes[0]) {
            allPaths[i++] = dubinsLSL(d, alpha, beta);
        }
        else{
            allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::LSL);
        }

        if (allPathTypes[1]) {
            allPaths[i++] = dubinsLSR(d, alpha, beta);
        }
        else{
            allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::LSR);
        }

        if (allPathTypes[2]) {
            allPaths[i++] = dubinsRSL(d, alpha, beta);
        }
        else{
            allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::RSL);
        }

        if (allPathTypes[3]) {
            allPaths[i++] = dubinsRSR(d, alpha, beta);
        }
        else{
            allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::RSR);
        }

        if (allPathTypes[4]) {
            allPaths[i++] = dubinsRLR(d, alpha, beta);
        }
        else{
            allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::RLR);
        }

        if (allPathTypes[5]) {
            allPaths[i++] = dubinsLRL(d, alpha, beta);
        }
        else{
            allPaths[i++] = DubinsPath(autonomous::nan, autonomous::nan, autonomous::nan, autonomous::dubins::LRL);
        }
    }        
    if (isOptimal) {
        struct FindShortestDubinsPath findShortest;

        // Find the shortest path and place it in a one-element vector
        std::vector<DubinsPath> singlePath;
        singlePath.push_back(*(std::min_element(allPaths.begin(), allPaths.end(), findShortest)));
        return singlePath;
    }

    return allPaths;
}

/*
 * computeAllDubinsPaths - compute Dubins shortest/all path.
 */

inline std::vector<DubinsPath> computeAllDubinsPaths(
        const real64_T startPose[3],
        const real64_T goalPose[3],
        const real64_T turningRadius,
        const boolean_T isOptimal,
        const boolean_T allPathTypes[dubins::TotalNumPaths]) {
    
    const real64_T dx = goalPose[0] - startPose[0];
    const real64_T dy = goalPose[1] - startPose[1];
    const real64_T d = sqrt(dx * dx + dy * dy) / turningRadius;

    const real64_T theta = atan2(dy, dx);

    const real64_T alpha = wrapToTwoPi(startPose[2] - theta);
    const real64_T beta = wrapToTwoPi(goalPose[2] - theta);
    
    return allDubins(d, alpha, beta, isOptimal, &allPathTypes[0]);
}

/*
 * interpolateAlongInitializedDPath - interpolate points along a
 * pre-computed Dubins path.
 */
inline void interpolateAlongInitializedDPath(const real64_T pathLength,
                                             const DubinsSegmentType* segments,
                                             const real_T* segmentLengths,
                                             const real64_T* from,
                                             const real64_T* towards,
                                             const real64_T t,
                                             const real64_T turningRadius,
                                             real64_T* state) {

    if (t <= 0.0) {
        state[0] = from[0];
        state[1] = from[1];
        state[2] = from[2];
    } else if (t >= 1.0) {
        state[0] = towards[0];
        state[1] = towards[1];
        state[2] = towards[2];
    } else {
        // Initialize at [0,0,theta]
        state[0] = 0;
        state[1] = 0;
        state[2] = from[2];

        real64_T seg = t * pathLength;
        real64_T v, phi;

        // Compute normalized update.
        for (uint32_T i = 0; i < 3 && seg > 0; ++i) {
            v = std::min(seg, segmentLengths[i]);
            phi = state[2];

            seg -= v;

            switch (segments[i]) {
            case autonomous::dubins::Left:
                state[0] += sin(phi + v) - sin(phi);
                state[1] += -cos(phi + v) + cos(phi);
                state[2] = wrapToTwoPi(phi + v);
                break;

            case autonomous::dubins::Right:
                state[0] += -sin(phi - v) + sin(phi);
                state[1] += cos(phi - v) - cos(phi);
                state[2] = wrapToTwoPi(phi - v);
                break;

            case autonomous::dubins::Straight:
                state[0] += v * cos(phi);
                state[1] += v * sin(phi);
                break;
            }
        }
        // Denormalize and update.
        state[0] = from[0] + state[0] * turningRadius;
        state[1] = from[1] + state[1] * turningRadius;
    }
}

/*
 * interpolateDubins - interpolate along Dubins curve between states.
 */
inline void interpolateDubins(const real64_T* from,
                              const real64_T* towards,
                              const real64_T maxDistance,
                              const uint32_T numSteps,
                              const real64_T turningRadius,
                              real64_T* state) {
    // Compute Dubins path.
    DubinsPath path = computeDubinsPath(from, towards, turningRadius);

    const DubinsSegmentType* segments = pathToSegment[path.getPathType()];

    // Compute the fraction of path to be traversed based on maximum
    // connection distance (maxDistance).
    //  * If the distance between the states is less than maxDistance, we
    //    interpolate all the through to the destination state (towards).
    //  * If the distance between the states is more than maxDistance, we
    //    only interpolate a fraction of the path between the two states.

    // Find the distance between the states.
    const real64_T pathLength = path.length();
    const real64_T dist = pathLength * turningRadius;

    // Find the fraction of the path to interpolate.
    const real64_T t = std::min(maxDistance / dist, 1.0);

    // Find interpolation step based on number of steps.
    const real64_T step = t / static_cast<real64_T>(numSteps);

    real64_T temp[3] = {0, 0, 0};
    real64_T fraction = 0;

    // Interpolate along transition points
    const uint32_T numTransitions = 2;
    const uint32_T arrLength = numSteps + numTransitions;

    const real64_T* segLengths = path.getSegmentLengths();

    for (uint32_T n = 0; n < numTransitions; ++n) {
        // Compute fraction along path corresponding to n-th transition
        // point
        fraction += (segLengths[n] / pathLength);

        // Saturate at maxDistance. This has the effect of returning
        // the ending pose for all transition poses that come after the
        // maxDistance has been reached.
        fraction = std::min(fraction, t);

        interpolateAlongInitializedDPath(pathLength, segments, segLengths, from, towards, fraction,
                                         turningRadius, &temp[0]);

        state[n] = temp[0];
        state[n + arrLength] = temp[1];
        state[n + 2 * arrLength] = temp[2];
    }

    // Interpolate along path at equidistant intervals
    fraction = 0;
    for (uint32_T n = numTransitions; n < arrLength; ++n) {
        fraction += step;
        interpolateAlongInitializedDPath(pathLength, segments, segLengths, from, towards, fraction,
                                         turningRadius, &temp[0]);

        state[n] = temp[0];
        state[n + arrLength] = temp[1];
        state[n + 2 * arrLength] = temp[2];
    }
}

/*
 * interpolateDubins - interpolate along given states and Dubins curve.
 */
inline void interpolateDubinsSegments(const real64_T* from,
                                      const real64_T* towards,
                                      const real64_T* samples,
                                      const uint32_T numSamples,
                                      const real64_T turningRadius,
                                      const real64_T* segmentLengths,
                                      const uint32_T* segmentTypes,
                                      real64_T* state) {
    DubinsSegmentType segments[3] = {static_cast<DubinsSegmentType>(segmentTypes[0]),
                                     static_cast<DubinsSegmentType>(segmentTypes[1]),
                                     static_cast<DubinsSegmentType>(segmentTypes[2])};

    real64_T temp[3] = {0, 0, 0};        

    const real64_T pathLength = segmentLengths[0] + segmentLengths[1] + segmentLengths[2];

    real64_T normalizedSegmentLengths[3] = {segmentLengths[0] / turningRadius,
                                            segmentLengths[1] / turningRadius,
                                            segmentLengths[2] / turningRadius};                                            
    
    const real64_T normalizedPathLength =
        normalizedSegmentLengths[0] + normalizedSegmentLengths[1] + normalizedSegmentLengths[2];

    for (uint32_T n = 0; n < numSamples; ++n) {
        // Compute fraction along path corresponding to n-th transition
        // point
        interpolateAlongInitializedDPath(normalizedPathLength, segments, normalizedSegmentLengths,
                                         from, towards, samples[n] / pathLength, turningRadius,
                                         &temp[0]);

        state[n]                    = temp[0];
        state[n + numSamples]       = temp[1];
        state[n + 2 * numSamples]   = temp[2];
    }
}
} // namespace dubins
} // namespace autonomous

#endif /* AUTONOMOUSCODEGEN_DUBINS_HPP_ */