<commit_before>namespace bmi {

  const int BMI_SUCCESS = 0;
  const int BMI_FAILURE = 1;

  const int MAX_COMPONENT_NAME = 2048;
  const int MAX_VAR_NAME = 2048;
  const int MAX_UNITS_NAME = 2048;
  const int MAX_TYPE_NAME = 2048;

  class Bmi {
    public:
      // Model control functions.
      virtual void Initialize(std::string config_file) = 0;
      virtual void Update() = 0;
      virtual void UpdateUntil(double time) = 0;
      virtual void Finalize() = 0;

      // Model information functions.
      virtual void GetComponentName(char * const name) = 0;
      virtual int GetInputItemCount() = 0;
      virtual int GetOutputItemCount() = 0;
      virtual void GetInputVarNames(char **names) = 0;
      virtual void GetOutputVarNames(char **names) = 0;

      // Variable information functions
      virtual int GetVarGrid(const char *name) = 0;
      virtual void GetVarType(const char *name, char *type) = 0;
      virtual void GetVarUnits(const char *name, char *units) = 0;
      virtual int GetVarItemsize(const char *name) = 0;
      virtual int GetVarNbytes(const char *name) = 0;
      virtual void GetVarLocation(const char *name, char *location) = 0;

      virtual double GetCurrentTime() = 0;
      virtual double GetStartTime() = 0;
      virtual double GetEndTime() = 0;
      virtual void GetTimeUnits(char *units) = 0;
      virtual double GetTimeStep() = 0;

      // Variable getters
      virtual void GetValue(const char *name, void *dest) = 0;
      virtual void *GetValuePtr(const char *name) = 0;
      virtual void GetValueAtIndices(const char *name, void *dest, int *inds, int count) = 0;

      // Variable setters
      virtual void SetValue(const char *name, void *src) = 0;
      virtual void SetValueAtIndices(const char *name, int *inds, int count, void *src) = 0;

      // Grid information functions
      virtual int GetGridRank(const int grid) = 0;
      virtual int GetGridSize(const int grid) = 0;
      virtual void GetGridType(const int grid, char *type) = 0;

      virtual void GetGridShape(const int grid, int *shape) = 0;
      virtual void GetGridSpacing(const int grid, double *spacing) = 0;
      virtual void GetGridOrigin(const int grid, double *origin) = 0;

      virtual void GetGridX(const int grid, double *x) = 0;
      virtual void GetGridY(const int grid, double *y) = 0;
      virtual void GetGridZ(const int grid, double *z) = 0;

      virtual int GetGridNodeCount(const int grid) = 0;
      virtual int GetGridEdgeCount(const int grid) = 0;
      virtual int GetGridFaceCount(const int grid) = 0;

      virtual void GetGridEdgeNodes(const int grid, int *edge_nodes) = 0;
      virtual void GetGridFaceEdges(const int grid, int *face_edges) = 0;
      virtual void GetGridFaceNodes(const int grid, int *face_nodes) = 0;
      virtual void GetGridNodesPerFace(const int, int *nodes_per_face) = 0;
  };
}
<commit_msg>Use C++ string instead of char *<commit_after>namespace bmi {

  const int BMI_SUCCESS = 0;
  const int BMI_FAILURE = 1;

  const int MAX_COMPONENT_NAME = 2048;
  const int MAX_VAR_NAME = 2048;
  const int MAX_UNITS_NAME = 2048;
  const int MAX_TYPE_NAME = 2048;

  class Bmi {
    public:
      // Model control functions.
      virtual void Initialize(std::string config_file) = 0;
      virtual void Update() = 0;
      virtual void UpdateUntil(double time) = 0;
      virtual void Finalize() = 0;

      // Model information functions.
      virtual void GetComponentName(char * const name) = 0;
      virtual int GetInputItemCount() = 0;
      virtual int GetOutputItemCount() = 0;
      virtual void GetInputVarNames(char **names) = 0;
      virtual void GetOutputVarNames(char **names) = 0;

      // Variable information functions
      virtual int GetVarGrid(std::string name) = 0;
      virtual void GetVarType(std::string name, char *type) = 0;
      virtual void GetVarUnits(std::string name, char *units) = 0;
      virtual int GetVarItemsize(std::string name) = 0;
      virtual int GetVarNbytes(std::string name) = 0;
      virtual void GetVarLocation(std::string name, char *location) = 0;

      virtual double GetCurrentTime() = 0;
      virtual double GetStartTime() = 0;
      virtual double GetEndTime() = 0;
      virtual void GetTimeUnits(char *units) = 0;
      virtual double GetTimeStep() = 0;

      // Variable getters
      virtual void GetValue(std::string name, void *dest) = 0;
      virtual void *GetValuePtr(std::string name) = 0;
      virtual void GetValueAtIndices(std::string name, void *dest, int *inds, int count) = 0;

      // Variable setters
      virtual void SetValue(std::string name, void *src) = 0;
      virtual void SetValueAtIndices(std::string name, int *inds, int count, void *src) = 0;

      // Grid information functions
      virtual int GetGridRank(const int grid) = 0;
      virtual int GetGridSize(const int grid) = 0;
      virtual void GetGridType(const int grid, char *type) = 0;

      virtual void GetGridShape(const int grid, int *shape) = 0;
      virtual void GetGridSpacing(const int grid, double *spacing) = 0;
      virtual void GetGridOrigin(const int grid, double *origin) = 0;

      virtual void GetGridX(const int grid, double *x) = 0;
      virtual void GetGridY(const int grid, double *y) = 0;
      virtual void GetGridZ(const int grid, double *z) = 0;

      virtual int GetGridNodeCount(const int grid) = 0;
      virtual int GetGridEdgeCount(const int grid) = 0;
      virtual int GetGridFaceCount(const int grid) = 0;

      virtual void GetGridEdgeNodes(const int grid, int *edge_nodes) = 0;
      virtual void GetGridFaceEdges(const int grid, int *face_edges) = 0;
      virtual void GetGridFaceNodes(const int grid, int *face_nodes) = 0;
      virtual void GetGridNodesPerFace(const int, int *nodes_per_face) = 0;
  };
}
<|endoftext|>


<commit_before>/*
 * JSON.cpp
 *
 *  Created on: May 23, 2017
 *      Author: kolban
 */

// See: https://github.com/DaveGamble/cJSON

#include <string>
#include <stdlib.h>
#include "JSON.h"

/**
 * @brief Create an empty JSON array.
 * @return An empty JSON array.
 */
JsonArray JSON::createArray() {
	return JsonArray(cJSON_CreateArray());
} // createArray

/**
 * @brief Create an empty JSON object.
 * @return An empty JSON object.
 */
JsonObject JSON::createObject() {
	return JsonObject(cJSON_CreateObject());
} // createObject

/**
 * @brief Delete a JSON array.
 * @param [in] jsonArray The array to be deleted.
 * @return N/A.
 */
void JSON::deleteArray(JsonArray jsonArray) {
	cJSON_Delete(jsonArray.m_node);
} // deleteArray

/**
 * @brief Delete a JSON object.
 * @param [in] jsonObject The object to be deleted.
 */
void JSON::deleteObject(JsonObject jsonObject) {
	cJSON_Delete(jsonObject.m_node);
} // deleteObject

/**
 * @brief Parse a string that contains a JSON array.
 * @param [in] text The JSON text string.
 * @return A JSON array.
 */
JsonArray JSON::parseArray(std::string text) {
	return JsonArray(cJSON_Parse(text.c_str()));
} // parseArray

/**
 * @brief Parse a string that contains a JSON object.
 * @param [in] text The JSON text string.
 * @return a JSON object.
 */
JsonObject JSON::parseObject(std::string text) {
	return JsonObject(cJSON_Parse(text.c_str()));
} // parseObject

JsonArray::JsonArray(cJSON* node) {
	m_node = node;
}

/**
 * @brief Add a boolean value to the array.
 * @param [in] value The boolean value to add to the array.
 */
void JsonArray::addBoolean(bool value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateBool(value));
} // addBoolean

/**
 * @brief Add a double value to the array.
 * @param [in] value The double value to add to the array.
 */
void JsonArray::addDouble(double value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateNumber(value));
} // addDouble

/**
 * @brief Add an int value to the array.
 * @param [in] value The int value to add to the array.
 */
void JsonArray::addInt(int value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateDouble((double)value, value));
} // addInt

/**
 * @brief Add an object value to the array.
 * @param [in] value The object value to add to the array.
 */
void JsonArray::addObject(JsonObject value) {
	cJSON_AddItemToArray(m_node, value.m_node);
} // addObject

/**
 * @brief Add a string value to the array.
 * @param [in] value The string value to add to the array.
 */
void JsonArray::addString(std::string value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateString(value.c_str()));
} // addString

/**
 * @brief Get the indexed boolean value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The boolean value at the given index.
 */
bool JsonArray::getBoolean(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	if (node->valueint == 0) {
		return false;
	}
	return true;
} // getBoolean

/**
 * @brief Get the indexed double value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The double value at the given index.
 */
double JsonArray::getDouble(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return node->valuedouble;
} // getDouble

/**
 * @brief Get the indexed int value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The int value at the given index.
 */
int JsonArray::getInt(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return node->valueint;
} // getInt

/**
 * @brief Get the indexed object value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The object value at the given index.
 */
JsonObject JsonArray::getObject(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return JsonObject(node);
} // getObject

/**
 * @brief Get the indexed object value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The object value at the given index.
 */
std::string JsonArray::getString(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return std::string(node->valuestring);
} // getString

/**
 * @brief Convert the JSON array to a string.
 * @return A JSON string representation of the array.
 */
std::string JsonArray::toString() {
	char *data = cJSON_Print(m_node);
	std::string ret(data);
	free(data);
	return ret;
} // toString

/**
 * @brief Get the number of elements from the array.
 * @return The int value that represents the number of elements.
 */
std::size_t JsonArray::size() {
	return cJSON_GetArraySize(m_node);
} // size

/**
 * @brief Constructor
 */
JsonObject::JsonObject(cJSON* node) {
	m_node = node;
} // JsonObject

JsonArray JsonObject::getArray(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return JsonArray(node);
}

/**
 * @brief Get the named boolean value from the object.
 * @param [in] name The name of the object property.
 * @return The boolean value from the object.
 */
bool JsonObject::getBoolean(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	if (node->valueint == 0) {
		return false;
	}
	return true;
} // getBoolean

