#ifndef TANGENT_PLANNER_RANGE_MSG_TANGENT_PLANNER_H
#define TANGENT_PLANNER_RANGE_MSG_TANGENT_PLANNER_H

#include <vector>
#include <iterator>
#include <list>
#include <memory>
#include <chrono>
#include "geometry_msgs/Pose2D.h"
#include "sensor_msgs/Range.h"
#include "geometry_msgs/Twist.h"
#define square(x) pow(x, 2)

namespace Planner {
    struct Map{
        std::shared_ptr<geometry_msgs::Pose2D> obstacle;
        std::shared_ptr<geometry_msgs::Pose2D> robot;
        std::shared_ptr<geometry_msgs::Pose2D> goal;
        Map(std::shared_ptr<geometry_msgs::Pose2D> &ob,
            std::shared_ptr<geometry_msgs::Pose2D> &rb,
            std::shared_ptr<geometry_msgs::Pose2D> &go){
            obstacle = ob;
            robot = rb;
            goal = go;
        }
        Map(std::shared_ptr<geometry_msgs::Pose2D> &rb,
            std::shared_ptr<geometry_msgs::Pose2D> &go){
            robot = rb;
            goal = go;
        }
    };
    class TangentPlanner {
    public:
        TangentPlanner();
        ~TangentPlanner();
        int sign(float number);
        void normalizeAngle(float &angle);
        float normalizeAngle(float &&angle);
        bool computeVelocityCommands();
        bool isAtGoal();
        bool isAtGoalOrientation();
        void resetVariables();
        bool foundObstacle();
        void calculateErrors();
        bool setGoal();
        void updateVelocity();
        bool foundLocalMinima();
        float calculateDistance(std::shared_ptr<geometry_msgs::Pose2D> point, std::shared_ptr<geometry_msgs::Pose2D> target);
        float convertAtan2(float&& angle);
        float average(float* array, std::size_t index=2, float av=0.0);
        void shift(float* array,std::size_t index=2);

        float goal_orientation_=0.f;
        float goal_distance_=0.f;
        float prev_angle_error_=0.f;
        float valid_dis_to_goal_=0.f;
        float xy_goal_tolerance_=0.05, yaw_goal_tolerance_=0.05;
        float heuristic_distance_=0.f, prev_heuristic_distance_=0.f;
        float dfollowed_=0.f;
        float robot_front_, robot_back_, robot_width_min_, robot_width_max_;
        bool turned_around_obs_ = false;

        // control values for turn controller
        float kp_=0.f, kp_i_=0.f;

        float sensor_msg_scale_=1.f;
        float factor_scale_ = 2.0;
        float max_trans_vel_=0.f, max_rot_vel_=0.f;

        // control values for motion controller
        float c_ = 0.f , b_ = 0.f;

        // the point to be followed
        std::vector<std::shared_ptr<geometry_msgs::Pose2D>> global_plan_;
        std::shared_ptr<geometry_msgs::Pose2D> current_pose_;
        std::shared_ptr<sensor_msgs::Range> range_msg_;
        std::shared_ptr<geometry_msgs::Twist> cmd_ptr_;


    private:
        enum BugState
        {
            MotionToGoal = 0,
            FollowBoundary = 1,
            TurnToGoal = 2,
            OrientToGoal = 3,
            StopAtGoal = 4
        } state_;
        enum Controller
        {
            Motion = 0,
            Turn = 1
        } controller_;

        Controller prev_controller_;
        int cycle_counter_ = 0;
        std::shared_ptr<geometry_msgs::Pose2D> start_follow_point_;
        std::vector<std::shared_ptr<Map>> map_;

        //will be calculated for motion controller
        float k1_ = 0.f;
        float k2_ = 0.f;
        float k3_ = 0.f;

        float error_angle_=0.f,i_error_angle_=0.f;
        float error_x_=0.f,i_error_x_=0.f;
        float error_y_=0.f,i_error_y_=0.f;

        std::tuple<std::shared_ptr<geometry_msgs::Pose2D>,std::shared_ptr<geometry_msgs::Pose2D>>calculateCenter();
        float calculateFactor();
        void followBoundaries();
        void selectDiscontinuityPoint();
        void updateController();
        void updateState();
        void resetError(){
            angle_error_t_ = 0;
            error_angle_ = 0;
            error_x_ = 0;
            error_y_ = 0;
            i_error_y_ = 0;
            i_error_x_ = 0;
        }


    protected:
        // log files
        FILE * tof_logfile;
        FILE * goal_logfile;
        FILE * odometry_logfile;
        FILE * twist_logfile;
        FILE * state_logfile;
        FILE * error_logfile;
        FILE * terminal_logfile;
        FILE * param_logfile;
        FILE * center_logfile;
        FILE * custom_logfile;
    };
}

#endif //TANGENT_PLANNER_RANGE_MSG_TANGENT_PLANNER_H