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

/**
 * @file
 * Main interfaces for Reeds-Shepp motion primitive calculations.
 * To fully support code generation, note that this file needs to be fully
 * compliant with the C++98 standard.
 */

#ifndef AUTONOMOUSCODEGEN_REEDSSHEPP_H_
#define AUTONOMOUSCODEGEN_REEDSSHEPP_H_

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

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

namespace autonomous {

namespace reedsshepp {
/*
 * The code for ReedsShepp primitives and computation of the various
 * segment paths is inspired from Open Motion Planning Library source.
 */

/*
 * RSSegmentType - Enumeration for holding segment type
 */
enum RSSegmentType { Left = 0, Right, Straight, NOP };

const RSSegmentType pathToSegment[18][5] = {
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Left,
     autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Right,
     autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Left,
     autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Right,
     autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight,
     autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight,
     autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
     autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
     autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight,
     autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight,
     autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
     autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
     autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
     autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
     autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
     autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
     autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
    {autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight,
     autonomous::reedsshepp::Left, autonomous::reedsshepp::Right},
    {autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight,
     autonomous::reedsshepp::Right, autonomous::reedsshepp::Left}};

/*
 * shortestReedsSheppPath returns shortest normalized Reeds-Shepp path.
 */
inline autonomous::reedsshepp::ReedsSheppPath shortestReedsSheppPath(real64_T x,
                                                                     real64_T y,
                                                                     real64_T phi,
                                                                     real64_T reverseCost) {
    autonomous::reedsshepp::ReedsSheppPath minPath, tempPath;
    real64_T minLength, tempLength;

    autonomous::reedsshepp::computeCSCPath(x, y, phi, reverseCost, minPath);
    minLength = minPath.length();

    autonomous::reedsshepp::computeCCCPath(x, y, phi, reverseCost, tempPath);
    tempLength = tempPath.length();

    if (tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    autonomous::reedsshepp::computeCCCCPath(x, y, phi, reverseCost, tempPath);
    tempLength = tempPath.length();

    if (tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    autonomous::reedsshepp::computeCCSCPath(x, y, phi, reverseCost, tempPath);
    tempLength = tempPath.length();

    if (tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    autonomous::reedsshepp::computeCCSCCPath(x, y, phi, reverseCost, tempPath);
    tempLength = tempPath.length();

    if (tempLength < minLength) {
        minLength = tempLength;
        minPath = tempPath;
    }

    return minPath;
}

/*
 * computeReedsSheppPath - compute Reeds-Shepp shortest/all path.
 */
inline autonomous::reedsshepp::ReedsSheppPath computeReedsSheppPath(
    const real64_T initialConfiguration[3],
    const real64_T finalConfiguration[3],
    const real64_T turningRadius,
    const real64_T reverseCost) {
    real64_T x1 = initialConfiguration[0];
    real64_T y1 = initialConfiguration[1];
    real64_T theta1 = initialConfiguration[2];
    real64_T x2 = finalConfiguration[0];
    real64_T y2 = finalConfiguration[1];
    real64_T theta2 = finalConfiguration[2];

    real64_T dx = x2 - x1;
    real64_T dy = y2 - y1;
    real64_T c = cos(theta1);
    real64_T s = sin(theta1);

    real64_T x = c * dx + s * dy;
    real64_T y = -s * dx + c * dy;
    real64_T phi = theta2 - theta1;

    return shortestReedsSheppPath(x / turningRadius, y / turningRadius, phi, reverseCost);
}

/*
 * AllReedsSheppPath returns all normalized Reeds-Shepp path.
 */
inline std::vector<autonomous::reedsshepp::ReedsSheppPath> AllReedsSheppPath(
    real64_T x,
    real64_T y,
    real64_T phi,
    real64_T forwardCost,
    real64_T reverseCost,
    const boolean_T isOptimal,
    const boolean_T allPathTypes[autonomous::reedsshepp::TotalNumPaths],
    const uint32_T numTotalPathTypes) {
    std::vector<autonomous::reedsshepp::ReedsSheppPath> allPaths;
    allPaths.reserve(numTotalPathTypes);

    autonomous::reedsshepp::computeCSCPathAll(x, y, phi, forwardCost, reverseCost, &allPathTypes[0],
                                              &allPaths);
    autonomous::reedsshepp::computeCCCPathAll(x, y, phi, forwardCost, reverseCost, &allPathTypes[0],
                                              &allPaths);
    autonomous::reedsshepp::computeCCCCPathAll(x, y, phi, forwardCost, reverseCost,
                                               &allPathTypes[0], &allPaths);
    autonomous::reedsshepp::computeCCSCPathAll(x, y, phi, forwardCost, reverseCost,
                                               &allPathTypes[0], &allPaths);
    autonomous::reedsshepp::computeCCSCCPathAll(x, y, phi, forwardCost, reverseCost,
                                                &allPathTypes[0], &allPaths);

    if (isOptimal) {
        real64_T optimalPath = autonomous::inf;
        uint32_T optimalInd = 0;

        // Round up to 15 decimal point to get same optimal path in
        // all system (maci, linux, windows). (Geck --> g1770360)
        real64_T round_val = pow(10.0, 15);
        for (uint32_T ind = 0; ind < numTotalPathTypes; ++ind) {
            // Round up to 15 decimal point to get same optimal path in
            // all system (maci, linux, windows . (Geck --> g1770360)
            real64_T tempcost = round(allPaths[ind].cost() * round_val) / round_val;
            if (optimalPath > tempcost) {
                optimalPath = tempcost;
                optimalInd = ind;
            }
        }

        std::vector<autonomous::reedsshepp::ReedsSheppPath> path(1);
        path[0] = allPaths[optimalInd];

        return path;
    }
    return allPaths;
}

/*
 * computeAllReedsSheppPaths - compute Reeds-Shepp shortest/all path.
 */
inline std::vector<autonomous::reedsshepp::ReedsSheppPath> computeAllReedsSheppPaths(
    const real64_T initialConfiguration[3],
    const real64_T finalConfiguration[3],
    const real64_T turningRadius,
    const real64_T forwardCost,
    const real64_T reverseCost,
    const boolean_T isOptimal,
    const boolean_T allPathTypes[autonomous::reedsshepp::TotalNumPaths],
    const uint32_T numTotalPathTypes) {
    real64_T x1 = initialConfiguration[0];
    real64_T y1 = initialConfiguration[1];
    real64_T theta1 = initialConfiguration[2];
    real64_T x2 = finalConfiguration[0];
    real64_T y2 = finalConfiguration[1];
    real64_T theta2 = finalConfiguration[2];

    real64_T dx = x2 - x1;
    real64_T dy = y2 - y1;
    real64_T c = cos(theta1);
    real64_T s = sin(theta1);

    real64_T x = c * dx + s * dy;
    real64_T y = -s * dx + c * dy;
    real64_T phi = theta2 - theta1;

    return AllReedsSheppPath(x / turningRadius, y / turningRadius, phi, forwardCost, reverseCost,
                             isOptimal, &allPathTypes[0], numTotalPathTypes);
}

/* 
 * Compute direction value for start/goal pose.
 *
 * 1) For segmentLengths input {0, 9, 2, -10, 0},
 *
 * computeDirectionAtEdges({0, 9, 2, -10, 0}, true) or
 * computeDirectionAtEdges({0, 9, 2, -10, 0}) will return 1, sign of first
 * non-zero segment (sign(9)) i.e. direction value of start pose.
 *
 * computeDirectionAtEdges({0, 9, 2, -10, 0}, false) will return -1, sign
 * of last non-zero segment (sign(-10)) i.e. direction value of goal pose.
 *
 * 2) For segmentLengths input {-8, -1, 2, 10, 0},
 *
 * computeDirectionAtEdges({-8, -1, 2, 10, 0}, true) or
 * computeDirectionAtEdges({-8, -1, 2, 10, 0}) will return -1, sign of first
 * non-zero segment (sign(-8)) i.e. direction value of start pose.
 *
 * computeDirectionAtEdges({-8, -1, 2, 10, 0}, false) will return 1, sign
 * of last non-zero segment (sign(10)) i.e. direction value of goal pose.
 *
 */
inline real64_T computeDirectionAtEdges(const real64_T* segmentLengths, boolean_T startFlag = true)
{
    real64_T direction = 1;
    for (uint32_T i = 0; i < 5; i++){
        if(segmentLengths[i] != 0){
            direction = ((segmentLengths[i]<0) ? -1 : 1);
            //Exit from loop for start pose direction computation 
            if (startFlag){
                break;
            }
        }                
    }
    return direction;
}

/*
 * interpolateAlongInitializedRSPath - interpolate points along a
 * pre-computed ReedsShepp path.
 */
inline real64_T interpolateAlongInitializedRSPath(const real64_T pathLength,
                                              const RSSegmentType* segments,
                                              const real64_T* segmentLengths,
                                              const real64_T* from,
                                              const real64_T* towards,
                                              const real64_T t,
                                              const real64_T turningRadius,
                                              real64_T* state) {
    
    real64_T direction = 1;
    
    if (t <= 0.0) {
        state[0] = from[0];
        state[1] = from[1];
        state[2] = from[2];
        
        // To ensure first pose direction is correct
        direction = computeDirectionAtEdges(segmentLengths);
    } else if (t >= 1.0) {
        state[0] = towards[0];
        state[1] = towards[1];
        state[2] = towards[2];
        
        // To ensure last pose direction is correct
        direction = computeDirectionAtEdges(segmentLengths, false);
    } 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 < 5 && seg > 0; ++i) {
            
            if (segmentLengths[i] < 0) {
                v = std::max(-seg, segmentLengths[i]);
                seg += v;
                direction = -1;
            } else {
                v = std::min(seg, segmentLengths[i]);
                seg -= v;
                direction = 1;
            }

            phi = state[2];

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

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

            case autonomous::reedsshepp::Straight:               
                state[0] += v * cos(phi);
                state[1] += v * sin(phi);
                break;

            case autonomous::reedsshepp::NOP:
                // no update needed
                break;
            }
        }
        // Denormalize and update.
        state[0] = from[0] + state[0] * turningRadius;
        state[1] = from[1] + state[1] * turningRadius;
    }
    
    return direction;
}

/*
 * interpolateRS - interpolate along ReedsShepp curve between states.
 */
inline void interpolateRS(const real64_T* from,
                          const real64_T* towards,
                          const real64_T maxDistance,
                          const uint32_T numSteps,
                          const real64_T turningRadius,
                          const real64_T reverseCost,
                          real64_T* state) {
    // Compute ReedsShepp path.
    autonomous::reedsshepp::ReedsSheppPath path =
        computeReedsSheppPath(from, towards, turningRadius, reverseCost);

    const RSSegmentType* 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);