/**
 * @brief Get the named double value from the object.
 * @param [in] name The name of the object property.
 * @return The double value from the object.
 */
double JsonObject::getDouble(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return node->valuedouble;
} // getDouble

/**
 * @brief Get the named int value from the object.
 * @param [in] name The name of the object property.
 * @return The int value from the object.
 */
int JsonObject::getInt(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return node->valueint;
} // getInt

/**
 * @brief Get the named object value from the object.
 * @param [in] name The name of the object property.
 * @return The object value from the object.
 */
JsonObject JsonObject::getObject(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return JsonObject(node);
} // getObject

/**
 * @brief Get the named string value from the object.
 * @param [in] name The name of the object property.
 * @return The string value from the object.
 */
std::string JsonObject::getString(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return std::string(node->valuestring);
} // getString

/**
 * @brief Determine if the object has the specified item.
 * @param [in] name The name of the property to check for presence.
 * @return True if the object contains this property.
 */
bool JsonObject::hasItem(std::string name) {
	return cJSON_GetObjectItem(m_node, name.c_str()) != nullptr;
} // hasItem

/**
 * @brief Determine if this represents a valid JSON node.
 * @return True if this is a valid node and false otherwise.
 */
bool JsonObject::isValid() {
	return m_node != nullptr;
} // isValid

/**
 * @brief Set the named array property.
 * @param [in] name The name of the property to add.
 * @param [in] array The array to add to the object.
 * @return N/A.
 */
void JsonObject::setArray(std::string name, JsonArray array) {
	cJSON_AddItemToObject(m_node, name.c_str(), array.m_node);
} // setArray

/**
 * @brief Set the named boolean property.
 * @param [in] name The name of the property to add.
 * @param [in] value The boolean to add to the object.
 * @return N/A.
 */
void JsonObject::setBoolean(std::string name, bool value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateBool(value));
} // setBoolean

/**
 * @brief Set the named double property.
 * @param [in] name The name of the property to add.
 * @param [in] value The double to add to the object.
 * @return N/A.
 */
void JsonObject::setDouble(std::string name, double value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateNumber(value));
} // setDouble

/**
 * @brief Set the named int property.
 * @param [in] name The name of the property to add.
 * @param [in] value The int to add to the object.
 * @return N/A.
 */
void JsonObject::setInt(std::string name, int value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateDouble((double)value, value));
} // setInt

/**
 * @brief Set the named object property.
 * @param [in] name The name of the property to add.
 * @param [in] value The object to add to the object.
 * @return N/A.
 */
void JsonObject::setObject(std::string name, JsonObject value) {
	cJSON_AddItemToObject(m_node, name.c_str(), value.m_node);
} // setObject

/**
 * @brief Set the named string property.
 * @param [in] name The name of the property to add.
 * @param [in] value The string to add to the object.
 * @return N/A.
 */
void JsonObject::setString(std::string name, std::string value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateString(value.c_str()));
} // setString

/**
 * @brief Convert the JSON object to a string.
 * @return A JSON string representation of the object.
 */
std::string JsonObject::toString() {
	char *data = cJSON_Print(m_node);
	std::string ret(data);
	free(data);
	return ret;
} // toString

<commit_msg>Fixes for for #248<commit_after>/*
 * JSON.cpp
 *
 *  Created on: May 23, 2017
 *      Author: kolban
 */

// See: https://github.com/DaveGamble/cJSON

#include <string>
#include <stdlib.h>
#include "JSON.h"

/**
 * @brief Create an empty JSON array.
 * @return An empty JSON array.
 */
JsonArray JSON::createArray() {
	return JsonArray(cJSON_CreateArray());
} // createArray

/**
 * @brief Create an empty JSON object.
 * @return An empty JSON object.
 */
JsonObject JSON::createObject() {
	return JsonObject(cJSON_CreateObject());
} // createObject

/**
 * @brief Delete a JSON array.
 * @param [in] jsonArray The array to be deleted.
 * @return N/A.
 */
void JSON::deleteArray(JsonArray jsonArray) {
	cJSON_Delete(jsonArray.m_node);
} // deleteArray

/**
 * @brief Delete a JSON object.
 * @param [in] jsonObject The object to be deleted.
 */
void JSON::deleteObject(JsonObject jsonObject) {
	cJSON_Delete(jsonObject.m_node);
} // deleteObject

/**
 * @brief Parse a string that contains a JSON array.
 * @param [in] text The JSON text string.
 * @return A JSON array.
 */
JsonArray JSON::parseArray(std::string text) {
	return JsonArray(cJSON_Parse(text.c_str()));
} // parseArray

/**
 * @brief Parse a string that contains a JSON object.
 * @param [in] text The JSON text string.
 * @return a JSON object.
 */
JsonObject JSON::parseObject(std::string text) {
	return JsonObject(cJSON_Parse(text.c_str()));
} // parseObject

JsonArray::JsonArray(cJSON* node) {
	m_node = node;
}

/**
 * @brief Add a boolean value to the array.
 * @param [in] value The boolean value to add to the array.
 */
void JsonArray::addBoolean(bool value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateBool(value));
} // addBoolean

/**
 * @brief Add a double value to the array.
 * @param [in] value The double value to add to the array.
 */
void JsonArray::addDouble(double value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateNumber(value));
} // addDouble

/**
 * @brief Add an int value to the array.
 * @param [in] value The int value to add to the array.
 */
void JsonArray::addInt(int value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateNumber((double)value));
} // addInt

/**
 * @brief Add an object value to the array.
 * @param [in] value The object value to add to the array.
 */
void JsonArray::addObject(JsonObject value) {
	cJSON_AddItemToArray(m_node, value.m_node);
} // addObject

/**
 * @brief Add a string value to the array.
 * @param [in] value The string value to add to the array.
 */
void JsonArray::addString(std::string value) {
	cJSON_AddItemToArray(m_node, cJSON_CreateString(value.c_str()));
} // addString

/**
 * @brief Get the indexed boolean value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The boolean value at the given index.
 */
bool JsonArray::getBoolean(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	if (node->valueint == 0) {
		return false;
	}
	return true;
} // getBoolean

/**
 * @brief Get the indexed double value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The double value at the given index.
 */
double JsonArray::getDouble(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return node->valuedouble;
} // getDouble

/**
 * @brief Get the indexed int value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The int value at the given index.
 */
int JsonArray::getInt(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return node->valueint;
} // getInt

/**
 * @brief Get the indexed object value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The object value at the given index.
 */
JsonObject JsonArray::getObject(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return JsonObject(node);
} // getObject

/**
 * @brief Get the indexed object value from the array.
 * @param [in] item The index of the array to retrieve.
 * @return The object value at the given index.
 */
std::string JsonArray::getString(int item) {
	cJSON *node = cJSON_GetArrayItem(m_node, item);
	return std::string(node->valuestring);
} // getString

/**
 * @brief Convert the JSON array to a string.
 * @return A JSON string representation of the array.
 */
std::string JsonArray::toString() {
	char *data = cJSON_Print(m_node);
	std::string ret(data);
	free(data);
	return ret;
} // toString

/**
 * @brief Get the number of elements from the array.
 * @return The int value that represents the number of elements.
 */
std::size_t JsonArray::size() {
	return cJSON_GetArraySize(m_node);
} // size

/**
 * @brief Constructor
 */
JsonObject::JsonObject(cJSON* node) {
	m_node = node;
} // JsonObject

JsonArray JsonObject::getArray(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return JsonArray(node);
}

/**
 * @brief Get the named boolean value from the object.
 * @param [in] name The name of the object property.
 * @return The boolean value from the object.
 */
bool JsonObject::getBoolean(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	if (node->valueint == 0) {
		return false;
	}
	return true;
} // getBoolean

/**
 * @brief Get the named double value from the object.
 * @param [in] name The name of the object property.
 * @return The double value from the object.
 */
double JsonObject::getDouble(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return node->valuedouble;
} // getDouble

/**
 * @brief Get the named int value from the object.
 * @param [in] name The name of the object property.
 * @return The int value from the object.
 */
int JsonObject::getInt(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return node->valueint;
} // getInt

/**
 * @brief Get the named object value from the object.
 * @param [in] name The name of the object property.
 * @return The object value from the object.
 */
JsonObject JsonObject::getObject(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return JsonObject(node);
} // getObject

/**
 * @brief Get the named string value from the object.
 * @param [in] name The name of the object property.
 * @return The string value from the object.
 */
std::string JsonObject::getString(std::string name) {
	cJSON *node = cJSON_GetObjectItem(m_node, name.c_str());
	return std::string(node->valuestring);
} // getString

/**
 * @brief Determine if the object has the specified item.
 * @param [in] name The name of the property to check for presence.
 * @return True if the object contains this property.
 */
bool JsonObject::hasItem(std::string name) {
	return cJSON_GetObjectItem(m_node, name.c_str()) != nullptr;
} // hasItem

/**
 * @brief Determine if this represents a valid JSON node.
 * @return True if this is a valid node and false otherwise.
 */
bool JsonObject::isValid() {
	return m_node != nullptr;
} // isValid

/**
 * @brief Set the named array property.
 * @param [in] name The name of the property to add.
 * @param [in] array The array to add to the object.
 * @return N/A.
 */
void JsonObject::setArray(std::string name, JsonArray array) {
	cJSON_AddItemToObject(m_node, name.c_str(), array.m_node);
} // setArray

/**
 * @brief Set the named boolean property.
 * @param [in] name The name of the property to add.
 * @param [in] value The boolean to add to the object.
 * @return N/A.
 */
void JsonObject::setBoolean(std::string name, bool value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateBool(value));
} // setBoolean

/**
 * @brief Set the named double property.
 * @param [in] name The name of the property to add.
 * @param [in] value The double to add to the object.
 * @return N/A.
 */
void JsonObject::setDouble(std::string name, double value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateNumber(value));
} // setDouble

/**
 * @brief Set the named int property.
 * @param [in] name The name of the property to add.
 * @param [in] value The int to add to the object.
 * @return N/A.
 */
void JsonObject::setInt(std::string name, int value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateNumber((double)value));
} // setInt

/**
 * @brief Set the named object property.
 * @param [in] name The name of the property to add.
 * @param [in] value The object to add to the object.
 * @return N/A.
 */
void JsonObject::setObject(std::string name, JsonObject value) {
	cJSON_AddItemToObject(m_node, name.c_str(), value.m_node);
} // setObject

