/***** MIT License ****
 *
 *   Copyright (c) 2016-2022 Davide Faconti
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy
 *   of this software and associated documentation files (the "Software"), to deal
 *   in the Software without restriction, including without limitation the rights
 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *   copies of the Software, and to permit persons to whom the Software is
 *   furnished to do so, subject to the following conditions:
 *
 *   The above copyright notice and this permission notice shall be included in all
 *   copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *   SOFTWARE.
 */

#include "rosx_introspection/ros_message.hpp"

#include <regex>
#include <sstream>
#include <string_view>

namespace RosMsgParser {

ROSMessage::ROSMessage(const std::string& msg_def) {
  std::istringstream messageDescriptor(msg_def);
  std::match_results<std::string::const_iterator> what;

  for (std::string line; std::getline(messageDescriptor, line, '\n');) {
    std::string::const_iterator begin = line.begin(), end = line.end();

    // Skip empty line or one that is a comment
    if (std::regex_search(begin, end, what, std::regex("(^\\s*$|^\\s*#)"))) {
      continue;
    }

    TrimStringLeft(line);

    if (line.compare(0, 5, "MSG: ") == 0) {
      line.erase(0, 5);
      _type = ROSType(line);
    } else {
      auto new_field = ROSField(line);
      _fields.push_back(new_field);
    }
  }
}

std::vector<std::string> SplitMultipleMessageDefinitions(const std::string& multi_def) {
  std::istringstream ss_msg(multi_def);

  std::vector<std::string> parts;
  std::string part;

  for (std::string line; std::getline(ss_msg, line, '\n');) {
    if (line.find("========") == 0) {
      parts.emplace_back(std::move(part));
      part = {};
    } else {
      part.append(line);
      part.append("\n");
    }
  }
  parts.emplace_back(std::move(part));

  return parts;
}

std::vector<ROSMessage::Ptr> ParseMessageDefinitions(const std::string& multi_def, const ROSType& root_type) {
  auto parts = SplitMultipleMessageDefinitions(multi_def);
  std::vector<ROSType> known_type;
  std::vector<ROSMessage::Ptr> parsed_msgs;

  const ROSType no_type;

  // iterating in reverse to fill known_type in the right order
  // i.e. with no missing dependencies
  for (int i = parts.size() - 1; i >= 0; i--) {
    auto msg = std::make_shared<ROSMessage>(parts[i]);

    if (i == 0) {
      // type NOT found in the definition, but provided as argument
      if (msg->type() == no_type && root_type != no_type) {
        msg->setType(root_type);
      } else if (msg->type() == no_type && root_type == no_type) {
        std::cout << multi_def << std::endl;
        throw std::runtime_error("Message type unspecified");
      }
    }

    // add to vector
    parsed_msgs.push_back(msg);
    known_type.push_back(msg->type());
  }

  // adjust types with undefined package type
  for (auto& msg : parsed_msgs) {
    for (ROSField& field : msg->fields()) {
      // if package name is missing, try to find msgName in the list of known_type
      if (field.type().pkgName().empty()) {
        std::vector<ROSType> guessed_type;

        for (const ROSType& known_type : known_type) {
          if (field.type().msgName() == known_type.msgName()) {
            guessed_type.push_back(known_type);
          }
        }

        if (!guessed_type.empty()) {
          bool better_match = false;

          // attempt to match ambiguous ros msg within package before
          // using external known type
          for (const ROSType& guessed_type : guessed_type) {
            if (guessed_type.pkgName() == root_type.pkgName()) {
              field.changeType(guessed_type);
              better_match = true;
              break;
            }
          }

          // if nothing from the same package, take a guess with the first matching msg
          // name
          if (!better_match) {
            field.changeType(guessed_type[0]);
          }
        }
      }
    }
  }

  std::reverse(parsed_msgs.begin(), parsed_msgs.end());
  return parsed_msgs;
}

MessageSchema::Ptr BuildMessageSchema(const std::string& topic_name, const std::vector<ROSMessage::Ptr>& parsed_msgs) {
  auto schema = std::make_shared<MessageSchema>();
  schema->topic_name = topic_name;
  schema->root_msg = parsed_msgs.front();

  for (const auto& msg : parsed_msgs) {
    schema->msg_library.insert({msg->type(), msg});
  }

  /// build field tree
  std::function<void(ROSMessage::Ptr, FieldTreeNode*)> recursiveTreeCreator;

  recursiveTreeCreator = [&](ROSMessage::Ptr msg, FieldTreeNode* field_node) {
    // note: should use reserve here, NOT resize
    const size_t NUM_FIELDS = msg->fields().size();
    field_node->children().reserve(NUM_FIELDS);

    for (const ROSField& field : msg->fields()) {
      if (field.isConstant()) {
        continue;
      }
      // Let's add first a child to string_node
      field_node->addChild(&field);
      FieldTreeNode* new_string_node = &(field_node->children().back());

      // builtin types will not trigger a recursion
      if (field.type().isBuiltin() == false) {
        auto new_msg = field.getMessagePtr(schema->msg_library);
        if (!new_msg) {
          throw std::runtime_error("Missing ROSType in library");
        }

        recursiveTreeCreator(new_msg, new_string_node);
      }
    }  // end of for fields
  };   // end of recursiveTreeCreator

  // build root and start recursion
  auto root_field = new ROSField(schema->root_msg->type(), topic_name);
  schema->field_tree.root()->setValue(root_field);

  recursiveTreeCreator(schema->root_msg, schema->field_tree.root());

  return schema;
}

}  // namespace RosMsgParser
