//
// Created by bastian on 28.02.22.
//
#ifndef BUILD_EXTENSIONS_H
#define BUILD_EXTENSIONS_H

#include "Area.h"
#include <geometry_msgs/msg/pose.hpp>

namespace BT {
    template<> inline
    std::shared_ptr<Position2D> convertFromString(StringView str) {
        auto split = splitString(str, ',');
        if (split.size() != 2) {
            throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
        }

        auto pos = std::make_shared<Position2D>();
        pos->x = convertFromString<double>(split[0]);
        pos->y = convertFromString<double>(split[1]);
        return pos;
    }

    template<> inline
    std::shared_ptr<Area> convertFromString(StringView str) {
        auto parts = splitString(str, '|');
        if (parts.size() < 3) {
            throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
        }

        auto output = std::make_shared<Area>();
        std::vector<Position2D> array(parts.size());
        for (unsigned long i = 0; i < parts.size(); ++i) {
            array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
        }
        output->triangles = array;
        return output;
    }

    template<> inline
    std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
        auto parts = splitString(str, ',');
        if (!(parts.size() == 3 || parts.size() == 7)) {
            throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
        }

        auto pose = std::make_shared<geometry_msgs::msg::Pose>();
        if(parts.size() == 7){
            pose->orientation.w = convertFromString<double>(parts[3]);
            pose->orientation.x = convertFromString<double>(parts[4]);
            pose->orientation.y = convertFromString<double>(parts[5]);
            pose->orientation.z = convertFromString<double>(parts[6]);
        }else{
            pose->orientation.w = 1;
            pose->orientation.x = 0;
            pose->orientation.y = 0;
            pose->orientation.z = 0;
        }
        pose->position.x = convertFromString<double>(parts[0]);
        pose->position.y = convertFromString<double>(parts[1]);
        pose->position.z = convertFromString<double>(parts[2]);

        return pose;
    }

    template<> inline
    std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
        auto parts = splitString(str, ',');
        if (parts.size() != 3) {
            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
        }

        auto point = std::make_shared<geometry_msgs::msg::Point>();
        point->x = convertFromString<double>(parts[0]);
        point->y = convertFromString<double>(parts[1]);
        point->z = convertFromString<double>(parts[2]);

        return point;
    }

    template<> inline
    std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
        auto parts = splitString(str, ',');
        if (parts.size() != 4) {
            throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
        }

        auto point = std::make_shared<geometry_msgs::msg::Quaternion>();
        point->w = convertFromString<double>(parts[0]);
        point->x = convertFromString<double>(parts[1]);
        point->y = convertFromString<double>(parts[2]);
        point->z = convertFromString<double>(parts[3]);

        return point;
    }
}

#endif //BUILD_EXTENSIONS_H