/**
 * @brief Set the named string property.
 * @param [in] name The name of the property to add.
 * @param [in] value The string to add to the object.
 * @return N/A.
 */
void JsonObject::setString(std::string name, std::string value) {
	cJSON_AddItemToObject(m_node, name.c_str(), cJSON_CreateString(value.c_str()));
} // setString

/**
 * @brief Convert the JSON object to a string.
 * @return A JSON string representation of the object.
 */
std::string JsonObject::toString() {
	char *data = cJSON_Print(m_node);
	std::string ret(data);
	free(data);
	return ret;
} // toString

<|endoftext|>


<commit_before>/**
 * Copyright (c) 2014, Timothy Stack
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 * * Neither the name of Timothy Stack nor the names of its contributors
 * may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TIMOTHY STACK AND CONTRIBUTORS ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * @file json_ptr.hh
 */

#ifndef __json_ptr_hh
#define __json_ptr_hh

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <sys/types.h>

#include <string>
#include <vector>

#include "yajlpp.hh"
#include "lnav_log.hh"

class json_ptr_walk {
public:
    const static yajl_callbacks callbacks;

    json_ptr_walk() : jpw_handle(yajl_free) {
        this->jpw_handle = yajl_alloc(&callbacks, NULL, this);
    };

    yajl_status parse(const char *buffer, ssize_t len) {
        yajl_status retval;

        retval = yajl_parse(this->jpw_handle, (const unsigned char *)buffer, len);
        this->update_error_msg(retval, buffer, len);
        return retval;
    };

    yajl_status complete_parse() {
        yajl_status retval;

        retval = yajl_complete_parse(this->jpw_handle);
        this->update_error_msg(retval, NULL, -1);
        return retval;
    };

    void update_error_msg(yajl_status status, const char *buffer, ssize_t len) {
        switch (status) {
        case yajl_status_ok:
            break;
        case yajl_status_client_canceled:
            this->jpw_error_msg = "internal error";
            break;
        case yajl_status_error:
            this->jpw_error_msg = std::string((const char *)yajl_get_error(
                this->jpw_handle, 1, (const unsigned char *)buffer, len));
            break;
        }
    };

    void clear() {
        this->jpw_values.clear();
    };

    void inc_array_index() {
        if (!this->jpw_array_indexes.empty() &&
            this->jpw_array_indexes.back() != -1) {
            this->jpw_array_indexes.back() += 1;
        }
    };

    std::string current_ptr() {
        std::string retval;

        for (int lpc = 0; lpc < this->jpw_array_indexes.size(); lpc++) {
            retval.append("/");
            if (this->jpw_array_indexes[lpc] == -1) {
                retval.append(this->jpw_keys[lpc]);
            }
            else {
                char num[64];

                snprintf(num, sizeof(num), "%d", this->jpw_array_indexes[lpc]);
                retval.append(num);
            }
        }

        return retval;
    };

    typedef std::vector<std::pair<std::string, std::string> > pair_list_t;

    auto_mem<yajl_handle_t> jpw_handle;
    std::string jpw_error_msg;
    pair_list_t jpw_values;
    std::vector<std::string> jpw_keys;
    std::vector<int32_t> jpw_array_indexes;
};

class json_ptr {
public:
    enum match_state_t {
        MS_DONE,
        MS_VALUE,
        MS_ELEMENT,
        MS_ERR_INVALID_TYPE,
        MS_ERR_NO_SLASH,
        MS_ERR_INVALID_ESCAPE,
        MS_ERR_INVALID_INDEX,
    };

    static size_t encode(char *dst, size_t dst_len, const char *src, size_t src_len = -1) {
        size_t retval = 0;

        if (src_len == -1) {
            src_len = strlen(src);
        }

        for (size_t lpc = 0; lpc < src_len; lpc++) {
            switch (src[lpc]) {
            case '/':
            case '~':
                if (retval < dst_len) {
                    dst[retval] = '~';
                    retval += 1;
                    if (src[lpc] == '~') {
                        dst[retval] = '0';
                    }
                    else {
                        dst[retval] = '1';
                    }
                }
                else {
                    retval += 1;
                }
                break;
            default:
                if (retval < dst_len) {
                    dst[retval] = src[lpc];
                }
                break;
            }
            retval += 1;
        }

        return retval;
    };

    json_ptr(const char *value)
        : jp_value(value), jp_pos(value), jp_depth(0), jp_array_index(-1),
          jp_state(MS_VALUE) {

    };

    bool expect_map(int32_t &depth) {
        bool retval;

        if (this->jp_state == MS_DONE) {
            retval = true;
        }
        else if (depth != this->jp_depth) {
            retval = true;
        }
        else if (this->reached_end()) {
            retval = true;
        }
        else if (this->jp_state == MS_VALUE) {
           if (this->jp_pos[0] == '/') {
               this->jp_pos += 1;
               this->jp_depth += 1;
               this->jp_state = MS_ELEMENT;
               retval = true;
           }
           else {
               this->jp_state = MS_ERR_NO_SLASH;
               retval = false;
           }
        }
        else {
            retval = true;
        }
        depth += 1;

        return retval;
    };

    bool at_key(int32_t depth, const char *component, ssize_t len = -1) {
        const char *component_end;
        int lpc;

        if (this->jp_state == MS_DONE || depth != this->jp_depth) {
            return true;
        }

        if (len == -1) {
            len = strlen(component);
        }
        component_end = component + len;

        for (lpc = 0; component < component_end; lpc++, component++) {
            char ch = this->jp_pos[lpc];

            if (this->jp_pos[lpc] == '~') {
                switch (this->jp_pos[lpc + 1]) {
                case '0':
                    ch = '~';
                    break;
                case '1':
                    ch = '/';
                    break;
                default:
                    this->jp_state = MS_ERR_INVALID_ESCAPE;
                    return false;
                }
                lpc += 1;
            }
            else if (this->jp_pos[lpc] == '/') {
                ch = '\0';
            }

            if (ch != *component) {
                return true;
            }
        }

        this->jp_pos += lpc;
        this->jp_state = MS_VALUE;

        return true;
    };

    void exit_container(int32_t &depth, int32_t &index) {
        depth -= 1;
        if (this->jp_state == MS_VALUE && depth == this->jp_depth) {
            this->jp_state = MS_DONE;
            index = -1;
        }
    };

    bool expect_array(int32_t &depth, int32_t &index) {
        bool retval;

        if (this->jp_state == MS_DONE) {
            retval = true;
        }
        else if (depth != this->jp_depth) {
            retval = true;
        }
        else if (this->reached_end()) {
            retval = true;
        }
        else if (this->jp_pos[0] == '/') {
            int offset;

            this->jp_depth += 1;

            if (sscanf(this->jp_pos, "/%d%n", &this->jp_array_index, &offset) != 1) {
                this->jp_state = MS_ERR_INVALID_INDEX;
                retval = false;
            }
            else if (this->jp_pos[offset] != '\0' && this->jp_pos[offset] != '/') {
                this->jp_state = MS_ERR_INVALID_INDEX;
                retval = false;
            }
            else {
                index = 0;
                this->jp_pos += offset;
                this->jp_state = MS_ELEMENT;
                retval = true;
            }
        }
        else {
            this->jp_state = MS_ERR_NO_SLASH;
            retval = false;
        }

        depth += 1;

        return retval;
    };

    bool at_index(int32_t &depth, int32_t &index, bool primitive = true) {
        bool retval;

        if (this->jp_state == MS_DONE) {
            retval = false;
        }
        else if (depth < this->jp_depth) {
            retval = false;
        }
        else if (depth == this->jp_depth) {
            if (index == -1) {
                if (this->jp_array_index == -1) {
                    retval = this->reached_end();
                    if (primitive && retval) {
                        this->jp_state = MS_DONE;
                    }
                }
                else {
                    retval = false;
                }
            }
            else if (index == this->jp_array_index) {
                retval = this->reached_end();
                index = -1;
                if (primitive && retval) {
                    this->jp_state = MS_DONE;
                }
            }
            else {
                index += 1;
                retval = false;
            }
        }
        else if (index == -1) {
            retval = this->reached_end();
        }
        else {
            retval = false;
        }

        return retval;
    };

    bool reached_end() {
        return this->jp_pos[0] == '\0';
    };

    std::string error_msg() const {
        std::string retval;

        switch (this->jp_state) {
        case MS_ERR_INVALID_ESCAPE:
            retval = ("invalid escape sequence near -- " + std::string(this->jp_pos));
            break;
        case MS_ERR_INVALID_INDEX:
            retval = ("expecting array index at -- " + std::string(this->jp_pos));
            break;
        case MS_ERR_INVALID_TYPE:
            retval = ("expecting container at -- " + std::string(
                this->jp_value, this->jp_pos - this->jp_value));
            break;
        default:
            break;
        }

        return retval;
    };

    const char *jp_value;
    const char *jp_pos;
    int32_t jp_depth;
    int32_t jp_array_index;
    match_state_t jp_state;
};

#endif
<commit_msg>[-Wsign-compare] json_ptr.hh<commit_after>/**
 * Copyright (c) 2014, Timothy Stack
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 * * Neither the name of Timothy Stack nor the names of its contributors
 * may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TIMOTHY STACK AND CONTRIBUTORS ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * @file json_ptr.hh
 */

#ifndef __json_ptr_hh
#define __json_ptr_hh

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <sys/types.h>

#include <string>
#include <vector>

#include "yajlpp.hh"
#include "lnav_log.hh"

class json_ptr_walk {
public:
    const static yajl_callbacks callbacks;

    json_ptr_walk() : jpw_handle(yajl_free) {
        this->jpw_handle = yajl_alloc(&callbacks, NULL, this);
    };

    yajl_status parse(const char *buffer, ssize_t len) {
        yajl_status retval;

        retval = yajl_parse(this->jpw_handle, (const unsigned char *)buffer, len);
        this->update_error_msg(retval, buffer, len);
        return retval;
    };

    yajl_status complete_parse() {
        yajl_status retval;

        retval = yajl_complete_parse(this->jpw_handle);
        this->update_error_msg(retval, NULL, -1);
        return retval;
    };

