#ifndef __NAV_COMM_H__
#define __NAV_COMM_H__

#include <stdio.h>
#include <vector>
#include <cmath>

namespace common
{

using Point = struct _Point;
using Path = std::vector<struct _Point>;
using Twist = struct _Twist;
using Config = struct _Config;

struct _Twist
{
    _Twist() : x(0.0), y(0.0), angular(0.0)
    {}
    float x;
    float y;
    float angle;
    float angular;
};

struct _Point {
    _Point() :  x(0.0),
                y(0.0),
                theta(0.0),
                head(0.0),
                max_linear(0.0)
    {}
    float x;
    float y;
    float theta;

    float head;
    float max_linear;
};

struct _Config
{    
    _Config() : max_linear(0.5), 
                max_transla(0.5),     
                max_angular(0.5),
                minmal_next(0.4),
                maxmal_next(0.8),
                const_next(false),
                maxmal_offset(1.2),
                close_ad(1.0),
                toler_x(0.01),
                toler_y(0.01),
                toler_theta(0.01), // 1°
                toler_start_theta(0.1),
                toler_follow(0.2),
                toler_follow_theta(0.51),    // 30°
                angular_slow_p(0.8),
                linear_slow_p(0.3)
    {}
    float max_linear;   // 最大线速度
    float max_transla; // 最大平移速度
    float max_angular;  // 最大角速度

    float minmal_next; // 最小探测距离
    float maxmal_next; // 最大探测距离
    bool const_next;

    float maxmal_offset; // 最大偏移距离;
    float close_ad;  // 接近调整
    // ()判断局部路劲与全局路径的差异度，差异度超过maxmal_offset则减小next,除非const_next

    float toler_x;  // x方向误差
    float toler_y;  // y方向误差
    float toler_theta;  // 角度方向误差
    float toler_start_theta;    // 起始角度误差
    float toler_follow; // 路径跟随容忍误差
    float toler_follow_theta;

    float angular_slow_p;
    float linear_slow_p;
};

enum NavState {
    DATA_MISS = -2,
    HEAD_WARN,
    HIT_OBSTACLE,
    OFF_COURSE,
    PATH_BLOCK,
    STOP,
    PATH_EMPTY,
    TOO_CLOSE,
    ING,
    ROTATE,
    DONE
};

enum NavStep {
    STANDBY = 0,
    TANGENT,
    TRACK,
    ANGLE,
    COMPLETE
};

inline float dist(Point p1, Point p2)
{
    return hypot(p1.x - p2.x, p1.y - p2.y);
}

/**
 *  路径的长度 
 */
inline float lineL(common::Path& path, int s, int e)
{
    float l = 0;

    for (int i = s;i < e - 1;i++) {
        l += dist(path[i], path[i + 1]);
    }
    return l;
}

/**
 *  p点p1与p2点所构成直线的距离 
 */
inline float PLD(common::Point& p1, common::Point& p2, common::Point& p)
{
    float mu = p.y * (p2.x - p1.x) - p.x * (p2.y - p1.y) + p1.x * (p2.y - p1.y) - p1.y * (p2.x - p1.x);
    float z = sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));

    return fabs(mu) / z;
}

inline float norm(float t)
{
    float pi = 3.1415926;

    while (t > 2.0 * pi) {
        t -= 2.0 * pi;
    }
    while (t < -2.0 * pi) {
        t += 2.0 * pi;
    }
    if (t > pi) {
        t -= 2.0 * pi;
    }
    if (t < -pi) {
        t += 2.0 * pi;
    }
    
    return t;
}

} // namespace common


#endif