//
// Created by bastian on 26.03.22.
//

#include "GenerateXYPose.h"

double BT::GenerateXYPose::getArea(const std::shared_ptr<Area> &area, unsigned long id) {
    Position2D a = area->triangles[id];
    Position2D b = area->triangles[id + 1];
    Position2D c = area->triangles[id + 2];

    return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y));
}

Position2D BT::GenerateXYPose::getRandomPosInTriangle(const std::shared_ptr<Area> &area, unsigned long id) {
    Position2D a = area->triangles[id];
    Position2D b = area->triangles[id + 1];
    Position2D c = area->triangles[id + 2];

    Position2D ab = {b.x - a.x, b.y - a.y};
    Position2D ac = {c.x - a.x, c.y - a.y};

    double u1 = dist(re);
    double u2 = dist(re);

    if (u1 + u2 > 1) {
        u1 = 1 - u1;
        u2 = 1 - u2;
    }

    return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
}

BT::GenerateXYPose::GenerateXYPose(const std::string &name, const BT::NodeConfiguration &config) :
        SyncActionNode(name, config) {
}

BT::PortsList BT::GenerateXYPose::providedPorts() {
    return {
            InputPort<std::shared_ptr<Area>>("area", "area of pose"),
            OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", "generated pose")
    };
}

BT::NodeStatus BT::GenerateXYPose::tick() {
    std::shared_ptr<Area> area;
    getInput("area", area);

    unsigned long triangleCount = area->triangles.size() - 2;

    uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
    std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)};

    std::vector<double> weights(triangleCount);
    double sum = 0;
    for (unsigned long i = 0; i < triangleCount; ++i) {
        double currentArea = getArea(area,i);
        weights[i] = currentArea;
        sum += currentArea;
    }
    for (unsigned long i = 0; i < triangleCount; ++i) {
        weights[i] = weights[i] / sum;
    }
    sum = 0;
    for (unsigned long i = 0; i < triangleCount; ++i) {
        weights[i] = weights[i] + sum;
        sum += weights[i];
    }

    double triangleRandom = dist(re);
    unsigned long i = 0;
    while (triangleRandom > weights[i + 1] && i < weights.size() - 1) {
        i++;
    }
    auto pos = getRandomPosInTriangle(area,i);

    auto randomPose = std::make_shared<geometry_msgs::msg::Pose>();
    randomPose->position.x = pos.x;
    randomPose->position.y = pos.y;
    randomPose->position.z = 0;
    randomPose->orientation.w = 0;
    randomPose->orientation.x = 0;
    randomPose->orientation.y = 0;
    randomPose->orientation.z = 0;
    
    std::cout << "Generated Pose." << std::endl;

    setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);
    return NodeStatus::SUCCESS;
}