    void update_error_msg(yajl_status status, const char *buffer, ssize_t len) {
        switch (status) {
        case yajl_status_ok:
            break;
        case yajl_status_client_canceled:
            this->jpw_error_msg = "internal error";
            break;
        case yajl_status_error:
            this->jpw_error_msg = std::string((const char *)yajl_get_error(
                this->jpw_handle, 1, (const unsigned char *)buffer, len));
            break;
        }
    };

    void clear() {
        this->jpw_values.clear();
    };

    void inc_array_index() {
        if (!this->jpw_array_indexes.empty() &&
            this->jpw_array_indexes.back() != -1) {
            this->jpw_array_indexes.back() += 1;
        }
    };

    std::string current_ptr() {
        std::string retval;

        for (size_t lpc = 0; lpc < this->jpw_array_indexes.size(); lpc++) {
            retval.append("/");
            if (this->jpw_array_indexes[lpc] == -1) {
                retval.append(this->jpw_keys[lpc]);
            }
            else {
                char num[64];

                snprintf(num, sizeof(num), "%d", this->jpw_array_indexes[lpc]);
                retval.append(num);
            }
        }

        return retval;
    };

    typedef std::vector<std::pair<std::string, std::string> > pair_list_t;

    auto_mem<yajl_handle_t> jpw_handle;
    std::string jpw_error_msg;
    pair_list_t jpw_values;
    std::vector<std::string> jpw_keys;
    std::vector<int32_t> jpw_array_indexes;
};

class json_ptr {
public:
    enum match_state_t {
        MS_DONE,
        MS_VALUE,
        MS_ELEMENT,
        MS_ERR_INVALID_TYPE,
        MS_ERR_NO_SLASH,
        MS_ERR_INVALID_ESCAPE,
        MS_ERR_INVALID_INDEX,
    };

    static size_t encode(char *dst, size_t dst_len, const char *src, size_t src_len = -1) {
        size_t retval = 0;

        if (src_len == (size_t)-1) {
            src_len = strlen(src);
        }

        for (size_t lpc = 0; lpc < src_len; lpc++) {
            switch (src[lpc]) {
            case '/':
            case '~':
                if (retval < dst_len) {
                    dst[retval] = '~';
                    retval += 1;
                    if (src[lpc] == '~') {
                        dst[retval] = '0';
                    }
                    else {
                        dst[retval] = '1';
                    }
                }
                else {
                    retval += 1;
                }
                break;
            default:
                if (retval < dst_len) {
                    dst[retval] = src[lpc];
                }
                break;
            }
            retval += 1;
        }

        return retval;
    };

    json_ptr(const char *value)
        : jp_value(value), jp_pos(value), jp_depth(0), jp_array_index(-1),
          jp_state(MS_VALUE) {

    };

    bool expect_map(int32_t &depth) {
        bool retval;

        if (this->jp_state == MS_DONE) {
            retval = true;
        }
        else if (depth != this->jp_depth) {
            retval = true;
        }
        else if (this->reached_end()) {
            retval = true;
        }
        else if (this->jp_state == MS_VALUE) {
           if (this->jp_pos[0] == '/') {
               this->jp_pos += 1;
               this->jp_depth += 1;
               this->jp_state = MS_ELEMENT;
               retval = true;
           }
           else {
               this->jp_state = MS_ERR_NO_SLASH;
               retval = false;
           }
        }
        else {
            retval = true;
        }
        depth += 1;

        return retval;
    };

    bool at_key(int32_t depth, const char *component, ssize_t len = -1) {
        const char *component_end;
        int lpc;

        if (this->jp_state == MS_DONE || depth != this->jp_depth) {
            return true;
        }

        if (len == -1) {
            len = strlen(component);
        }
        component_end = component + len;

        for (lpc = 0; component < component_end; lpc++, component++) {
            char ch = this->jp_pos[lpc];

            if (this->jp_pos[lpc] == '~') {
                switch (this->jp_pos[lpc + 1]) {
                case '0':
                    ch = '~';
                    break;
                case '1':
                    ch = '/';
                    break;
                default:
                    this->jp_state = MS_ERR_INVALID_ESCAPE;
                    return false;
                }
                lpc += 1;
            }
            else if (this->jp_pos[lpc] == '/') {
                ch = '\0';
            }

            if (ch != *component) {
                return true;
            }
        }

        this->jp_pos += lpc;
        this->jp_state = MS_VALUE;

        return true;
    };

    void exit_container(int32_t &depth, int32_t &index) {
        depth -= 1;
        if (this->jp_state == MS_VALUE && depth == this->jp_depth) {
            this->jp_state = MS_DONE;
            index = -1;
        }
    };

    bool expect_array(int32_t &depth, int32_t &index) {
        bool retval;

        if (this->jp_state == MS_DONE) {
            retval = true;
        }
        else if (depth != this->jp_depth) {
            retval = true;
        }
        else if (this->reached_end()) {
            retval = true;
        }
        else if (this->jp_pos[0] == '/') {
            int offset;

            this->jp_depth += 1;

            if (sscanf(this->jp_pos, "/%d%n", &this->jp_array_index, &offset) != 1) {
                this->jp_state = MS_ERR_INVALID_INDEX;
                retval = false;
            }
            else if (this->jp_pos[offset] != '\0' && this->jp_pos[offset] != '/') {
                this->jp_state = MS_ERR_INVALID_INDEX;
                retval = false;
            }
            else {
                index = 0;
                this->jp_pos += offset;
                this->jp_state = MS_ELEMENT;
                retval = true;
            }
        }
        else {
            this->jp_state = MS_ERR_NO_SLASH;
            retval = false;
        }

        depth += 1;

        return retval;
    };

    bool at_index(int32_t &depth, int32_t &index, bool primitive = true) {
        bool retval;

        if (this->jp_state == MS_DONE) {
            retval = false;
        }
        else if (depth < this->jp_depth) {
            retval = false;
        }
        else if (depth == this->jp_depth) {
            if (index == -1) {
                if (this->jp_array_index == -1) {
                    retval = this->reached_end();
                    if (primitive && retval) {
                        this->jp_state = MS_DONE;
                    }
                }
                else {
                    retval = false;
                }
            }
            else if (index == this->jp_array_index) {
                retval = this->reached_end();
                index = -1;
                if (primitive && retval) {
                    this->jp_state = MS_DONE;
                }
            }
            else {
                index += 1;
                retval = false;
            }
        }
        else if (index == -1) {
            retval = this->reached_end();
        }
        else {
            retval = false;
        }

        return retval;
    };

    bool reached_end() {
        return this->jp_pos[0] == '\0';
    };

    std::string error_msg() const {
        std::string retval;

        switch (this->jp_state) {
        case MS_ERR_INVALID_ESCAPE:
            retval = ("invalid escape sequence near -- " + std::string(this->jp_pos));
            break;
        case MS_ERR_INVALID_INDEX:
            retval = ("expecting array index at -- " + std::string(this->jp_pos));
            break;
        case MS_ERR_INVALID_TYPE:
            retval = ("expecting container at -- " + std::string(
                this->jp_value, this->jp_pos - this->jp_value));
            break;
        default:
            break;
        }

        return retval;
    };

    const char *jp_value;
    const char *jp_pos;
    int32_t jp_depth;
    int32_t jp_array_index;
    match_state_t jp_state;
};

#endif
<|endoftext|>


<commit_before><commit_msg>Rename variables in function declarations.<commit_after><|endoftext|>


<commit_before><commit_msg>Bugfix: `Biont`: initialize `symbiont_species`.<commit_after><|endoftext|>


<commit_before><commit_msg>Edited whitespace.<commit_after><|endoftext|>


<commit_before>#include "rapid_pbd/world.h"

#include <math.h>
#include <string>
#include <vector>

#include "rapid_pbd_msgs/Action.h"
#include "rapid_pbd_msgs/Landmark.h"
#include "rapid_pbd_msgs/Program.h"
#include "rapid_pbd_msgs/Step.h"
#include "transform_graph/graph.h"

#include "rapid_pbd/joint_state.h"
#include "rapid_pbd/robot_config.h"

#include "ros/ros.h"

namespace msgs = rapid_pbd_msgs;

