#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) {}

    rapidjson::Value Point32::toJson(rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value val(rapidjson::kObjectType);

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

        return val;
    }


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

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

    rapidjson::Value Time::toJson(rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value val(rapidjson::kObjectType);

        val.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator);
        val.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator);

        return val;
    }


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

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

    rapidjson::Value Header::toJson(rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value val(rapidjson::kObjectType);

        val.AddMember("seq", rapidjson::Value().SetUint(this->seq), allocator);
        val.AddMember("stamp", this->stamp.toJson(allocator), allocator);
        val.AddMember("frame_id", rapidjson::Value().SetString(this->frame_id.data(), this->frame_id.length(), allocator), allocator);

        return val;
    }


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

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

    rapidjson::Value Polygon::toJson(rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value val(rapidjson::kObjectType);

        rapidjson::Value points(rapidjson::kArrayType);

        for(std::vector<Point32>::iterator it = this->points.begin(); it != this->points.end(); ++it)
            points.PushBack(it->toJson(allocator), allocator);

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

        return val;
    }


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

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

    rapidjson::Value PolygonStamped::toJson(rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value val(rapidjson::kObjectType);

        val.AddMember("header", this->header.toJson(allocator), allocator);
        val.AddMember("polygon", this->polygon.toJson(allocator), allocator);

        return val;
    }



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

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

    rapidjson::Value PolygonArray::toJson(rapidjson::Document::AllocatorType &allocator)
    {
        rapidjson::Value val(rapidjson::kObjectType);

        val.AddMember("header", this->header.toJson(allocator), allocator);

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

        for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it)
            polygons.PushBack(it->toJson(allocator), allocator);
        val.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);
        val.AddMember("labels", labels, allocator);

        rapidjson::Value likelyhood(rapidjson::kArrayType);

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

        return val;
    }

}