    std::vector<real64_T> temp(3, 0);
    real64_T fraction = 0;

    // Interpolate along transition points
    const uint32_T numTransitions = 4;
    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 += (autonomous::abs(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);

        interpolateAlongInitializedRSPath(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;
        interpolateAlongInitializedRSPath(pathLength, segments, segLengths, from, towards, fraction,
                                          turningRadius, &temp[0]);

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

/*
 * interpolateReedsSheppSegments - interpolate along given states and Reeds-Shepp curve.
 */
inline void interpolateReedsSheppSegments(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 int32_T* segmentDirections,
                                      const uint32_T* segmentTypes,
                                      real64_T* state,
                                      real64_T* directions) {
    RSSegmentType segments[5] = {static_cast<RSSegmentType>(segmentTypes[0]),
                                     static_cast<RSSegmentType>(segmentTypes[1]),
                                     static_cast<RSSegmentType>(segmentTypes[2]),
                                     static_cast<RSSegmentType>(segmentTypes[3]),
                                     static_cast<RSSegmentType>(segmentTypes[4])};
    
    real64_T temp[3] = {0, 0, 0};

    const real64_T pathLength = segmentLengths[0] + segmentLengths[1] + 
            segmentLengths[2] + segmentLengths[3] + segmentLengths[4];
    
    real64_T normalizedSegmentLengths[5] = {segmentLengths[0] / turningRadius,
                                            segmentLengths[1] / turningRadius,
                                            segmentLengths[2] / turningRadius,
                                            segmentLengths[3] / turningRadius,
                                            segmentLengths[4] / turningRadius};
                                              
    const real64_T normalizedPathLength =
        normalizedSegmentLengths[0] + normalizedSegmentLengths[1] + 
            normalizedSegmentLengths[2] + normalizedSegmentLengths[3] +
            normalizedSegmentLengths[4];        
    
    real64_T round_val = pow(10.0, 15);
    
    normalizedSegmentLengths[0] = round(normalizedSegmentLengths[0] * round_val) / round_val;
    normalizedSegmentLengths[1] = round(normalizedSegmentLengths[1] * round_val) / round_val;
    normalizedSegmentLengths[2] = round(normalizedSegmentLengths[2] * round_val) / round_val;
    normalizedSegmentLengths[3] = round(normalizedSegmentLengths[3] * round_val) / round_val;
    normalizedSegmentLengths[4] = round(normalizedSegmentLengths[4] * round_val) / round_val;
    
    // Round up to 15 decimal point to get same optimal path in all system 
    // (maci, linux, windows). (Geck --> g1770360)
    normalizedSegmentLengths[0] = normalizedSegmentLengths[0]*static_cast<real64_T>(segmentDirections[0]);
    normalizedSegmentLengths[1] = normalizedSegmentLengths[1]*static_cast<real64_T>(segmentDirections[1]);
    normalizedSegmentLengths[2] = normalizedSegmentLengths[2]*static_cast<real64_T>(segmentDirections[2]);
    normalizedSegmentLengths[3] = normalizedSegmentLengths[3]*static_cast<real64_T>(segmentDirections[3]);
    normalizedSegmentLengths[4] = normalizedSegmentLengths[4]*static_cast<real64_T>(segmentDirections[4]);    

    for (uint32_T n = 0; n < numSamples; ++n) {
        // Compute fraction along path corresponding to n-th transition
        // point        
        
        // Round up to 15 decimal point to get same optimal path in all 
        // system (maci, linux, windows). (Geck --> g1770360)
        real64_T roundedSample = round(samples[n] * round_val) / round_val;
        roundedSample = roundedSample / pathLength;               
        
        if (directions){
            directions[n] = interpolateAlongInitializedRSPath(normalizedPathLength, segments, normalizedSegmentLengths,
                    from, towards, roundedSample, turningRadius, &temp[0]);
        }else{
            interpolateAlongInitializedRSPath(normalizedPathLength, segments, normalizedSegmentLengths,
                    from, towards, roundedSample, turningRadius, &temp[0]);
        }

        state[n]                    = round(temp[0] * round_val) / round_val;
        state[n + numSamples]       = round(temp[1] * round_val) / round_val;
        state[n + 2 * numSamples]   = round(temp[2] * round_val) / round_val;        
    }
}

} // namespace reedsshepp
} // namespace autonomous

#endif /* AUTONOMOUSCODEGEN_REEDSSHEPP_H_ */