// Copyright 1996-2023 Cyberbotics Ltd.

//

// Licensed under the Apache License, Version 2.0 (the "License");

// you may not use this file except in compliance with the License.

// You may obtain a copy of the License at

//

// https://www.apache.org/licenses/LICENSE-2.0

//

// Unless required by applicable law or agreed to in writing, software

// distributed under the License is distributed on an "AS IS" BASIS,

// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

// See the License for the specific language governing permissions and

// limitations under the License.

#define WB_ALLOW_MIXING_C_AND_CPP_API

#include <webots/supervisor.h>

#include <webots/Node.hpp>

#include <iostream>

#include <map>

using namespace std;

using namespace webots;

static map<WbNodeRef, Node *> nodeMap;

Node *Node::findNode(WbNodeRef ref) {

if (!ref)

return NULL;

map<WbNodeRef, Node *>::iterator iter = nodeMap.find(ref);

if (iter != nodeMap.end())

return iter->second;

Node *node = new Node(ref);

nodeMap.insert(pair<WbNodeRef, Node *>(ref, node));

return node;

}

void Node::cleanup() {

map<WbNodeRef, Node *>::iterator iter;

for (iter = nodeMap.begin(); iter != nodeMap.end(); ++iter)

delete (*iter).second;

nodeMap.clear();

}

Node::Node(WbNodeRef ref) {

nodeRef = ref;

}

void Node::remove() {

wb_supervisor_node_remove(nodeRef);

}

int Node::getId() const {

return wb_supervisor_node_get_id(nodeRef);

}

Node::Type Node::getType() const {

return Type(wb_supervisor_node_get_type(nodeRef));

}

std::string Node::getDef() const {

return string(wb_supervisor_node_get_def(nodeRef));

}

std::string Node::getTypeName() const {

return string(wb_supervisor_node_get_type_name(nodeRef));

}

std::string Node::getBaseTypeName() const {

return string(wb_supervisor_node_get_base_type_name(nodeRef));

}

Node *Node::getParentNode() const {

WbNodeRef parentRef = wb_supervisor_node_get_parent_node(nodeRef);

return findNode(parentRef);

}

Node *Node::getFromProtoDef(const std::string &name) const {

WbNodeRef internalNodeRef = wb_supervisor_node_get_from_proto_def(nodeRef, name.c_str());

return findNode(internalNodeRef);

}

bool Node::isProto() const {

return wb_supervisor_node_is_proto(nodeRef);

}

int Node::getNumberOfFields() const {

return wb_supervisor_node_get_number_of_fields(nodeRef);

}

int Node::getProtoNumberOfFields() const {

return wb_supervisor_node_get_proto_number_of_fields(nodeRef);

}

Field *Node::getField(const std::string &fieldName) const {

WbFieldRef fieldRef = wb_supervisor_node_get_field(nodeRef, fieldName.c_str());

return Field::findField(fieldRef);

}

Field *Node::getFieldByIndex(const int index) const {

WbFieldRef fieldRef = wb_supervisor_node_get_field_by_index(nodeRef, index);

return Field::findField(fieldRef);

}

Field *Node::getProtoField(const std::string &fieldName) const {

WbFieldRef fieldRef = wb_supervisor_node_get_proto_field(nodeRef, fieldName.c_str());

return Field::findField(fieldRef);

}

Field *Node::getProtoFieldByIndex(const int index) const {

WbFieldRef fieldRef = wb_supervisor_node_get_proto_field_by_index(nodeRef, index);

return Field::findField(fieldRef);

}

const double *Node::getPosition() const {

return wb_supervisor_node_get_position(nodeRef);

}

const double *Node::getOrientation() const {

return wb_supervisor_node_get_orientation(nodeRef);

}

const double *Node::getPose() const {

return wb_supervisor_node_get_pose(nodeRef, NULL);

}

const double *Node::getPose(const Node *fromNode) const {

return wb_supervisor_node_get_pose(nodeRef, fromNode->nodeRef);

}

void Node::enableContactPointsTracking(int samplingPeriod, bool includeDescendants) const {

wb_supervisor_node_enable_contact_points_tracking(nodeRef, samplingPeriod, includeDescendants);

}

void Node::disableContactPointsTracking(bool includeDescendants) const {

wb_supervisor_node_disable_contact_points_tracking(nodeRef);

}

ContactPoint *Node::getContactPoints(bool includeDescendants, int *size) const {

return wb_supervisor_node_get_contact_points(nodeRef, includeDescendants, size);

}

void Node::enablePoseTracking(int samplingPeriod) const {

wb_supervisor_node_enable_pose_tracking(nodeRef, samplingPeriod, NULL);

}

void Node::disablePoseTracking() const {

wb_supervisor_node_disable_pose_tracking(nodeRef, NULL);

}

void Node::enablePoseTracking(int samplingPeriod, const Node *fromNode) const {

wb_supervisor_node_enable_pose_tracking(nodeRef, samplingPeriod, fromNode->nodeRef);

}

void Node::disablePoseTracking(const Node *fromNode) const {

wb_supervisor_node_disable_pose_tracking(nodeRef, fromNode->nodeRef);

}

const double *Node::getCenterOfMass() const {

return wb_supervisor_node_get_center_of_mass(nodeRef);

}

const double *Node::getContactPoint(int index) const {

return wb_supervisor_node_get_contact_point(nodeRef, index);

}

Node *Node::getContactPointNode(int index) const {

return findNode(wb_supervisor_node_get_contact_point_node(nodeRef, index));

}

int Node::getNumberOfContactPoints(bool includeDescendants) const {

return wb_supervisor_node_get_number_of_contact_points(nodeRef, includeDescendants);

}

bool Node::getStaticBalance() const {

return wb_supervisor_node_get_static_balance(nodeRef);

}

const double *Node::getVelocity() const {

return wb_supervisor_node_get_velocity(nodeRef);

}

std::string Node::exportString() const {

return string(wb_supervisor_node_export_string(nodeRef));

}

void Node::setVelocity(const double velocity[6]) {

wb_supervisor_node_set_velocity(nodeRef, velocity);

}

void Node::resetPhysics() {

wb_supervisor_node_reset_physics(nodeRef);

}

void Node::restartController() {

wb_supervisor_node_restart_controller(nodeRef);

}

void Node::setVisibility(Node *from, bool visible) {

wb_supervisor_node_set_visibility(nodeRef, from->nodeRef, visible);

}

void Node::moveViewpoint() const {

wb_supervisor_node_move_viewpoint(nodeRef);

}

void Node::addForce(const double force[3], bool relative) {

wb_supervisor_node_add_force(nodeRef, force, relative);

}

void Node::addForceWithOffset(const double force[3], const double offset[3], bool relative) {

wb_supervisor_node_add_force_with_offset(nodeRef, force, offset, relative);

}

void Node::addTorque(const double torque[3], bool relative) {

wb_supervisor_node_add_torque(nodeRef, torque, relative);

}

void Node::saveState(const std::string &stateName) {

wb_supervisor_node_save_state(nodeRef, stateName.c_str());

}

void Node::loadState(const std::string &stateName) {

wb_supervisor_node_load_state(nodeRef, stateName.c_str());

}

void Node::setJointPosition(double position, int index) {

wb_supervisor_node_set_joint_position(nodeRef, position, index);

}