//
// Created by bastian on 30.03.22.
//

#include "InAreaTest.h"
#include "../Extensions.hpp"

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

double sign(Position2D p1, Position2D p2, Position2D p3) {
    return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y);
}

bool pointInTri(Position2D point, Position2D triA, Position2D triB, Position2D triC) {
    double d1 = sign(point, triA, triB);
    double d2 = sign(point, triB, triC);
    double d3 = sign(point, triC, triA);

    return !(((d1 < 0) || (d2 < 0) || (d3 < 0)) && ((d1 > 0) || (d2 > 0) || (d3 > 0)));
}

BT::InAreaTest::InAreaTest(const std::string &name, const BT::NodeConfiguration &config) : ConditionNode(name, config) {}

BT::NodeStatus BT::InAreaTest::tick() {

    std::shared_ptr<Area> area;
    if (!getInput<std::shared_ptr<Area>>("area", area)) {
        std::cout<<"No area given"<<std::endl;
        return NodeStatus::FAILURE;
    }

    std::shared_ptr<geometry_msgs::msg::Pose> pose;
    if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", pose)) {
        std::cout<<"No pose given"<<std::endl;
        return NodeStatus::FAILURE;
    }

    Position2D point = {pose->position.x, pose->position.y};
    for (unsigned long i = 0; i < area->triangles.size() - 2; i++) {
        if (pointInTri(point, area->triangles[i], area->triangles[i + 1], area->triangles[i + 2])) {
            return NodeStatus::SUCCESS;
        }
    }

    return NodeStatus::FAILURE;
}