namespace rapid {
namespace pbd {
void GetWorld(const RobotConfig& robot_config, const msgs::Program& program,
              size_t step_id, World* world) {
  world->scene_id = "";
  JointState js(program.start_joint_state);
  world->joint_state = js;
  world->surface_box_landmarks.clear();

  // TODO: If this gets noticeably slow, change it so that it searches backward
  // instead of simulating forward. The channels to search are:
  // - Gripper action / actuator group
  // - Joint/Cartesian arm action / actuator group
  // - Scene ID
  // - Landmarks
  for (size_t step_i = 0; step_i <= step_id; ++step_i) {
    if (program.steps.size() == 0) {
      break;
    }
    const msgs::Step& step = program.steps[step_i];
    if (step.scene_id != "") {
      world->scene_id = step.scene_id;
    }

    for (size_t action_i = 0; action_i < step.actions.size(); ++action_i) {
      const msgs::Action& action = step.actions[action_i];
      if (action.type == msgs::Action::ACTUATE_GRIPPER) {
        std::vector<std::string> joint_names;
        robot_config.gripper_joints_for_group(action.actuator_group,
                                              &joint_names);
        std::vector<double> positions;
        if (action.gripper_command.position > 0.0001) {
          robot_config.gripper_open_positions(&positions);
        } else {
          robot_config.gripper_close_positions(&positions);
        }
        for (size_t i = 0; i < joint_names.size(); ++i) {
          world->joint_state.SetPosition(joint_names[i], positions[i]);
        }
      } else if (action.type == msgs::Action::MOVE_TO_JOINT_GOAL) {
        const trajectory_msgs::JointTrajectory& trajectory =
            action.joint_trajectory;
        if (trajectory.points.size() == 0) {
          continue;
        }
        for (size_t i = 0; i < trajectory.joint_names.size(); ++i) {
          const std::string& name = trajectory.joint_names[i];
          double position = trajectory.points[0].positions[i];
          world->joint_state.SetPosition(name, position);
        }
      } else if (action.type == msgs::Action::MOVE_TO_CARTESIAN_GOAL) {
        std::vector<std::string> joint_names;
        std::vector<double> joint_values;

        transform_graph::Graph graph;
        graph.Add("landmark",
                  transform_graph::RefFrame(robot_config.base_link()),
                  action.landmark.pose_stamped.pose);
        graph.Add("ee", transform_graph::RefFrame("landmark"), action.pose);
        transform_graph::Transform ee_in_base;
        graph.ComputeDescription(
            transform_graph::LocalFrame("ee"),
            transform_graph::RefFrame(robot_config.base_link()), &ee_in_base);
        geometry_msgs::Pose pose;
        ee_in_base.ToPose(&pose);
        bool success = robot_config.ComputeIk(action.actuator_group, pose,
                                              &joint_names, &joint_values);
        if (!success) {
          ROS_ERROR_STREAM("Failed to compute IK for actuator "
                           << action.actuator_group << "on step" << step_i
                           << ", action " << action_i << ", pose: " << pose);
          continue;
        }
        for (size_t j = 0; j < joint_names.size(); ++j) {
          const std::string& name = joint_names[j];
          double position = joint_values[j];
          world->joint_state.SetPosition(name, position);
        }
      }
    }

    std::vector<msgs::Landmark> surface_boxes;
    for (size_t landmark_i = 0; landmark_i < step.landmarks.size();
         ++landmark_i) {
      const msgs::Landmark& landmark = step.landmarks[landmark_i];
      if (landmark.type == msgs::Landmark::SURFACE_BOX) {
        surface_boxes.push_back(landmark);
      }
      // Deal with other landmark types here
    }
    // Replace all surface objects if there are any.
    if (surface_boxes.size() > 0) {
      world->surface_box_landmarks = surface_boxes;
    }
  }
}

namespace {
// Returns dimensions as a vector, with x always <= y.
void GetDims(const msgs::Landmark& landmark, std::vector<double>* dims) {
  dims->resize(3);
  dims->at(0) =
      std::min(landmark.surface_box_dims.x, landmark.surface_box_dims.y);
  dims->at(1) =
      std::max(landmark.surface_box_dims.x, landmark.surface_box_dims.y);
  dims->at(2) = landmark.surface_box_dims.z;
}

// Returns squared norm of vectors a and b.
double BoxDissimilarity(const std::vector<double>& a,
                        const std::vector<double>& b) {
  double dx = (a[0] - b[0]);
  double dy = (a[1] - b[1]);
  double dz = (a[2] - b[2]);
  return dx * dx + dy * dy + dz * dz;
}
}

bool MatchLandmark(const World& world, const rapid_pbd_msgs::Landmark& landmark,
                   rapid_pbd_msgs::Landmark* match) {
  const double kMaxDistance = 0.075 * 0.075;
  std::vector<double> landmark_dims;
  GetDims(landmark, &landmark_dims);
  if (landmark.type == msgs::Landmark::SURFACE_BOX) {
    double best = std::numeric_limits<double>::max();
    for (size_t i = 0; i < world.surface_box_landmarks.size(); ++i) {
      const msgs::Landmark& world_landmark = world.surface_box_landmarks[i];
      std::vector<double> world_landmark_dims;
      GetDims(world_landmark, &world_landmark_dims);
      double distance = BoxDissimilarity(landmark_dims, world_landmark_dims);
      if (distance < best) {
        best = distance;
        *match = world_landmark;
      }
    }
    return best <= kMaxDistance;
  } else {
    return false;
  }
}
}  // namespace pbd
}  // namespace rapid
<commit_msg>Get IK using service.<commit_after>#include "rapid_pbd/world.h"

#include <math.h>
#include <string>
#include <vector>

#include "moveit_msgs/GetPositionIK.h"
#include "rapid_pbd_msgs/Action.h"
#include "rapid_pbd_msgs/Landmark.h"
#include "rapid_pbd_msgs/Program.h"
#include "rapid_pbd_msgs/Step.h"
#include "transform_graph/graph.h"

#include "rapid_pbd/joint_state.h"
#include "rapid_pbd/robot_config.h"

#include "ros/ros.h"

namespace msgs = rapid_pbd_msgs;

namespace rapid {
namespace pbd {
void GetWorld(const RobotConfig& robot_config, const msgs::Program& program,
              size_t step_id, World* world) {
  world->scene_id = "";
  JointState js(program.start_joint_state);
  world->joint_state = js;
  world->surface_box_landmarks.clear();

  // TODO: If this gets noticeably slow, change it so that it searches backward
  // instead of simulating forward. The channels to search are:
  // - Gripper action / actuator group
  // - Joint/Cartesian arm action / actuator group
  // - Scene ID
  // - Landmarks
  for (size_t step_i = 0; step_i <= step_id; ++step_i) {
    if (program.steps.size() == 0) {
      break;
    }
    const msgs::Step& step = program.steps[step_i];
    if (step.scene_id != "") {
      world->scene_id = step.scene_id;
    }

    for (size_t action_i = 0; action_i < step.actions.size(); ++action_i) {
      const msgs::Action& action = step.actions[action_i];
      if (action.type == msgs::Action::ACTUATE_GRIPPER) {
        std::vector<std::string> joint_names;
        robot_config.gripper_joints_for_group(action.actuator_group,
                                              &joint_names);
        std::vector<double> positions;
        if (action.gripper_command.position > 0.0001) {
          robot_config.gripper_open_positions(&positions);
        } else {
          robot_config.gripper_close_positions(&positions);
        }
        for (size_t i = 0; i < joint_names.size(); ++i) {
          world->joint_state.SetPosition(joint_names[i], positions[i]);
        }
      } else if (action.type == msgs::Action::MOVE_TO_JOINT_GOAL) {
        const trajectory_msgs::JointTrajectory& trajectory =
            action.joint_trajectory;
        if (trajectory.points.size() == 0) {
          continue;
        }
        for (size_t i = 0; i < trajectory.joint_names.size(); ++i) {
          const std::string& name = trajectory.joint_names[i];
          double position = trajectory.points[0].positions[i];
          world->joint_state.SetPosition(name, position);
        }
      } else if (action.type == msgs::Action::MOVE_TO_CARTESIAN_GOAL) {
        transform_graph::Graph graph;
        graph.Add("landmark",
                  transform_graph::RefFrame(robot_config.base_link()),
                  action.landmark.pose_stamped.pose);
        graph.Add("ee", transform_graph::RefFrame("landmark"), action.pose);
        transform_graph::Transform ee_in_base;
        graph.ComputeDescription(
            transform_graph::LocalFrame("ee"),
            transform_graph::RefFrame(robot_config.base_link()), &ee_in_base);
        geometry_msgs::Pose pose;
        ee_in_base.ToPose(&pose);

        // TODO: This looks pretty inefficient but doesn't "feel" slow yet.
        // Could use a rewrite if it does feel slow.
        moveit_msgs::GetPositionIKRequest ik_req;
        if (action.actuator_group == msgs::Action::ARM) {
          ik_req.ik_request.group_name = "arm";
        } else if (action.actuator_group == msgs::Action::LEFT_ARM) {
          ik_req.ik_request.group_name = "left_arm";
        } else if (action.actuator_group == msgs::Action::RIGHT_ARM) {
          ik_req.ik_request.group_name = "right_arm";
        }
        ik_req.ik_request.pose_stamped.header.frame_id =
            robot_config.base_link();
        ik_req.ik_request.pose_stamped.pose = pose;
        ik_req.ik_request.attempts = 3;
        ik_req.ik_request.timeout = ros::Duration(1);
        robot_config.joints_for_group(action.actuator_group,
                                      &ik_req.ik_request.ik_link_names);

        moveit_msgs::GetPositionIKResponse ik_res;
        ros::service::call("/compute_ik", ik_req, ik_res);
        bool success =
            ik_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS;

        if (!success) {
          ROS_ERROR_STREAM("Failed to compute IK for actuator "
                           << action.actuator_group << "on step" << step_i
                           << ", action " << action_i << ", pose: " << pose);
          continue;
        }

        std::vector<std::string> joint_names;
        robot_config.joints_for_group(action.actuator_group, &joint_names);
        std::set<std::string> joint_set;
        joint_set.insert(joint_names.begin(), joint_names.end());
        for (size_t j = 0; j < ik_res.solution.joint_state.name.size(); ++j) {
          const std::string& name = ik_res.solution.joint_state.name[j];
          const double value = ik_res.solution.joint_state.position[j];
          if (joint_set.find(name) != joint_set.end()) {
            world->joint_state.SetPosition(name, value);
          }
        }
      }
    }

    std::vector<msgs::Landmark> surface_boxes;
    for (size_t landmark_i = 0; landmark_i < step.landmarks.size();
         ++landmark_i) {
      const msgs::Landmark& landmark = step.landmarks[landmark_i];
      if (landmark.type == msgs::Landmark::SURFACE_BOX) {
        surface_boxes.push_back(landmark);
      }
      // Deal with other landmark types here
    }
    // Replace all surface objects if there are any.
    if (surface_boxes.size() > 0) {
      world->surface_box_landmarks = surface_boxes;
    }
  }
}

namespace {
// Returns dimensions as a vector, with x always <= y.
void GetDims(const msgs::Landmark& landmark, std::vector<double>* dims) {
  dims->resize(3);
  dims->at(0) =
      std::min(landmark.surface_box_dims.x, landmark.surface_box_dims.y);
  dims->at(1) =
      std::max(landmark.surface_box_dims.x, landmark.surface_box_dims.y);
  dims->at(2) = landmark.surface_box_dims.z;
}

// Returns squared norm of vectors a and b.
double BoxDissimilarity(const std::vector<double>& a,
                        const std::vector<double>& b) {
  double dx = (a[0] - b[0]);
  double dy = (a[1] - b[1]);
  double dz = (a[2] - b[2]);
  return dx * dx + dy * dy + dz * dz;
}
}

bool MatchLandmark(const World& world, const rapid_pbd_msgs::Landmark& landmark,
                   rapid_pbd_msgs::Landmark* match) {
  const double kMaxDistance = 0.075 * 0.075;
  std::vector<double> landmark_dims;
  GetDims(landmark, &landmark_dims);
  if (landmark.type == msgs::Landmark::SURFACE_BOX) {
    double best = std::numeric_limits<double>::max();
    for (size_t i = 0; i < world.surface_box_landmarks.size(); ++i) {
      const msgs::Landmark& world_landmark = world.surface_box_landmarks[i];
      std::vector<double> world_landmark_dims;
      GetDims(world_landmark, &world_landmark_dims);
      double distance = BoxDissimilarity(landmark_dims, world_landmark_dims);
      if (distance < best) {
        best = distance;
        *match = world_landmark;
      }
    }
    return best <= kMaxDistance;
  } else {
    return false;
  }
}
}  // namespace pbd
}  // namespace rapid
<|endoftext|>


