#ifndef ROBOT_H
#define ROBOT_H

#include <QPointF>
#include <QReadWriteLock>
#include <QDebug>
#include <cmath>
#include "ru/gui/map_widget.h"
#include "ru/utils/angle.h"

//! ???? ????? ????????
enum point_state{
        ps_none = 0,
        ps_old = 1,
        ps_current = 2,
        ps_new = 3
};

// ???????? ????????? ????? ??????
// ??? ???????? ? ?????? ? ????????
class RobotDescription {
public:
    float wheel_left_length;
    float wheel_right_length;

    float chasis_width_d2;  // ???????? ?????????? ????? ????????

    float steps_count;    // ????? ???????? ?? ??????

    RobotDescription() {
        wheel_left_length = 1;
        wheel_right_length = 1;
        chasis_width_d2 = 0.5;
        steps_count = 32;
    }
    RobotDescription(float wheel_l, float wheel_r, float width_d2, float steps_count) {
        RobotDescription::wheel_left_length    = wheel_l;
        RobotDescription::wheel_right_length   = wheel_r;
        RobotDescription::chasis_width_d2      = width_d2;
        RobotDescription::steps_count          = steps_count;
    }
};

//! ??????? ??????
class RobotPosition {
public:
    float x;
    float y;
    float a;
    float speed;    //!< ???????? ? ?/?
    RobotPosition() {
        x = 0;
        y = 0;
        a = 0;
        speed = 0;
    }

    RobotPosition(float x, float y, float ang, float speed = 0.0f) {
        RobotPosition::x = x;
        RobotPosition::y = y;
        RobotPosition::a = ang;
        RobotPosition::speed = speed;
    }

    inline operator QPointF() const {
        return QPointF(x,y);
    }

    inline float distanceTo(const RobotPosition& pos)
    {
        return sqrt(sqr(pos.x-x)+sqr(pos.y-y));
    }
};


//-----------------------------------------------------
//! Gozel desc
class robot_gozel_description {
public:
    float chassis_lenght;   //!< ????? ???????? ????
    float chassis_width;    //!< ?????? ???????? ????

    float wheel_length;     //!< ????? ??????

    float steps_count;      //!< ????? ???????? ?? ?????? ??????

    robot_gozel_description() {
        chassis_lenght = 2.9f;
        chassis_width = 1.6f;
        wheel_length = 2.101f;
        steps_count = 32.0;
    }
    robot_gozel_description(float lenght, float width, float wheel_length, float steps_count) {
        robot_gozel_description::chassis_lenght  = lenght;
        robot_gozel_description::chassis_width   = width;
        robot_gozel_description::wheel_length    = wheel_length;
        robot_gozel_description::steps_count     = steps_count;
    }
};

//---------------------------------------------------------------------------------------------------------
//! ????? ????????
class Waypoint {
public:
        // ?????????
        RobotPosition pos;

        // ??????. ????? ?????? ??? ????????? ??? ???
        point_state attr;      // 0 - ??? 1 - ????????? 2 - ??????? 3 - ?????
        // tag
        int point_tag;

        Waypoint();
        Waypoint(const RobotPosition& pos, point_state attr = ps_new);
};

//! ??? ??????????
typedef std::vector<Waypoint> WaypointList;


#endif // ROBOT_H