#include "snake.h"



namespace rosbridge_msgs {



    //===================================================================
    // Point32
    //===================================================================
    Point32::Point32(): x(0), y(0), z(0) {}

    Point32::Point32(_Float32 x, _Float32 y, _Float32 z): x(x), y(y), z(z) {}

    bool Point32::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {

        doc.AddMember("x", rapidjson::Value().SetFloat(this->x), allocator);
        doc.AddMember("y", rapidjson::Value().SetFloat(this->y), allocator);
        doc.AddMember("z", rapidjson::Value().SetFloat(this->z), allocator);

        return true;
    }


    //===================================================================
    // Time
    //===================================================================
    Time::Time(): secs(0), nsecs(0) {}

    Time::Time(uint32_t secs, uint32_t nsecs): secs(secs), nsecs(nsecs) {}

    bool Time::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        doc.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator);
        doc.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator);

        return true;
    }


    //===================================================================
    // Header
    //===================================================================
    Header::Header():  seq(0), frame_id("") {}

    Header::Header(uint32_t seq, const Time &stamp, std::string frame_id):  seq(seq), stamp(stamp), frame_id(frame_id) {}

    bool Header::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        doc.AddMember("seq", rapidjson::Value().SetUint(this->seq), allocator);
        rapidjson::Document stamp(rapidjson::kObjectType);
        if (!this->stamp.toJson(stamp, allocator))
                return false;
        doc.AddMember("stamp", stamp, allocator);
        doc.AddMember("frame_id", rapidjson::Value().SetString(this->frame_id.data(), this->frame_id.length(), allocator), allocator);

        return true;
    }


    //===================================================================
    // Polygon
    //===================================================================
    Polygon::Polygon(){}

    Polygon::Polygon(const std::vector<Point32> &points) : points(points) {}

    bool Polygon::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value points(rapidjson::kArrayType);

        for(std::vector<Point32>::iterator it = this->points.begin(); it != this->points.end(); ++it) {
            rapidjson::Document point(rapidjson::kObjectType);
            if (!it->toJson(point, allocator))
                return false;
            points.PushBack(point, allocator);
        }

        doc.AddMember("points", points, allocator);

        return true;
    }


    //===================================================================
    // PolygonStamped
    //===================================================================
    PolygonStamped::PolygonStamped() {}

    PolygonStamped::PolygonStamped(const Header &header, const Polygon &polygon) : header(header), polygon(polygon){}

    bool PolygonStamped::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (!this->header.toJson(header, allocator))
            return false;
        rapidjson::Document polygon(rapidjson::kObjectType);
        if (!this->polygon.toJson(polygon, allocator))
            return false;
        doc.AddMember("header", header, allocator);
        doc.AddMember("polygon", polygon, allocator);

        return true;
    }



    //===================================================================
    // PolygonArray
    //===================================================================
    PolygonArray::PolygonArray() {}

    PolygonArray::PolygonArray(const Header &header,
                               const std::vector<PolygonStamped> &polygons,
                               const std::vector<uint32_t> &labels,
                               const std::vector<_Float32> &likelihood)
        : header(header), polygons(polygons), labels(labels), likelihood(likelihood) {}

    bool PolygonArray::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Document header(rapidjson::kObjectType);
        if (!this->header.toJson(header, allocator))
            return false;
        doc.AddMember("header", header, allocator);

        rapidjson::Value polygons(rapidjson::kArrayType);

        for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it){
            rapidjson::Document polygon(rapidjson::kObjectType);
            if (!it->toJson(polygon, allocator))
                return false;
            polygons.PushBack(polygon, allocator);
        }
        doc.AddMember("polygons", polygons, allocator);

        rapidjson::Value labels(rapidjson::kArrayType);

        for(auto it = this->labels.begin(); it != this->labels.end(); ++it)
            labels.PushBack(rapidjson::Value().SetUint(*it), allocator);
        doc.AddMember("labels", labels, allocator);

        rapidjson::Value likelihood(rapidjson::kArrayType);

        for(auto it = this->likelihood.begin(); it != this->likelihood.end(); ++it)
            likelihood.PushBack(rapidjson::Value().SetFloat(*it), allocator);
        doc.AddMember("likelihood", likelihood, allocator);

        return true;
    }

}