<commit_before>/*****************************************************************************\
 *                        ANALYSIS PERFORMANCE TOOLS                         *
 *                               libparaver-api                              *
 *                       Paraver Main Computing Library                      *
 *****************************************************************************
 *     ___     This library is free software; you can redistribute it and/or *
 *    /  __         modify it under the terms of the GNU LGPL as published   *
 *   /  /  _____    by the Free Software Foundation; either version 2.1      *
 *  /  /  /     \   of the License, or (at your option) any later version.   *
 * (  (  ( B S C )                                                           *
 *  \  \  \_____/   This library is distributed in hope that it will be      *
 *   \  \__         useful but WITHOUT ANY WARRANTY; without even the        *
 *    \___          implied warranty of MERCHANTABILITY or FITNESS FOR A     *
 *                  PARTICULAR PURPOSE. See the GNU LGPL for more details.   *
 *                                                                           *
 * You should have received a copy of the GNU Lesser General Public License  *
 * along with this library; if not, write to the Free Software Foundation,   *
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA          *
 * The GNU LEsser General Public License is contained in the file COPYING.   *
 *                                 ---------                                 *
 *   Barcelona Supercomputing Center - Centro Nacional de Supercomputacion   *
\*****************************************************************************/

/* -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- *\
 | @file: $HeadURL$
 | @last_commit: $Date$
 | @version:     $Revision$
\* -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- */

#include "kfilter.h"
#include "kwindow.h"
#include "functionmanagement.h"

bool KFilter::passFilter( MemoryTrace::iterator *it )
{
  if ( it->getType() & EVENT )
    return filterEvents( it );
  else if ( it->getType() & COMM ||
            it->getType() & RSEND ||
            it->getType() & RRECV )
    return filterComms( it );
  return true;
}

bool KFilter::filterComms( MemoryTrace::iterator *it )
{
  bool stop = true;
  TSemanticValue info;

  if ( !( logical && physical ) )
  {
    if ( !logical && !physical )
      return false;
    if ( it->getType() & LOG )
    {
      if ( !logical )
        return false;
    }
    else if ( it->getType() & PHY )
    {
      if ( !physical )
      {
        if ( !( logical && ( ( it->getType() & RECV ) &&
                             ( window->getTrace()->getLogicalReceive( it->getCommIndex() ) <
                               window->getTrace()->getPhysicalReceive( it->getCommIndex() ) ) ) )
           )
          return false;
      }
    }
  }

  bool tmpResult = functionCommFrom->getDefaultValue();
  if ( existCommFrom )
  {
    if ( window->getLevel() >= SYSTEM )
    {
      TCPUOrder tmpCPU = window->getTrace()->getSenderCPU( it->getCommIndex() );
      info = ( TSemanticValue ) window->cpuObjectToWindowObject( tmpCPU );
    }
    else
    {
      TThreadOrder tmpThread = window->getTrace()->getSenderThread( it->getCommIndex() );
      info = ( TSemanticValue ) window->threadObjectToWindowObject( tmpThread );
    }
    for ( PRV_UINT32 i = 0; i < commFrom.size(); i++ )
    {
      stop = functionCommFrom->execute( ( TSemanticValue ) commFrom[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionCommFrom->getResult();
  }

  if ( opFromTo == AND && !tmpResult )
    return false;
  else if( opFromTo == OR && tmpResult )
    return true;

  tmpResult = functionCommTo->getDefaultValue();
  if ( existCommTo )
  {
    if ( window->getLevel() >= SYSTEM )
    {
      TCPUOrder tmpCPU = window->getTrace()->getReceiverCPU( it->getCommIndex() );
      info = ( TSemanticValue ) window->cpuObjectToWindowObject( tmpCPU );
    }
    else
    {
      TThreadOrder tmpThread = window->getTrace()->getReceiverThread( it->getCommIndex() );
      info = ( TSemanticValue ) window->threadObjectToWindowObject( tmpThread );
    }
    for ( PRV_UINT32 i = 0; i < commTo.size(); i++ )
    {
      stop = functionCommTo->execute( ( TSemanticValue ) commTo[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionCommTo->getResult();
  }

  if ( !tmpResult )
    return false;

  tmpResult = functionCommTags->getDefaultValue();
  if ( existCommTags )
  {
    info = ( TSemanticValue ) window->getTrace()->getCommTag( it->getCommIndex() );
    for ( PRV_UINT32 i = 0; i < commTags.size(); i++ )
    {
      stop = functionCommTags->execute( ( TSemanticValue ) commTags[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionCommTags->getResult();
  }

  if ( opTagSize == AND && !tmpResult )
    return false;
  else if( opTagSize == OR && tmpResult )
    return true;

  tmpResult = functionCommSizes->getDefaultValue();
  if ( existCommSize )
  {
    info = ( TSemanticValue ) window->getTrace()->getCommSize( it->getCommIndex() );
    for ( PRV_UINT32 i = 0; i < commSizes.size(); i++ )
    {
      stop = functionCommSizes->execute( ( TSemanticValue ) commSizes[ i ], info );
      if ( stop == false )
        break;
    }
    tmpResult = functionCommSizes->getResult();
  }

  if ( !tmpResult )
    return false;

  tmpResult = functionBandWidth->getDefaultValue();
  if ( existBandWidth )
  {
    info = ( TSemanticValue ) window->getTrace()->getCommSize( it->getCommIndex() )
           / ( TSemanticValue )
           ( window->getTrace()->getPhysicalReceive( it->getCommIndex() ) -
             window->getTrace()->getPhysicalSend( it->getCommIndex() ) );
    for ( PRV_UINT32 i = 0; i < bandWidth.size(); i++ )
    {
      stop = functionBandWidth->execute( ( TSemanticValue ) bandWidth[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionBandWidth->getResult();
  }

  return tmpResult;
}

bool KFilter::filterEvents( MemoryTrace::iterator *it )
{
  bool stop = true;
  bool tmpResult = functionEventTypes->getDefaultValue();
  TSemanticValue info;

  if ( existEventTypes )
  {
    info = ( TSemanticValue ) it->getEventType();
    for ( PRV_UINT32 i = 0; i < eventTypes.size(); i++ )
    {
      stop = functionEventTypes->execute( ( TSemanticValue ) eventTypes[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionEventTypes->getResult();
  }

  if ( opTypeValue == AND && !tmpResult )
    return false;
  else if( opTypeValue == OR && tmpResult )
    return true;

  if ( existEventValues )
  {
    tmpResult = functionEventValues->getDefaultValue();

    info = ( TSemanticValue ) it->getEventValue();
    for ( PRV_UINT32 i = 0; i < eventValues.size(); i++ )
    {
      stop = functionEventValues->execute( ( TSemanticValue ) eventValues[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionEventValues->getResult();
  }

  return tmpResult;
}

string FilterAll::name = "All";
bool FilterAll::execute( TSemanticValue param, TSemanticValue data )
{
  result = true;
  return true;
}

string FilterNone::name = "None";
bool FilterNone::execute( TSemanticValue param, TSemanticValue data )
{
  result = false;
  return true;
}

string FilterEqual::name = "=";
bool FilterEqual::execute( TSemanticValue param, TSemanticValue data )
{
  result = data == param;
  if ( result )
    return true;
  return false;
}

string FilterNotEqual::name = "!=";
bool FilterNotEqual::execute( TSemanticValue param, TSemanticValue data )
{
  result = data != param;
  if ( result )
    return false;
  return true;
}

string FilterGreater::name = ">";
bool FilterGreater::execute( TSemanticValue param, TSemanticValue data )
{
  result = data > param;
  if ( result )
    return false;
  return true;
}

string FilterFewer::name = "<";
bool FilterFewer::execute( TSemanticValue param, TSemanticValue data )
{
  result = data < param;
  if ( result )
    return false;
  return true;
}

string FilterRange::name = "[x,y]";
bool FilterRange::execute( TSemanticValue param, TSemanticValue data )
{
  bool tmp = true;

  if ( position == MINOR )
  {
    result = data >= param;
    if ( result )
    {
      tmp = false;
      position = MAJOR;
    }
    else
      tmp = true;
  }
  else if ( position == MAJOR )
  {
    result = data <= param;
    position = MINOR;
    tmp = true;
  }
  return tmp;
}

void KFilter::clearCommFrom()
{
  commFrom.clear();
  existCommFrom = false;
}

void KFilter::insertCommFrom( TObjectOrder value )
{
  commFrom.push_back( value );
  existCommFrom = true;
}

void KFilter::getCommFrom( vector<TObjectOrder>& onVector ) const
{
  onVector = commFrom;
}

void KFilter::setCommFromFunction( string newFunction )
{
  delete functionCommFrom;
  functionCommFrom = FunctionManagement<FilterFunction>::getInstance()
                     ->getFunction( newFunction );
}

string KFilter::getCommFromFunction() const
{
  return functionCommFrom->getName();
}

void KFilter::clearCommTo()
{
  commTo.clear();
  existCommTo = false;
}

void KFilter::insertCommTo( TObjectOrder value )
{
  commTo.push_back( value );
  existCommTo = true;
}

void KFilter::getCommTo( vector<TObjectOrder>& onVector ) const
{
  onVector = commTo;
}

void KFilter::setCommToFunction( string newFunction )
{
  delete functionCommTo;
  functionCommTo = FunctionManagement<FilterFunction>::getInstance()
                   ->getFunction( newFunction );
}

string KFilter::getCommToFunction() const
{
  return functionCommTo->getName();
}

void KFilter::clearCommTags()
{
  commTags.clear();
  existCommTags = false;
}

void KFilter::insertCommTag( TCommTag value )
{
  commTags.push_back( value );
  existCommTags = true;
}

void KFilter::getCommTag( vector<TCommTag>& onVector ) const
{
  onVector = commTags;
}

void KFilter::setCommTagFunction( string newFunction )
{
  delete functionCommTags;
  functionCommTags = FunctionManagement<FilterFunction>::getInstance()
                     ->getFunction( newFunction );
}

string KFilter::getCommTagFunction() const
{
  return functionCommTags->getName();
}

void KFilter::clearCommSizes()
{
  commSizes.clear();
  existCommSize = false;
}

void KFilter::insertCommSize( TCommSize value )
{
  commSizes.push_back( value );
  existCommSize = true;
}

void KFilter::getCommSize( vector<TCommSize>& onVector ) const
{
  onVector = commSizes;
}

void KFilter::setCommSizeFunction( string newFunction )
{
  delete functionCommSizes;
  functionCommSizes = FunctionManagement<FilterFunction>::getInstance()
                      ->getFunction( newFunction );
}

string KFilter::getCommSizeFunction() const
{
  return functionCommSizes->getName();
}

void KFilter::clearBandWidth()
{
  bandWidth.clear();
  existBandWidth = false;
}

void KFilter::insertBandWidth( TSemanticValue value )
{
  bandWidth.push_back( value );
  existBandWidth = true;
}

void KFilter::getBandWidth( vector<TSemanticValue>& onVector ) const
{
  onVector = bandWidth;
}

void KFilter::setBandWidthFunction( string newFunction )
{
  delete functionBandWidth;
  functionBandWidth = FunctionManagement<FilterFunction>::getInstance()
                      ->getFunction( newFunction );
}

string KFilter::getBandWidthFunction() const
{
  return functionBandWidth->getName();
}

void KFilter::clearEventTypes()
{
  eventTypes.clear();
  existEventTypes = false;
}

void KFilter::insertEventType( TEventType value )
{
  eventTypes.push_back( value );
  existEventTypes = true;
}

void KFilter::getEventType( vector<TEventType>& onVector ) const
{
  onVector = eventTypes;
}

void KFilter::setEventTypeFunction( string newFunction )
{
  delete functionEventTypes;
  functionEventTypes = FunctionManagement<FilterFunction>::getInstance()
                       ->getFunction( newFunction );
}

string KFilter::getEventTypeFunction() const
{
  return functionEventTypes->getName();
}

void KFilter::getValidEvents( vector<TEventType>& onVector,
                              const set<TEventType>& eventsLoaded ) const
{
  bool stop = true;

  for ( set<TEventType>::const_iterator itEvt = eventsLoaded.begin();
        itEvt != eventsLoaded.end(); ++itEvt )
  {
    for ( PRV_UINT32 i = 0; i < eventTypes.size(); i++ )
    {
      stop = functionEventTypes->execute( ( TSemanticValue ) eventTypes[ i ], ( *itEvt ) );
      if ( stop )
        break;
    }
    if( functionEventTypes->getResult() )
      onVector.push_back( ( *itEvt ) );
  }

}

void KFilter::clearEventValues()
{
  eventValues.clear();
  existEventValues = false;
}

void KFilter::insertEventValue( TEventValue value )
{
  eventValues.push_back( value );
  existEventValues = true;
}

void KFilter::getEventValue( vector<TEventValue>& onVector ) const
{
  onVector = eventValues;
}

void KFilter::setEventValueFunction( string newFunction )
{
  delete functionEventValues;
  functionEventValues = FunctionManagement<FilterFunction>::getInstance()
                        ->getFunction( newFunction );
}

string KFilter::getEventValueFunction() const
{
  return functionEventValues->getName();
}

KFilter *KFilter::clone( KWindow *clonedWindow )
{
  KFilter *clonedKFilter = new KFilter( clonedWindow );

  // Constructor allocates FilterFunctions
  delete clonedKFilter->functionCommFrom;
  delete clonedKFilter->functionCommTo;
  delete clonedKFilter->functionCommTags;
  delete clonedKFilter->functionCommSizes;
  delete clonedKFilter->functionBandWidth;
  delete clonedKFilter->functionEventTypes;
  delete clonedKFilter->functionEventValues;

  // Copy values and clone FilterFunctions
  clonedKFilter->logical = logical;
  clonedKFilter->physical = physical;

  clonedKFilter->existCommFrom = existCommFrom;
  clonedKFilter->commFrom = vector<TObjectOrder>( commFrom );
  clonedKFilter->functionCommFrom = functionCommFrom->clone();

  clonedKFilter->opFromTo = opFromTo;

  clonedKFilter->existCommTo = existCommTo;
  clonedKFilter->commTo = vector<TObjectOrder>( commTo );
  clonedKFilter->functionCommTo = functionCommTo->clone();

  clonedKFilter->existCommTags = existCommTags;
  clonedKFilter->commTags = vector<TCommTag>( commTags );
  clonedKFilter->functionCommTags = functionCommTags->clone();

  clonedKFilter->opTagSize = opTagSize;

  clonedKFilter->existCommSize = existCommSize;
  clonedKFilter->commSizes = vector<TCommSize>( commSizes );
  clonedKFilter->functionCommSizes = functionCommSizes->clone();

  clonedKFilter->existBandWidth = existBandWidth;
  clonedKFilter->bandWidth = vector<TSemanticValue>( bandWidth );
  clonedKFilter->functionBandWidth = functionBandWidth->clone();

  clonedKFilter->existEventTypes = existEventTypes;
  clonedKFilter->eventTypes = vector<TEventType>( eventTypes );
  clonedKFilter->functionEventTypes = functionEventTypes->clone();

  clonedKFilter->opTypeValue = opTypeValue;

  clonedKFilter->existEventValues = existEventValues;
  clonedKFilter->eventValues = vector<TEventValue>( eventValues );
  clonedKFilter->functionEventValues = functionEventValues->clone();

  return clonedKFilter;
}
<commit_msg>*** empty log message ***<commit_after>/*****************************************************************************\
 *                        ANALYSIS PERFORMANCE TOOLS                         *
 *                               libparaver-api                              *
 *                       Paraver Main Computing Library                      *
 *****************************************************************************
 *     ___     This library is free software; you can redistribute it and/or *
 *    /  __         modify it under the terms of the GNU LGPL as published   *
 *   /  /  _____    by the Free Software Foundation; either version 2.1      *
 *  /  /  /     \   of the License, or (at your option) any later version.   *
 * (  (  ( B S C )                                                           *
 *  \  \  \_____/   This library is distributed in hope that it will be      *
 *   \  \__         useful but WITHOUT ANY WARRANTY; without even the        *
 *    \___          implied warranty of MERCHANTABILITY or FITNESS FOR A     *
 *                  PARTICULAR PURPOSE. See the GNU LGPL for more details.   *
 *                                                                           *
 * You should have received a copy of the GNU Lesser General Public License  *
 * along with this library; if not, write to the Free Software Foundation,   *
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA          *
 * The GNU LEsser General Public License is contained in the file COPYING.   *
 *                                 ---------                                 *
 *   Barcelona Supercomputing Center - Centro Nacional de Supercomputacion   *
\*****************************************************************************/

/* -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- *\
 | @file: $HeadURL$
 | @last_commit: $Date$
 | @version:     $Revision$
\* -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- */

#include "kfilter.h"
#include "kwindow.h"
#include "functionmanagement.h"

bool KFilter::passFilter( MemoryTrace::iterator *it )
{
  if ( it->getType() & EVENT )
    return filterEvents( it );
  else if ( it->getType() & COMM ||
            it->getType() & RSEND ||
            it->getType() & RRECV )
    return filterComms( it );
  return true;
}

bool KFilter::filterComms( MemoryTrace::iterator *it )
{
  bool stop = true;
  TSemanticValue info;

  if ( !( logical && physical ) )
  {
    if ( !logical && !physical )
      return false;
    if ( it->getType() & LOG )
    {
      if ( !logical )
        return false;
    }
    else if ( it->getType() & PHY )
    {
      if ( !physical )
      {
        if ( !( logical && ( ( it->getType() & RECV || it->getType() & RRECV ) &&
                             ( window->getTrace()->getLogicalReceive( it->getCommIndex() ) <
                               window->getTrace()->getPhysicalReceive( it->getCommIndex() ) ) ) )
           )
          return false;
      }
    }
  }

  bool tmpResult = functionCommFrom->getDefaultValue();
  if ( existCommFrom )
  {
    if ( window->getLevel() >= SYSTEM )
    {
      TCPUOrder tmpCPU = window->getTrace()->getSenderCPU( it->getCommIndex() );
      info = ( TSemanticValue ) window->cpuObjectToWindowObject( tmpCPU );
    }
    else
    {
      TThreadOrder tmpThread = window->getTrace()->getSenderThread( it->getCommIndex() );
      info = ( TSemanticValue ) window->threadObjectToWindowObject( tmpThread );
    }
    for ( PRV_UINT32 i = 0; i < commFrom.size(); i++ )
    {
      stop = functionCommFrom->execute( ( TSemanticValue ) commFrom[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionCommFrom->getResult();
  }

  if ( opFromTo == AND && !tmpResult )
    return false;
  else if( opFromTo == OR && tmpResult )
    return true;

  tmpResult = functionCommTo->getDefaultValue();
  if ( existCommTo )
  {
    if ( window->getLevel() >= SYSTEM )
    {
      TCPUOrder tmpCPU = window->getTrace()->getReceiverCPU( it->getCommIndex() );
      info = ( TSemanticValue ) window->cpuObjectToWindowObject( tmpCPU );
    }
    else
    {
      TThreadOrder tmpThread = window->getTrace()->getReceiverThread( it->getCommIndex() );
      info = ( TSemanticValue ) window->threadObjectToWindowObject( tmpThread );
    }
    for ( PRV_UINT32 i = 0; i < commTo.size(); i++ )
    {
      stop = functionCommTo->execute( ( TSemanticValue ) commTo[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionCommTo->getResult();
  }

  if ( !tmpResult )
    return false;

  tmpResult = functionCommTags->getDefaultValue();
  if ( existCommTags )
  {
    info = ( TSemanticValue ) window->getTrace()->getCommTag( it->getCommIndex() );
    for ( PRV_UINT32 i = 0; i < commTags.size(); i++ )
    {
      stop = functionCommTags->execute( ( TSemanticValue ) commTags[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionCommTags->getResult();
  }

  if ( opTagSize == AND && !tmpResult )
    return false;
  else if( opTagSize == OR && tmpResult )
    return true;

  tmpResult = functionCommSizes->getDefaultValue();
  if ( existCommSize )
  {
    info = ( TSemanticValue ) window->getTrace()->getCommSize( it->getCommIndex() );
    for ( PRV_UINT32 i = 0; i < commSizes.size(); i++ )
    {
      stop = functionCommSizes->execute( ( TSemanticValue ) commSizes[ i ], info );
      if ( stop == false )
        break;
    }
    tmpResult = functionCommSizes->getResult();
  }

  if ( !tmpResult )
    return false;

  tmpResult = functionBandWidth->getDefaultValue();
  if ( existBandWidth )
  {
    info = ( TSemanticValue ) window->getTrace()->getCommSize( it->getCommIndex() )
           / ( TSemanticValue )
           ( window->getTrace()->getPhysicalReceive( it->getCommIndex() ) -
             window->getTrace()->getPhysicalSend( it->getCommIndex() ) );
    for ( PRV_UINT32 i = 0; i < bandWidth.size(); i++ )
    {
      stop = functionBandWidth->execute( ( TSemanticValue ) bandWidth[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionBandWidth->getResult();
  }

  return tmpResult;
}

bool KFilter::filterEvents( MemoryTrace::iterator *it )
{
  bool stop = true;
  bool tmpResult = functionEventTypes->getDefaultValue();
  TSemanticValue info;

  if ( existEventTypes )
  {
    info = ( TSemanticValue ) it->getEventType();
    for ( PRV_UINT32 i = 0; i < eventTypes.size(); i++ )
    {
      stop = functionEventTypes->execute( ( TSemanticValue ) eventTypes[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionEventTypes->getResult();
  }

  if ( opTypeValue == AND && !tmpResult )
    return false;
  else if( opTypeValue == OR && tmpResult )
    return true;

  if ( existEventValues )
  {
    tmpResult = functionEventValues->getDefaultValue();

    info = ( TSemanticValue ) it->getEventValue();
    for ( PRV_UINT32 i = 0; i < eventValues.size(); i++ )
    {
      stop = functionEventValues->execute( ( TSemanticValue ) eventValues[ i ], info );
      if ( stop )
        break;
    }
    tmpResult = functionEventValues->getResult();
  }

  return tmpResult;
}

string FilterAll::name = "All";
bool FilterAll::execute( TSemanticValue param, TSemanticValue data )
{
  result = true;
  return true;
}

string FilterNone::name = "None";
bool FilterNone::execute( TSemanticValue param, TSemanticValue data )
{
  result = false;
  return true;
}

string FilterEqual::name = "=";
bool FilterEqual::execute( TSemanticValue param, TSemanticValue data )
{
  result = data == param;
  if ( result )
    return true;
  return false;
}

string FilterNotEqual::name = "!=";
bool FilterNotEqual::execute( TSemanticValue param, TSemanticValue data )
{
  result = data != param;
  if ( result )
    return false;
  return true;
}

string FilterGreater::name = ">";
bool FilterGreater::execute( TSemanticValue param, TSemanticValue data )
{
  result = data > param;
  if ( result )
    return false;
  return true;
}

string FilterFewer::name = "<";
bool FilterFewer::execute( TSemanticValue param, TSemanticValue data )
{
  result = data < param;
  if ( result )
    return false;
  return true;
}

string FilterRange::name = "[x,y]";
bool FilterRange::execute( TSemanticValue param, TSemanticValue data )
{
  bool tmp = true;

  if ( position == MINOR )
  {
    result = data >= param;
    if ( result )
    {
      tmp = false;
      position = MAJOR;
    }
    else
      tmp = true;
  }
  else if ( position == MAJOR )
  {
    result = data <= param;
    position = MINOR;
    tmp = true;
  }
  return tmp;
}

void KFilter::clearCommFrom()
{
  commFrom.clear();
  existCommFrom = false;
}

void KFilter::insertCommFrom( TObjectOrder value )
{
  commFrom.push_back( value );
  existCommFrom = true;
}

void KFilter::getCommFrom( vector<TObjectOrder>& onVector ) const
{
  onVector = commFrom;
}

void KFilter::setCommFromFunction( string newFunction )
{
  delete functionCommFrom;
  functionCommFrom = FunctionManagement<FilterFunction>::getInstance()
                     ->getFunction( newFunction );
}

string KFilter::getCommFromFunction() const
{
  return functionCommFrom->getName();
}

void KFilter::clearCommTo()
{
  commTo.clear();
  existCommTo = false;
}

void KFilter::insertCommTo( TObjectOrder value )
{
  commTo.push_back( value );
  existCommTo = true;
}

void KFilter::getCommTo( vector<TObjectOrder>& onVector ) const
{
  onVector = commTo;
}

void KFilter::setCommToFunction( string newFunction )
{
  delete functionCommTo;
  functionCommTo = FunctionManagement<FilterFunction>::getInstance()
                   ->getFunction( newFunction );
}

string KFilter::getCommToFunction() const
{
  return functionCommTo->getName();
}

void KFilter::clearCommTags()
{
  commTags.clear();
  existCommTags = false;
}

void KFilter::insertCommTag( TCommTag value )
{
  commTags.push_back( value );
  existCommTags = true;
}

void KFilter::getCommTag( vector<TCommTag>& onVector ) const
{
  onVector = commTags;
}

void KFilter::setCommTagFunction( string newFunction )
{
  delete functionCommTags;
  functionCommTags = FunctionManagement<FilterFunction>::getInstance()
                     ->getFunction( newFunction );
}

string KFilter::getCommTagFunction() const
{
  return functionCommTags->getName();
}

void KFilter::clearCommSizes()
{
  commSizes.clear();
  existCommSize = false;
}

void KFilter::insertCommSize( TCommSize value )
{
  commSizes.push_back( value );
  existCommSize = true;
}

void KFilter::getCommSize( vector<TCommSize>& onVector ) const
{
  onVector = commSizes;
}

void KFilter::setCommSizeFunction( string newFunction )
{
  delete functionCommSizes;
  functionCommSizes = FunctionManagement<FilterFunction>::getInstance()
                      ->getFunction( newFunction );
}

string KFilter::getCommSizeFunction() const
{
  return functionCommSizes->getName();
}

void KFilter::clearBandWidth()
{
  bandWidth.clear();
  existBandWidth = false;
}

void KFilter::insertBandWidth( TSemanticValue value )
{
  bandWidth.push_back( value );
  existBandWidth = true;
}

void KFilter::getBandWidth( vector<TSemanticValue>& onVector ) const
{
  onVector = bandWidth;
}

void KFilter::setBandWidthFunction( string newFunction )
{
  delete functionBandWidth;
  functionBandWidth = FunctionManagement<FilterFunction>::getInstance()
                      ->getFunction( newFunction );
}

string KFilter::getBandWidthFunction() const
{
  return functionBandWidth->getName();
}

void KFilter::clearEventTypes()
{
  eventTypes.clear();
  existEventTypes = false;
}

void KFilter::insertEventType( TEventType value )
{
  eventTypes.push_back( value );
  existEventTypes = true;
}

void KFilter::getEventType( vector<TEventType>& onVector ) const
{
  onVector = eventTypes;
}

void KFilter::setEventTypeFunction( string newFunction )
{
  delete functionEventTypes;
  functionEventTypes = FunctionManagement<FilterFunction>::getInstance()
                       ->getFunction( newFunction );
}

string KFilter::getEventTypeFunction() const
{
  return functionEventTypes->getName();
}

void KFilter::getValidEvents( vector<TEventType>& onVector,
                              const set<TEventType>& eventsLoaded ) const
{
  bool stop = true;

  for ( set<TEventType>::const_iterator itEvt = eventsLoaded.begin();
        itEvt != eventsLoaded.end(); ++itEvt )
  {
    for ( PRV_UINT32 i = 0; i < eventTypes.size(); i++ )
    {
      stop = functionEventTypes->execute( ( TSemanticValue ) eventTypes[ i ], ( *itEvt ) );
      if ( stop )
        break;
    }
    if( functionEventTypes->getResult() )
      onVector.push_back( ( *itEvt ) );
  }

}

void KFilter::clearEventValues()
{
  eventValues.clear();
  existEventValues = false;
}

void KFilter::insertEventValue( TEventValue value )
{
  eventValues.push_back( value );
  existEventValues = true;
}

void KFilter::getEventValue( vector<TEventValue>& onVector ) const
{
  onVector = eventValues;
}

void KFilter::setEventValueFunction( string newFunction )
{
  delete functionEventValues;
  functionEventValues = FunctionManagement<FilterFunction>::getInstance()
                        ->getFunction( newFunction );
}

string KFilter::getEventValueFunction() const
{
  return functionEventValues->getName();
}

KFilter *KFilter::clone( KWindow *clonedWindow )
{
  KFilter *clonedKFilter = new KFilter( clonedWindow );

  // Constructor allocates FilterFunctions
  delete clonedKFilter->functionCommFrom;
  delete clonedKFilter->functionCommTo;
  delete clonedKFilter->functionCommTags;
  delete clonedKFilter->functionCommSizes;
  delete clonedKFilter->functionBandWidth;
  delete clonedKFilter->functionEventTypes;
  delete clonedKFilter->functionEventValues;

  // Copy values and clone FilterFunctions
  clonedKFilter->logical = logical;
  clonedKFilter->physical = physical;

  clonedKFilter->existCommFrom = existCommFrom;
  clonedKFilter->commFrom = vector<TObjectOrder>( commFrom );
  clonedKFilter->functionCommFrom = functionCommFrom->clone();

  clonedKFilter->opFromTo = opFromTo;

  clonedKFilter->existCommTo = existCommTo;
  clonedKFilter->commTo = vector<TObjectOrder>( commTo );
  clonedKFilter->functionCommTo = functionCommTo->clone();

  clonedKFilter->existCommTags = existCommTags;
  clonedKFilter->commTags = vector<TCommTag>( commTags );
  clonedKFilter->functionCommTags = functionCommTags->clone();

  clonedKFilter->opTagSize = opTagSize;

  clonedKFilter->existCommSize = existCommSize;
  clonedKFilter->commSizes = vector<TCommSize>( commSizes );
  clonedKFilter->functionCommSizes = functionCommSizes->clone();

  clonedKFilter->existBandWidth = existBandWidth;
  clonedKFilter->bandWidth = vector<TSemanticValue>( bandWidth );
  clonedKFilter->functionBandWidth = functionBandWidth->clone();

  clonedKFilter->existEventTypes = existEventTypes;
  clonedKFilter->eventTypes = vector<TEventType>( eventTypes );
  clonedKFilter->functionEventTypes = functionEventTypes->clone();

  clonedKFilter->opTypeValue = opTypeValue;

  clonedKFilter->existEventValues = existEventValues;
  clonedKFilter->eventValues = vector<TEventValue>( eventValues );
  clonedKFilter->functionEventValues = functionEventValues->clone();

  return clonedKFilter;
}
<|endoftext|>