<commit_before>// Copyright 2019 DeepMind Technologies Ltd. All rights reserved.
//
// 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
//
//     http://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.

#include "open_spiel/algorithms/mcts.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <random>
#include <vector>

#include "open_spiel/abseil-cpp/absl/algorithm/container.h"
#include "open_spiel/abseil-cpp/absl/random/uniform_int_distribution.h"
#include "open_spiel/abseil-cpp/absl/strings/str_cat.h"
#include "open_spiel/abseil-cpp/absl/strings/str_format.h"
#include "open_spiel/abseil-cpp/absl/time/clock.h"
#include "open_spiel/abseil-cpp/absl/time/time.h"
#include "open_spiel/spiel.h"
#include "open_spiel/spiel_utils.h"

namespace open_spiel {
namespace algorithms {

// Return the memory use of a vector. Useful to track and limit memory use when
// running for a long time and build a big tree (eg to solve a game).
template <class T>
constexpr inline int VectorMemory(const std::vector<T>& vec) {
  return sizeof(T) * vec.capacity();
}

std::vector<double> RandomRolloutEvaluator::Evaluate(const State& state) {
  std::vector<double> result;
  for (int i = 0; i < n_rollouts_; ++i) {
    std::unique_ptr<State> working_state = state.Clone();
    while (!working_state->IsTerminal()) {
      if (working_state->IsChanceNode()) {
        ActionsAndProbs outcomes = working_state->ChanceOutcomes();
        Action action =
            SampleAction(outcomes,
                         std::uniform_real_distribution<double>(0.0, 1.0)(rng_))
                .first;
        working_state->ApplyAction(action);
      } else {
        std::vector<Action> actions = working_state->LegalActions();
        absl::uniform_int_distribution<int> dist(0, actions.size() - 1);
        int index = dist(rng_);
        working_state->ApplyAction(actions[index]);
      }
    }

    std::vector<double> returns = working_state->Returns();
    if (result.empty()) {
      result.swap(returns);
    } else {
      SPIEL_CHECK_EQ(returns.size(), result.size());
      for (int i = 0; i < result.size(); ++i) {
        result[i] += returns[i];
      }
    }
  }
  for (int i = 0; i < result.size(); ++i) {
    result[i] /= n_rollouts_;
  }
  return result;
}

ActionsAndProbs RandomRolloutEvaluator::Prior(const State& state) {
  // Returns equal probability for all actions.
  if (state.IsChanceNode()) {
    return state.ChanceOutcomes();
  } else {
    std::vector<Action> legal_actions = state.LegalActions();
    ActionsAndProbs prior;
    prior.reserve(legal_actions.size());
    for (const Action& action : legal_actions) {
      prior.emplace_back(action, 1.0 / legal_actions.size());
    }
    return prior;
  }
}

// UCT value of given child
double SearchNode::UCTValue(int parent_explore_count, double uct_c) const {
  if (!outcome.empty()) {
    return outcome[player];
  }

  if (explore_count == 0) return std::numeric_limits<double>::infinity();

  // The "greedy-value" of choosing a given child is always with respect to
  // the current player for this node.
  return total_reward / explore_count +
         uct_c * std::sqrt(std::log(parent_explore_count) / explore_count);
}

double SearchNode::PUCTValue(int parent_explore_count, double uct_c) const {
  // Returns the PUCT value of this node.
  if (!outcome.empty()) {
    return outcome[player];
  }

  return ((explore_count != 0 ? total_reward / explore_count : 0) +
          uct_c * prior * std::sqrt(parent_explore_count) /
              (explore_count + 1));
}

bool SearchNode::CompareFinal(const SearchNode& b) const {
  double out = (outcome.empty() ? 0 : outcome[player]);
  double out_b = (b.outcome.empty() ? 0 : b.outcome[b.player]);
  if (out != out_b) {
    return out < out_b;
  }
  if (explore_count != b.explore_count) {
    return explore_count < b.explore_count;
  }
  return total_reward < b.total_reward;
}

const SearchNode& SearchNode::BestChild() const {
  // Returns the best action from this node, either proven or most visited.
  //
  // This ordering leads to choosing:
  // - Highest proven score > 0 over anything else, including a promising but
  //   unproven action.
  // - A proven draw only if it has higher exploration than others that are
  //   uncertain, or the others are losses.
  // - Uncertain action with most exploration over loss of any difficulty
  // - Hardest loss if everything is a loss
  // - Highest expected reward if explore counts are equal (unlikely).
  // - Longest win, if multiple are proven (unlikely due to early stopping).
  return *std::max_element(children.begin(), children.end(),
                           [](const SearchNode& a, const SearchNode& b) {
                             return a.CompareFinal(b);
                           });
}

std::string SearchNode::ChildrenStr(const State& state) const {
  std::string out;
  if (!children.empty()) {
    std::vector<const SearchNode*> refs;  // Sort a list of refs, not a copy.
    refs.reserve(children.size());
    for (const SearchNode& child : children) {
      refs.push_back(&child);
    }
    std::sort(refs.begin(), refs.end(),
              [](const SearchNode* a, const SearchNode* b) {
                return b->CompareFinal(*a);
              });
    for (const SearchNode* child : refs) {
      absl::StrAppend(&out, child->ToString(state), "\n");
    }
  }
  return out;
}

std::string SearchNode::ToString(const State& state) const {
  return absl::StrFormat(
      "%6s: player: %d, prior: %5.3f, value: %6.3f, sims: %5d, outcome: %s, "
      "%3d children",
      (action != kInvalidAction ? state.ActionToString(player, action)
                                : "none"),
      player, prior, (explore_count ? total_reward / explore_count : 0.),
      explore_count,
      (outcome.empty()
           ? "none"
           : absl::StrFormat("%4.1f",
                             outcome[player == kChancePlayerId ? 0 : player])),
      children.size());
}

std::vector<double> dirichlet_noise(int count, double alpha,
                                    std::mt19937* rng) {
  auto noise = std::vector<double>{};
  noise.reserve(count);

  std::gamma_distribution<double> gamma(alpha, 1.0);
  for (int i = 0; i < count; ++i) {
    noise.emplace_back(gamma(*rng));
  }

  double sum = absl::c_accumulate(noise, 0.0);
  for (double& v : noise) {
    v /= sum;
  }
  return noise;
}

MCTSBot::MCTSBot(const Game& game, Evaluator* evaluator,
                 double uct_c, int max_simulations, int64_t max_memory_mb,
                 bool solve, int seed, bool verbose,
                 ChildSelectionPolicy child_selection_policy,
                 double dirichlet_alpha, double dirichlet_epsilon)
    : uct_c_{uct_c},
      max_simulations_{max_simulations},
      max_memory_(max_memory_mb << 20),  // megabytes -> bytes
      verbose_(verbose),
      solve_(solve),
      max_utility_(game.MaxUtility()),
      dirichlet_alpha_(dirichlet_alpha),
      dirichlet_epsilon_(dirichlet_epsilon),
      rng_(seed),
      child_selection_policy_(child_selection_policy),
      evaluator_{evaluator} {
  GameType game_type = game.GetType();
  if (game_type.reward_model != GameType::RewardModel::kTerminal)
    SpielFatalError("Game must have terminal rewards.");
  if (game_type.dynamics != GameType::Dynamics::kSequential)
    SpielFatalError("Game must have sequential turns.");
}

Action MCTSBot::Step(const State& state) {
  absl::Time start = absl::Now();
  std::unique_ptr<SearchNode> root = MCTSearch(state);
  const SearchNode& best = root->BestChild();

  if (verbose_) {
    double seconds = absl::ToDoubleSeconds(absl::Now() - start);
    std::cerr
        << absl::StrFormat(
               "Finished %d sims in %.3f secs, %.1f sims/s, tree size: %d mb.",
               root->explore_count, seconds, (root->explore_count / seconds),
               memory_used_ / (1 << 20))
        << std::endl;
    std::cerr << "Root:" << std::endl;
    std::cerr << root->ToString(state) << std::endl;
    std::cerr << "Children:" << std::endl;
    std::cerr << root->ChildrenStr(state) << std::endl;
    if (!best.children.empty()) {
      std::unique_ptr<State> chosen_state = state.Clone();
      chosen_state->ApplyAction(best.action);
      std::cerr << "Children of chosen:" << std::endl;
      std::cerr << best.ChildrenStr(*chosen_state) << std::endl;
    }
  }

  return best.action;
}

std::pair<ActionsAndProbs, Action> MCTSBot::StepWithPolicy(const State& state) {
  Action action = Step(state);
  return {{{action, 1.}}, action};
}

std::unique_ptr<State> MCTSBot::ApplyTreePolicy(
    SearchNode* root, const State& state,
    std::vector<SearchNode*>* visit_path) {
  visit_path->push_back(root);
  std::unique_ptr<State> working_state = state.Clone();
  SearchNode* current_node = root;
  while (!working_state->IsTerminal() && current_node->explore_count > 0) {
    if (current_node->children.empty()) {
      // For a new node, initialize its state, then choose a child as normal.
      ActionsAndProbs legal_actions = evaluator_->Prior(*working_state);
      if (current_node == root && dirichlet_alpha_ > 0) {
        std::vector<double> noise =
            dirichlet_noise(legal_actions.size(), dirichlet_alpha_, &rng_);
        for (int i = 0; i < legal_actions.size(); i++) {
          legal_actions[i].second =
              (1 - dirichlet_epsilon_) * legal_actions[i].second +
              dirichlet_epsilon_ * noise[i];
        }
      }
      // Reduce bias from move generation order.
      std::shuffle(legal_actions.begin(), legal_actions.end(), rng_);
      Player player = working_state->CurrentPlayer();
      current_node->children.reserve(legal_actions.size());
      for (auto [action, prior] : legal_actions) {
        current_node->children.emplace_back(action, player, prior);
      }
      memory_used_ += VectorMemory(legal_actions);
    }

    SearchNode* chosen_child = nullptr;
    if (working_state->IsChanceNode()) {
      // For chance nodes, rollout according to chance node's probability
      // distribution
      Action chosen_action =
          SampleAction(working_state->ChanceOutcomes(),
                       std::uniform_real_distribution<double>(0.0, 1.0)(rng_))
              .first;

      for (SearchNode& child : current_node->children) {
        if (child.action == chosen_action) {
          chosen_child = &child;
          break;
        }
      }
    } else {
      // Otherwise choose node with largest UCT value.
      double max_value = -std::numeric_limits<double>::infinity();
      for (SearchNode& child : current_node->children) {
        double val;
        switch (child_selection_policy_) {
          case ChildSelectionPolicy::UCT:
            val = child.UCTValue(current_node->explore_count, uct_c_);
            break;
          case ChildSelectionPolicy::PUCT:
            val = child.PUCTValue(current_node->explore_count, uct_c_);
            break;
        }
        if (val > max_value) {
          max_value = val;
          chosen_child = &child;
        }
      }
    }

    working_state->ApplyAction(chosen_child->action);
    current_node = chosen_child;
    visit_path->push_back(current_node);
  }

  return working_state;
}

std::unique_ptr<SearchNode> MCTSBot::MCTSearch(const State& state) {
  Player player_id = state.CurrentPlayer();
  memory_used_ = 0;
  auto root = std::make_unique<SearchNode>(kInvalidAction, player_id, 1);
  std::vector<SearchNode*> visit_path;
  std::vector<double> returns;
  visit_path.reserve(64);
  for (int i = 0; i < max_simulations_; ++i) {
    visit_path.clear();
    returns.clear();

    std::unique_ptr<State> working_state =
        ApplyTreePolicy(root.get(), state, &visit_path);

    bool solved;
    if (working_state->IsTerminal()) {
      returns = working_state->Returns();
      visit_path[visit_path.size() - 1]->outcome = returns;
      memory_used_ += VectorMemory(returns);
      solved = solve_;
    } else {
      returns = evaluator_->Evaluate(*working_state);
      solved = false;
    }

    // Propagate values back.
    for (auto it = visit_path.rbegin(); it != visit_path.rend(); ++it) {
      SearchNode* node = *it;

      node->total_reward +=
          returns[node->player == kChancePlayerId ? player_id : node->player];
      node->explore_count += 1;

      // Back up solved results as well.
      if (solved && !node->children.empty()) {
        Player player = node->children[0].player;
        if (player == kChancePlayerId) {
          // Only back up chance nodes if all have the same outcome.
          // An alternative would be to back up the weighted average of
          // outcomes if all children are solved, but that is less clear.
          const std::vector<double>& outcome = node->children[0].outcome;
          if (!outcome.empty() &&
              std::all_of(node->children.begin() + 1, node->children.end(),
                          [&outcome](const SearchNode& c) {
                            return c.outcome == outcome;
                          })) {
            node->outcome = outcome;
            memory_used_ += VectorMemory(node->outcome);
          } else {
            solved = false;
          }
        } else {
          // If any have max utility (won?), or all children are solved,
          // choose the one best for the player choosing.
          const SearchNode* best = nullptr;
          bool all_solved = true;
          for (const SearchNode& child : node->children) {
            if (child.outcome.empty()) {
              all_solved = false;
            } else if (best == nullptr ||
                       child.outcome[player] > best->outcome[player]) {
              best = &child;
            }
          }
          if (best != nullptr &&
              (all_solved || best->outcome[player] == max_utility_)) {
            node->outcome = best->outcome;
            memory_used_ += VectorMemory(node->outcome);
          } else {
            solved = false;
          }
        }
      }
    }

    if (!root->outcome.empty() ||  // Full game tree is solved.
        (max_memory_ && memory_used_ >= max_memory_) ||
        root->children.size() == 1) {
      break;
    }
  }

  return root;
}

}  // namespace algorithms
}  // namespace open_spiel
<commit_msg>Initialize directly instead of using auto.<commit_after>// Copyright 2019 DeepMind Technologies Ltd. All rights reserved.
//
// 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
//
//     http://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.

#include "open_spiel/algorithms/mcts.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <random>
#include <vector>

#include "open_spiel/abseil-cpp/absl/algorithm/container.h"
#include "open_spiel/abseil-cpp/absl/random/uniform_int_distribution.h"
#include "open_spiel/abseil-cpp/absl/strings/str_cat.h"
#include "open_spiel/abseil-cpp/absl/strings/str_format.h"
#include "open_spiel/abseil-cpp/absl/time/clock.h"
#include "open_spiel/abseil-cpp/absl/time/time.h"
#include "open_spiel/spiel.h"
#include "open_spiel/spiel_utils.h"

namespace open_spiel {
namespace algorithms {

// Return the memory use of a vector. Useful to track and limit memory use when
// running for a long time and build a big tree (eg to solve a game).
template <class T>
constexpr inline int VectorMemory(const std::vector<T>& vec) {
  return sizeof(T) * vec.capacity();
}

std::vector<double> RandomRolloutEvaluator::Evaluate(const State& state) {
  std::vector<double> result;
  for (int i = 0; i < n_rollouts_; ++i) {
    std::unique_ptr<State> working_state = state.Clone();
    while (!working_state->IsTerminal()) {
      if (working_state->IsChanceNode()) {
        ActionsAndProbs outcomes = working_state->ChanceOutcomes();
        Action action =
            SampleAction(outcomes,
                         std::uniform_real_distribution<double>(0.0, 1.0)(rng_))
                .first;
        working_state->ApplyAction(action);
      } else {
        std::vector<Action> actions = working_state->LegalActions();
        absl::uniform_int_distribution<int> dist(0, actions.size() - 1);
        int index = dist(rng_);
        working_state->ApplyAction(actions[index]);
      }
    }

    std::vector<double> returns = working_state->Returns();
    if (result.empty()) {
      result.swap(returns);
    } else {
      SPIEL_CHECK_EQ(returns.size(), result.size());
      for (int i = 0; i < result.size(); ++i) {
        result[i] += returns[i];
      }
    }
  }
  for (int i = 0; i < result.size(); ++i) {
    result[i] /= n_rollouts_;
  }
  return result;
}

ActionsAndProbs RandomRolloutEvaluator::Prior(const State& state) {
  // Returns equal probability for all actions.
  if (state.IsChanceNode()) {
    return state.ChanceOutcomes();
  } else {
    std::vector<Action> legal_actions = state.LegalActions();
    ActionsAndProbs prior;
    prior.reserve(legal_actions.size());
    for (const Action& action : legal_actions) {
      prior.emplace_back(action, 1.0 / legal_actions.size());
    }
    return prior;
  }
}

// UCT value of given child
double SearchNode::UCTValue(int parent_explore_count, double uct_c) const {
  if (!outcome.empty()) {
    return outcome[player];
  }

  if (explore_count == 0) return std::numeric_limits<double>::infinity();

  // The "greedy-value" of choosing a given child is always with respect to
  // the current player for this node.
  return total_reward / explore_count +
         uct_c * std::sqrt(std::log(parent_explore_count) / explore_count);
}

double SearchNode::PUCTValue(int parent_explore_count, double uct_c) const {
  // Returns the PUCT value of this node.
  if (!outcome.empty()) {
    return outcome[player];
  }

  return ((explore_count != 0 ? total_reward / explore_count : 0) +
          uct_c * prior * std::sqrt(parent_explore_count) /
              (explore_count + 1));
}

bool SearchNode::CompareFinal(const SearchNode& b) const {
  double out = (outcome.empty() ? 0 : outcome[player]);
  double out_b = (b.outcome.empty() ? 0 : b.outcome[b.player]);
  if (out != out_b) {
    return out < out_b;
  }
  if (explore_count != b.explore_count) {
    return explore_count < b.explore_count;
  }
  return total_reward < b.total_reward;
}

const SearchNode& SearchNode::BestChild() const {
  // Returns the best action from this node, either proven or most visited.
  //
  // This ordering leads to choosing:
  // - Highest proven score > 0 over anything else, including a promising but
  //   unproven action.
  // - A proven draw only if it has higher exploration than others that are
  //   uncertain, or the others are losses.
  // - Uncertain action with most exploration over loss of any difficulty
  // - Hardest loss if everything is a loss
  // - Highest expected reward if explore counts are equal (unlikely).
  // - Longest win, if multiple are proven (unlikely due to early stopping).
  return *std::max_element(children.begin(), children.end(),
                           [](const SearchNode& a, const SearchNode& b) {
                             return a.CompareFinal(b);
                           });
}

std::string SearchNode::ChildrenStr(const State& state) const {
  std::string out;
  if (!children.empty()) {
    std::vector<const SearchNode*> refs;  // Sort a list of refs, not a copy.
    refs.reserve(children.size());
    for (const SearchNode& child : children) {
      refs.push_back(&child);
    }
    std::sort(refs.begin(), refs.end(),
              [](const SearchNode* a, const SearchNode* b) {
                return b->CompareFinal(*a);
              });
    for (const SearchNode* child : refs) {
      absl::StrAppend(&out, child->ToString(state), "\n");
    }
  }
  return out;
}

std::string SearchNode::ToString(const State& state) const {
  return absl::StrFormat(
      "%6s: player: %d, prior: %5.3f, value: %6.3f, sims: %5d, outcome: %s, "
      "%3d children",
      (action != kInvalidAction ? state.ActionToString(player, action)
                                : "none"),
      player, prior, (explore_count ? total_reward / explore_count : 0.),
      explore_count,
      (outcome.empty()
           ? "none"
           : absl::StrFormat("%4.1f",
                             outcome[player == kChancePlayerId ? 0 : player])),
      children.size());
}

std::vector<double> dirichlet_noise(int count, double alpha,
                                    std::mt19937* rng) {
  std::vector<double> noise;
  noise.reserve(count);

  std::gamma_distribution<double> gamma(alpha, 1.0);
  for (int i = 0; i < count; ++i) {
    noise.emplace_back(gamma(*rng));
  }

  double sum = absl::c_accumulate(noise, 0.0);
  for (double& v : noise) {
    v /= sum;
  }
  return noise;
}

MCTSBot::MCTSBot(const Game& game, Evaluator* evaluator,
                 double uct_c, int max_simulations, int64_t max_memory_mb,
                 bool solve, int seed, bool verbose,
                 ChildSelectionPolicy child_selection_policy,
                 double dirichlet_alpha, double dirichlet_epsilon)
    : uct_c_{uct_c},
      max_simulations_{max_simulations},
      max_memory_(max_memory_mb << 20),  // megabytes -> bytes
      verbose_(verbose),
      solve_(solve),
      max_utility_(game.MaxUtility()),
      dirichlet_alpha_(dirichlet_alpha),
      dirichlet_epsilon_(dirichlet_epsilon),
      rng_(seed),
      child_selection_policy_(child_selection_policy),
      evaluator_{evaluator} {
  GameType game_type = game.GetType();
  if (game_type.reward_model != GameType::RewardModel::kTerminal)
    SpielFatalError("Game must have terminal rewards.");
  if (game_type.dynamics != GameType::Dynamics::kSequential)
    SpielFatalError("Game must have sequential turns.");
}

Action MCTSBot::Step(const State& state) {
  absl::Time start = absl::Now();
  std::unique_ptr<SearchNode> root = MCTSearch(state);
  const SearchNode& best = root->BestChild();

  if (verbose_) {
    double seconds = absl::ToDoubleSeconds(absl::Now() - start);
    std::cerr
        << absl::StrFormat(
               "Finished %d sims in %.3f secs, %.1f sims/s, tree size: %d mb.",
               root->explore_count, seconds, (root->explore_count / seconds),
               memory_used_ / (1 << 20))
        << std::endl;
    std::cerr << "Root:" << std::endl;
    std::cerr << root->ToString(state) << std::endl;
    std::cerr << "Children:" << std::endl;
    std::cerr << root->ChildrenStr(state) << std::endl;
    if (!best.children.empty()) {
      std::unique_ptr<State> chosen_state = state.Clone();
      chosen_state->ApplyAction(best.action);
      std::cerr << "Children of chosen:" << std::endl;
      std::cerr << best.ChildrenStr(*chosen_state) << std::endl;
    }
  }

  return best.action;
}

std::pair<ActionsAndProbs, Action> MCTSBot::StepWithPolicy(const State& state) {
  Action action = Step(state);
  return {{{action, 1.}}, action};
}

std::unique_ptr<State> MCTSBot::ApplyTreePolicy(
    SearchNode* root, const State& state,
    std::vector<SearchNode*>* visit_path) {
  visit_path->push_back(root);
  std::unique_ptr<State> working_state = state.Clone();
  SearchNode* current_node = root;
  while (!working_state->IsTerminal() && current_node->explore_count > 0) {
    if (current_node->children.empty()) {
      // For a new node, initialize its state, then choose a child as normal.
      ActionsAndProbs legal_actions = evaluator_->Prior(*working_state);
      if (current_node == root && dirichlet_alpha_ > 0) {
        std::vector<double> noise =
            dirichlet_noise(legal_actions.size(), dirichlet_alpha_, &rng_);
        for (int i = 0; i < legal_actions.size(); i++) {
          legal_actions[i].second =
              (1 - dirichlet_epsilon_) * legal_actions[i].second +
              dirichlet_epsilon_ * noise[i];
        }
      }
      // Reduce bias from move generation order.
      std::shuffle(legal_actions.begin(), legal_actions.end(), rng_);
      Player player = working_state->CurrentPlayer();
      current_node->children.reserve(legal_actions.size());
      for (auto [action, prior] : legal_actions) {
        current_node->children.emplace_back(action, player, prior);
      }
      memory_used_ += VectorMemory(legal_actions);
    }

    SearchNode* chosen_child = nullptr;
    if (working_state->IsChanceNode()) {
      // For chance nodes, rollout according to chance node's probability
      // distribution
      Action chosen_action =
          SampleAction(working_state->ChanceOutcomes(),
                       std::uniform_real_distribution<double>(0.0, 1.0)(rng_))
              .first;

      for (SearchNode& child : current_node->children) {
        if (child.action == chosen_action) {
          chosen_child = &child;
          break;
        }
      }
    } else {
      // Otherwise choose node with largest UCT value.
      double max_value = -std::numeric_limits<double>::infinity();
      for (SearchNode& child : current_node->children) {
        double val;
        switch (child_selection_policy_) {
          case ChildSelectionPolicy::UCT:
            val = child.UCTValue(current_node->explore_count, uct_c_);
            break;
          case ChildSelectionPolicy::PUCT:
            val = child.PUCTValue(current_node->explore_count, uct_c_);
            break;
        }
        if (val > max_value) {
          max_value = val;
          chosen_child = &child;
        }
      }
    }

    working_state->ApplyAction(chosen_child->action);
    current_node = chosen_child;
    visit_path->push_back(current_node);
  }

  return working_state;
}

std::unique_ptr<SearchNode> MCTSBot::MCTSearch(const State& state) {
  Player player_id = state.CurrentPlayer();
  memory_used_ = 0;
  auto root = std::make_unique<SearchNode>(kInvalidAction, player_id, 1);
  std::vector<SearchNode*> visit_path;
  std::vector<double> returns;
  visit_path.reserve(64);
  for (int i = 0; i < max_simulations_; ++i) {
    visit_path.clear();
    returns.clear();

    std::unique_ptr<State> working_state =
        ApplyTreePolicy(root.get(), state, &visit_path);

    bool solved;
    if (working_state->IsTerminal()) {
      returns = working_state->Returns();
      visit_path[visit_path.size() - 1]->outcome = returns;
      memory_used_ += VectorMemory(returns);
      solved = solve_;
    } else {
      returns = evaluator_->Evaluate(*working_state);
      solved = false;
    }

    // Propagate values back.
    for (auto it = visit_path.rbegin(); it != visit_path.rend(); ++it) {
      SearchNode* node = *it;

      node->total_reward +=
          returns[node->player == kChancePlayerId ? player_id : node->player];
      node->explore_count += 1;

      // Back up solved results as well.
      if (solved && !node->children.empty()) {
        Player player = node->children[0].player;
        if (player == kChancePlayerId) {
          // Only back up chance nodes if all have the same outcome.
          // An alternative would be to back up the weighted average of
          // outcomes if all children are solved, but that is less clear.
          const std::vector<double>& outcome = node->children[0].outcome;
          if (!outcome.empty() &&
              std::all_of(node->children.begin() + 1, node->children.end(),
                          [&outcome](const SearchNode& c) {
                            return c.outcome == outcome;
                          })) {
            node->outcome = outcome;
            memory_used_ += VectorMemory(node->outcome);
          } else {
            solved = false;
          }
        } else {
          // If any have max utility (won?), or all children are solved,
          // choose the one best for the player choosing.
          const SearchNode* best = nullptr;
          bool all_solved = true;
          for (const SearchNode& child : node->children) {
            if (child.outcome.empty()) {
              all_solved = false;
            } else if (best == nullptr ||
                       child.outcome[player] > best->outcome[player]) {
              best = &child;
            }
          }
          if (best != nullptr &&
              (all_solved || best->outcome[player] == max_utility_)) {
            node->outcome = best->outcome;
            memory_used_ += VectorMemory(node->outcome);
          } else {
            solved = false;
          }
        }
      }
    }

    if (!root->outcome.empty() ||  // Full game tree is solved.
        (max_memory_ && memory_used_ >= max_memory_) ||
        root->children.size() == 1) {
      break;
    }
  }

  return root;
}

}  // namespace algorithms
}  // namespace open_spiel
<|endoftext|>


<commit_before>/*
 *   IRC interfaces for La Cogita IRC chatbot
 *   Copyright (C) 2007 Linas Vepstas <linas@linas.org>
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the 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 General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/*
 * Go onto IRC
 * This is pretty totally a pure hack with little/no design to it.
 * Linas October 2007
 */

#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <getopt.h>

#include <string>
#include <vector>
#include <set>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include "IRC.h"
#include "CogitaConfig.h"

#include "whirr-sockets.h"

using namespace opencog::chatbot;
using std::string;

CogitaConfig cc;

/* printf can puke if these fields are NULL */
void fixup_reply(irc_reply_data* ird)
{
	ird->nick = (NULL == ird->nick) ? "" : ird->nick;
	ird->ident = (NULL == ird->ident) ? "" : ird->ident;
	ird->host = (NULL == ird->host) ? "" : ird->host;
	ird->target = (NULL == ird->target) ? "" : ird->target;
}

/**
 * Join channel shortly after logging into the irc server.
 */
int end_of_motd(const char* params, irc_reply_data* ird, void* data)
{
	IRC* conn = (IRC*) data;
	fixup_reply(ird);

	printf("chatbot got params=%s\n", params);
	printf("chatbot got motd nick=%s ident=%s host=%s target=%s\n",
		ird->nick, ird->ident, ird->host, ird->target);

	sleep(1);
	conn->join (cc.ircChannels[0].c_str());
	printf("chatbot sent channel join %s\n", cc.ircChannels[0].c_str());
	sleep(2);
	conn->notice (cc.ircChannels[0].c_str(), "ola");
	printf("chatbot sent channel notice\n");
	sleep(2);
	conn->privmsg (cc.ircChannels[0].c_str(), "here we are");
	printf("chatbot said hello to the channel\n");
	return 0;
}

/* return true if not all whitespace */
static bool is_nonblank(const char * str)
{
	size_t len = strlen(str);
	if (0 == len) return false;
	size_t blanks = strspn(str, " \n\r\t\v");
	if (blanks == len) return false;
	return true;
}

/**
 * Handle a message received from IRC.
 */
int got_privmsg(const char* params, irc_reply_data* ird, void* data)
{
	IRC* conn = (IRC*) data;
	fixup_reply(ird);

	printf("input=%s\n", params);
	printf("nick=%s ident=%s host=%s target=%s\n", ird->nick, ird->ident, ird->host, ird->target);

	typedef enum {ENGLISH=1, SHELL_CMD, SCM_CMD} CmdType;
	CmdType cmd = ENGLISH;

	const char * start = NULL;
	int priv = 0;
	if (!strcmp (ird->target, cc.nick.c_str())) {priv = 1; start = params+1; }

	if (!strncmp (&params[1], cc.nick.c_str(), cc.nick.size())) {
		start = params+1 + cc.nick.size();
		start = strchr(start, ':');
		if (start) start ++;
	} else if (!strncmp (params, ":cog-sh:", 8)) {
		start = params+8; cmd = SHELL_CMD;
	} else if (!strncmp (params, ":scm:", 5)) {
		start = params+5; cmd = SCM_CMD;
	} else {
		// Check for alternative nick/attention strings
		foreach (string it, cc.attn) {
			if (! it.compare(0,it.size(),&params[1]) ) {
				start = params + it.size();
				start = strchr(start, ':');
				if (start) start ++;
				break;
			}
		}
	}

	if (!start) return 0;
	char * msg_target = NULL;
	if (priv)
	{
		msg_target = ird->nick;
	}
	else
	{
		msg_target = ird->target;
	}

	// Reply to request for chat client version
	if ((0x1 == start[0]) && !strncmp (&start[1], "VERSION", 7))
	{
		printf ("VERSION: %s\n", cc.vstring.c_str());
		conn->privmsg (msg_target, cc.vstring.c_str());
		return 0;
	}

	// printf ("duude starting with 0x%x %s\n", start[0], start);
	size_t textlen = strlen(start);
	size_t len = textlen;
	len += strlen ("(say-id-english )");
	len += strlen (ird->nick);
	len += 120;

	char * cmdline = (char *) malloc(sizeof (char) * (len+1));

	if (ENGLISH == cmd)
	{
		// Get into the opencog scheme shell, and run the command
		strcpy (cmdline, "scm hush\n(say-id-english \"");
		strcat (cmdline, ird->nick);
		strcat (cmdline, "\" \"");
		size_t toff = strlen(cmdline);
		strcat (cmdline, start);
		strcat (cmdline, "\")\n");

		// strip out quotation marks, replace with blanks, for now.
		for (size_t i =0; i<textlen; i++)
		{
			if ('\"' == cmdline[toff+i]) cmdline[toff+i] = ' ';
		}
	}

// #define ENABLE_SHELL_ESCAPES 0
#if ENABLE_SHELL_ESCAPES
	/*
	 * XXX DANGER DANGER Extreme Caution Advised XXX
	 * Shell escapes are a potential security hole, as they allow access
	 * to the cog-server to total strangers. In particular, the scheme
	 * interface is a general programming API and can be used to root
	 * the system.
	 */
	else if (SHELL_CMD == cmd)
	{
		strcpy (cmdline, start);
		strcat (cmdline, "\n");
	}
	else if (SCM_CMD == cmd)
	{
		strcpy (cmdline, "scm hush\n");
		strcat (cmdline, start);
		strcat (cmdline, "\n");
	}
#else
	else
	{
		conn->privmsg (msg_target, "Shell escapes disabled in this chatbot version\n");
		return 0;
	}
#endif /* ENABLE_SHELL_ESCAPES */

#define FLOOD_CHAR_COUNT 120

	size_t flood_cnt = FLOOD_CHAR_COUNT;
	size_t cnt = 0;
	bool dosend = true;

	// printf ("Sending to opencog: %s\n", cmdline);
	char * reply = whirr_sock_io (cmdline);
	free(cmdline);
	cmdline = NULL;

	printf ("opencog reply: %s\n", reply);

	/* Each newline has to be on its own line */
	/* Limit length of reply so we don't get kicked for flooding */
	char * p = reply;
	while (*p)
	{
		char *ep = strchr (p, '\n');

		// The last line -- no newline found.
		if (!ep)
		{
			if (is_nonblank(p))
				conn->privmsg (msg_target, p);
			break;
		}
		ep ++;
		int save = *ep;
		*ep = 0x0;

		// If the line starts with ":scm", resubmit it to the
		// server. This is a kind-of cheap, hacky way of doing
		// multi-processing.
		if (0 == strncmp(p, ":scm", 4))
		{
			char * cr = strchr(p, '\r');
			if (cr) *cr = '\n';
			char * r = whirr_sock_io (p+1);
			free(reply);
			reply = r;
			p = reply;
			printf ("opencog reply: %s\n", reply);
			continue;
		}

		// If the line starts with ":dbg", the do not send to chatroom
		if (0 == strncmp(p, ":dbg", 4))
		{
			*ep = save;
			p = ep;
			dosend = false;
			continue;
		}
		if (0 == strncmp(p, ":end-dbg", 8))
		{
			*ep = save;
			p = ep;
			dosend = true;
			continue;
		}

		// Else send output to chatroom
		if (dosend && is_nonblank(p))
		{
			conn->privmsg (msg_target, p);
			cnt += strlen (p);

			/* Sleep so that we don't get kicked for flooding */
			if (flood_cnt < cnt) { sleep(1); cnt -= FLOOD_CHAR_COUNT; }
			if (50 < flood_cnt) flood_cnt -= 15;
		}
		*ep = save;
		p = ep;
	}
	free(reply);

	return 0;
}

int got_kick(const char* params, irc_reply_data* ird, void* data)
{
	fixup_reply(ird);
	printf("got kicked -- input=%s\n", params);
	printf("nick=%s ident=%s host=%s target=%s\n", ird->nick, ird->ident, ird->host, ird->target);
	return 0;
}

/**
 * @todo allow command line options via tclap http://tclap.sourceforge.net/ -
 * package libtclap-dev in Ubuntu.
 * However, its probably more portable to use plain-old getopt,
 * or maybe getopt_long, lets keep the dependency list minimal.
 * @todo use Config class to store defaults, and retrieve opencog.conf vars.
 */
int main (int argc, char * argv[])
{
	whirr_sock_setup();

	IRC conn;

	if (cc.parseOptions(argc,argv)) return 0;

	conn.hook_irc_command("376", &end_of_motd);
	conn.hook_irc_command("PRIVMSG", &got_privmsg);
	conn.hook_irc_command("KICK", &got_kick);

	const char *login = getlogin();

	// The login-name, nick, etc. are there only to make it look
	// pretty on IRC ident.
	conn.start (cc.ircNetwork.c_str(), cc.ircPort, cc.nick.c_str(), login,
	            "La Cogita OpenCog chatbot", "asdf");

	conn.message_loop();

	fprintf(stderr, "%s: Fatal Error: Remote side closed socket\n",
		argv[0]);

	return 1;
}

<commit_msg>whoops!<commit_after>/*
 *   IRC interfaces for La Cogita IRC chatbot
 *   Copyright (C) 2007 Linas Vepstas <linas@linas.org>
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the 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 General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/*
 * Go onto IRC
 * This is pretty totally a pure hack with little/no design to it.
 * Linas October 2007
 */

#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <getopt.h>

#include <string>
#include <vector>
#include <set>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include "IRC.h"
#include "CogitaConfig.h"

#include "whirr-sockets.h"

using namespace opencog::chatbot;
using std::string;

CogitaConfig cc;

/* printf can puke if these fields are NULL */
void fixup_reply(irc_reply_data* ird)
{
	ird->nick = (NULL == ird->nick) ? (char *) "" : ird->nick;
	ird->ident = (NULL == ird->ident) ? (char *) "" : ird->ident;
	ird->host = (NULL == ird->host) ? (char *) "" : ird->host;
	ird->target = (NULL == ird->target) ? (char *) "" : ird->target;
}

/**
 * Join channel shortly after logging into the irc server.
 */
int end_of_motd(const char* params, irc_reply_data* ird, void* data)
{
	IRC* conn = (IRC*) data;
	fixup_reply(ird);

	printf("chatbot got params=%s\n", params);
	printf("chatbot got motd nick=%s ident=%s host=%s target=%s\n",
		ird->nick, ird->ident, ird->host, ird->target);

	sleep(1);
	conn->join (cc.ircChannels[0].c_str());
	printf("chatbot sent channel join %s\n", cc.ircChannels[0].c_str());
	sleep(2);
	conn->notice (cc.ircChannels[0].c_str(), "ola");
	printf("chatbot sent channel notice\n");
	sleep(2);
	conn->privmsg (cc.ircChannels[0].c_str(), "here we are");
	printf("chatbot said hello to the channel\n");
	return 0;
}

/* return true if not all whitespace */
static bool is_nonblank(const char * str)
{
	size_t len = strlen(str);
	if (0 == len) return false;
	size_t blanks = strspn(str, " \n\r\t\v");
	if (blanks == len) return false;
	return true;
}

/**
 * Handle a message received from IRC.
 */
int got_privmsg(const char* params, irc_reply_data* ird, void* data)
{
	IRC* conn = (IRC*) data;
	fixup_reply(ird);

	printf("input=%s\n", params);
	printf("nick=%s ident=%s host=%s target=%s\n", ird->nick, ird->ident, ird->host, ird->target);

	typedef enum {ENGLISH=1, SHELL_CMD, SCM_CMD} CmdType;
	CmdType cmd = ENGLISH;

	const char * start = NULL;
	int priv = 0;
	if (!strcmp (ird->target, cc.nick.c_str())) {priv = 1; start = params+1; }

	if (!strncmp (&params[1], cc.nick.c_str(), cc.nick.size())) {
		start = params+1 + cc.nick.size();
		start = strchr(start, ':');
		if (start) start ++;
	} else if (!strncmp (params, ":cog-sh:", 8)) {
		start = params+8; cmd = SHELL_CMD;
	} else if (!strncmp (params, ":scm:", 5)) {
		start = params+5; cmd = SCM_CMD;
	} else {
		// Check for alternative nick/attention strings
		foreach (string it, cc.attn) {
			if (! it.compare(0,it.size(),&params[1]) ) {
				start = params + it.size();
				start = strchr(start, ':');
				if (start) start ++;
				break;
			}
		}
	}

	if (!start) return 0;
	char * msg_target = NULL;
	if (priv)
	{
		msg_target = ird->nick;
	}
	else
	{
		msg_target = ird->target;
	}

	// Reply to request for chat client version
	if ((0x1 == start[0]) && !strncmp (&start[1], "VERSION", 7))
	{
		printf ("VERSION: %s\n", cc.vstring.c_str());
		conn->privmsg (msg_target, cc.vstring.c_str());
		return 0;
	}

	// printf ("duude starting with 0x%x %s\n", start[0], start);
	size_t textlen = strlen(start);
	size_t len = textlen;
	len += strlen ("(say-id-english )");
	len += strlen (ird->nick);
	len += 120;

	char * cmdline = (char *) malloc(sizeof (char) * (len+1));

	if (ENGLISH == cmd)
	{
		// Get into the opencog scheme shell, and run the command
		strcpy (cmdline, "scm hush\n(say-id-english \"");
		strcat (cmdline, ird->nick);
		strcat (cmdline, "\" \"");
		size_t toff = strlen(cmdline);
		strcat (cmdline, start);
		strcat (cmdline, "\")\n");

		// strip out quotation marks, replace with blanks, for now.
		for (size_t i =0; i<textlen; i++)
		{
			if ('\"' == cmdline[toff+i]) cmdline[toff+i] = ' ';
		}
	}

// #define ENABLE_SHELL_ESCAPES 0
#if ENABLE_SHELL_ESCAPES
	/*
	 * XXX DANGER DANGER Extreme Caution Advised XXX
	 * Shell escapes are a potential security hole, as they allow access
	 * to the cog-server to total strangers. In particular, the scheme
	 * interface is a general programming API and can be used to root
	 * the system.
	 */
	else if (SHELL_CMD == cmd)
	{
		strcpy (cmdline, start);
		strcat (cmdline, "\n");
	}
	else if (SCM_CMD == cmd)
	{
		strcpy (cmdline, "scm hush\n");
		strcat (cmdline, start);
		strcat (cmdline, "\n");
	}
#else
	else
	{
		conn->privmsg (msg_target, "Shell escapes disabled in this chatbot version\n");
		return 0;
	}
#endif /* ENABLE_SHELL_ESCAPES */

#define FLOOD_CHAR_COUNT 120

	size_t flood_cnt = FLOOD_CHAR_COUNT;
	size_t cnt = 0;
	bool dosend = true;

	// printf ("Sending to opencog: %s\n", cmdline);
	char * reply = whirr_sock_io (cmdline);
	free(cmdline);
	cmdline = NULL;

	printf ("opencog reply: %s\n", reply);

	/* Each newline has to be on its own line */
	/* Limit length of reply so we don't get kicked for flooding */
	char * p = reply;
	while (*p)
	{
		char *ep = strchr (p, '\n');

		// The last line -- no newline found.
		if (!ep)
		{
			if (is_nonblank(p))
				conn->privmsg (msg_target, p);
			break;
		}
		ep ++;
		int save = *ep;
		*ep = 0x0;

		// If the line starts with ":scm", resubmit it to the
		// server. This is a kind-of cheap, hacky way of doing
		// multi-processing.
		if (0 == strncmp(p, ":scm", 4))
		{
			char * cr = strchr(p, '\r');
			if (cr) *cr = '\n';
			char * r = whirr_sock_io (p+1);
			free(reply);
			reply = r;
			p = reply;
			printf ("opencog reply: %s\n", reply);
			continue;
		}

		// If the line starts with ":dbg", the do not send to chatroom
		if (0 == strncmp(p, ":dbg", 4))
		{
			*ep = save;
			p = ep;
			dosend = false;
			continue;
		}
		if (0 == strncmp(p, ":end-dbg", 8))
		{
			*ep = save;
			p = ep;
			dosend = true;
			continue;
		}

		// Else send output to chatroom
		if (dosend && is_nonblank(p))
		{
			conn->privmsg (msg_target, p);
			cnt += strlen (p);

			/* Sleep so that we don't get kicked for flooding */
			if (flood_cnt < cnt) { sleep(1); cnt -= FLOOD_CHAR_COUNT; }
			if (50 < flood_cnt) flood_cnt -= 15;
		}
		*ep = save;
		p = ep;
	}
	free(reply);

	return 0;
}

int got_kick(const char* params, irc_reply_data* ird, void* data)
{
	fixup_reply(ird);
	printf("got kicked -- input=%s\n", params);
	printf("nick=%s ident=%s host=%s target=%s\n", ird->nick, ird->ident, ird->host, ird->target);
	return 0;
}

/**
 * @todo allow command line options via tclap http://tclap.sourceforge.net/ -
 * package libtclap-dev in Ubuntu.
 * However, its probably more portable to use plain-old getopt,
 * or maybe getopt_long, lets keep the dependency list minimal.
 * @todo use Config class to store defaults, and retrieve opencog.conf vars.
 */
int main (int argc, char * argv[])
{
	whirr_sock_setup();

	IRC conn;

	if (cc.parseOptions(argc,argv)) return 0;

	conn.hook_irc_command("376", &end_of_motd);
	conn.hook_irc_command("PRIVMSG", &got_privmsg);
	conn.hook_irc_command("KICK", &got_kick);

	const char *login = getlogin();

	// The login-name, nick, etc. are there only to make it look
	// pretty on IRC ident.
	conn.start (cc.ircNetwork.c_str(), cc.ircPort, cc.nick.c_str(), login,
	            "La Cogita OpenCog chatbot", "asdf");

	conn.message_loop();

	fprintf(stderr, "%s: Fatal Error: Remote side closed socket\n",
		argv[0]);

	return 1;
}

<|endoftext|>


<commit_before>// OpenQASM example, executes an OpenQASM circuit read from the input stream or
// a file (if specified)
// Source: ./examples/qasm/qasm.cpp

#include <iostream>

#include "qpp.h"

int main(int argc, char** argv) {
    using namespace qpp;

    QCircuit qc;
    if (argc < 2)
        // read the circuit from the input stream
        qc = qasm::read(std::cin);
    else
        // read the circuit from a file
        qc = qasm::read_from_file(argv[1]);

    // initialize the quantum engine with a circuit
    QEngine q_engine{qc};

    // display the quantum circuit
    std::cout << ">> BEGIN CIRCUIT\n";
    std::cout << q_engine.get_circuit() << '\n';
    std::cout << ">> END CIRCUIT\n\n";

    // execute the quantum circuit
    q_engine.execute();

    // display the measurement statistics
    std::cout << ">> BEGIN ENGINE STATISTICS\n";
    std::cout << q_engine << '\n';
    std::cout << ">> END ENGINE STATISTICS\n\n";

    // display the final state
    ket psi_final = q_engine.get_psi();
    std::cout << ">> Final state:\n";
    std::cout << disp(psi_final) << '\n';
}
<commit_msg>qasm example update<commit_after>// OpenQASM example, executes an OpenQASM circuit read from the input stream or
// a file (if specified)
// Source: ./examples/qasm/qasm.cpp

#include <iostream>

#include "qpp.h"

int main(int argc, char** argv) {
    using namespace qpp;

    QCircuit qc;
    if (argc < 2)
        // read the circuit from the input stream
        qc = qasm::read(std::cin);
    else
        // read the circuit from a file
        qc = qasm::read_from_file(argv[1]);

    // initialize the quantum engine with a circuit
    QEngine q_engine{qc};

    // display the quantum circuit
    std::cout << ">> BEGIN CIRCUIT\n";
    std::cout << q_engine.get_circuit() << '\n';
    std::cout << ">> END CIRCUIT\n\n";

    // execute the quantum circuit
    q_engine.execute();

    // display the measurement statistics
    std::cout << ">> BEGIN ENGINE STATISTICS\n";
    std::cout << q_engine << '\n';
    std::cout << ">> END ENGINE STATISTICS\n\n";

    // display the final state
    ket psi_final = q_engine.get_psi();
    std::cout << ">> Final state:\n" << disp(psi_final) << '\n';
}
<|endoftext|>


<commit_before>#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iostream>

#include <openssl/ssl.h>

#include "aktualizr_secondary_config.h"

#include "logging.h"

namespace bpo = boost::program_options;

void check_info_options(const bpo::options_description &description, const bpo::variables_map &vm) {
  if (vm.count("help") != 0) {
    std::cout << description << '\n';
    exit(EXIT_SUCCESS);
  }
  if (vm.count("version") != 0) {
    std::cout << "Current aktualizr-secondary version is: " << AKTUALIZR_VERSION << "\n";
    exit(EXIT_SUCCESS);
  }
}

bpo::variables_map parse_options(int argc, char *argv[]) {
  bpo::options_description description("aktualizr-secondary command line options");
  // clang-format off
  description.add_options()
      ("help,h", "print usage")
      ("version,v", "Current aktualizr-secondary version")
      ("loglevel", bpo::value<int>(), "set log level 0-4 (trace, debug, warning, info, error)")
      ("config,c", bpo::value<std::string>()->required(), "toml configuration file")
      ("server-port,p", bpo::value<int>(), "command server listening port");
  // clang-format on

  bpo::variables_map vm;
  std::vector<std::string> unregistered_options;
  try {
    bpo::basic_parsed_options<char> parsed_options =
        bpo::command_line_parser(argc, argv).options(description).allow_unregistered().run();
    bpo::store(parsed_options, vm);
    check_info_options(description, vm);
    bpo::notify(vm);
    unregistered_options = bpo::collect_unrecognized(parsed_options.options, bpo::include_positional);
    if (vm.count("help") == 0 && !unregistered_options.empty()) {
      std::cout << description << "\n";
      exit(EXIT_FAILURE);
    }
  } catch (const bpo::required_option &ex) {
    if (ex.get_option_name() == "--config") {
      std::cout << ex.get_option_name() << " is missing.\nYou have to provide a valid configuration "
                                           "file using toml format. See the example configuration file "
                                           "in config/aktualizr_secondary/config.toml.example"
                << std::endl;
      exit(EXIT_FAILURE);
    } else {
      // print the error and append the default commandline option description
      std::cout << ex.what() << std::endl << description;
      exit(EXIT_SUCCESS);
    }
  } catch (const bpo::error &ex) {
    check_info_options(description, vm);

    // log boost error
    LOG_WARNING << "boost command line option error: " << ex.what();

    // print the error message to the standard output too, as the user provided
    // a non-supported commandline option
    std::cout << ex.what() << '\n';

    // set the returnValue, thereby ctest will recognize
    // that something went wrong
    exit(EXIT_FAILURE);
  }

  return vm;
}

/*****************************************************************************/
int main(int argc, char *argv[]) {
  logger_init();

  bpo::variables_map commandline_map = parse_options(argc, argv);

  // check for loglevel
  if (commandline_map.count("loglevel") != 0) {
    // set the log level from command line option
    boost::log::trivial::severity_level severity =
        static_cast<boost::log::trivial::severity_level>(commandline_map["loglevel"].as<int>());
    if (severity < boost::log::trivial::trace) {
      LOG_DEBUG << "Invalid log level";
      severity = boost::log::trivial::trace;
    }
    if (boost::log::trivial::fatal < severity) {
      LOG_WARNING << "Invalid log level";
      severity = boost::log::trivial::fatal;
    }
    if (severity <= boost::log::trivial::debug) {
      SSL_load_error_strings();
    }
    logger_set_threshold(severity);
  }

  LOG_INFO << "Aktualizr-secondary version " AKTUALIZR_VERSION " starting";
  LOG_DEBUG << "Current directory: " << boost::filesystem::current_path().string();
  // Initialize config with default values, the update with config, then with cmd
  boost::filesystem::path secondary_config_path(commandline_map["config"].as<std::string>());
  if (!boost::filesystem::exists(secondary_config_path)) {
    std::cout << "aktualizr-secondary: configuration file " << boost::filesystem::absolute(secondary_config_path)
              << " not found. Exiting." << std::endl;
    exit(EXIT_FAILURE);
  }

  // TODO: do something
  return 0;
}
<commit_msg>Fix naming typo in aktualizr-secondary<commit_after>#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iostream>

#include <openssl/ssl.h>

#include "aktualizr_secondary_config.h"

#include "logging.h"

namespace bpo = boost::program_options;

void check_secondary_options(const bpo::options_description &description, const bpo::variables_map &vm) {
  if (vm.count("help") != 0) {
    std::cout << description << '\n';
    exit(EXIT_SUCCESS);
  }
  if (vm.count("version") != 0) {
    std::cout << "Current aktualizr-secondary version is: " << AKTUALIZR_VERSION << "\n";
    exit(EXIT_SUCCESS);
  }
}

bpo::variables_map parse_options(int argc, char *argv[]) {
  bpo::options_description description("aktualizr-secondary command line options");
  // clang-format off
  description.add_options()
      ("help,h", "print usage")
      ("version,v", "Current aktualizr-secondary version")
      ("loglevel", bpo::value<int>(), "set log level 0-4 (trace, debug, warning, info, error)")
      ("config,c", bpo::value<std::string>()->required(), "toml configuration file")
      ("server-port,p", bpo::value<int>(), "command server listening port");
  // clang-format on

  bpo::variables_map vm;
  std::vector<std::string> unregistered_options;
  try {
    bpo::basic_parsed_options<char> parsed_options =
        bpo::command_line_parser(argc, argv).options(description).allow_unregistered().run();
    bpo::store(parsed_options, vm);
    check_secondary_options(description, vm);
    bpo::notify(vm);
    unregistered_options = bpo::collect_unrecognized(parsed_options.options, bpo::include_positional);
    if (vm.count("help") == 0 && !unregistered_options.empty()) {
      std::cout << description << "\n";
      exit(EXIT_FAILURE);
    }
  } catch (const bpo::required_option &ex) {
    if (ex.get_option_name() == "--config") {
      std::cout << ex.get_option_name() << " is missing.\nYou have to provide a valid configuration "
                                           "file using toml format. See the example configuration file "
                                           "in config/aktualizr_secondary/config.toml.example"
                << std::endl;
      exit(EXIT_FAILURE);
    } else {
      // print the error and append the default commandline option description
      std::cout << ex.what() << std::endl << description;
      exit(EXIT_SUCCESS);
    }
  } catch (const bpo::error &ex) {
    check_secondary_options(description, vm);

    // log boost error
    LOG_WARNING << "boost command line option error: " << ex.what();

    // print the error message to the standard output too, as the user provided
    // a non-supported commandline option
    std::cout << ex.what() << '\n';

    // set the returnValue, thereby ctest will recognize
    // that something went wrong
    exit(EXIT_FAILURE);
  }

  return vm;
}

/*****************************************************************************/
int main(int argc, char *argv[]) {
  logger_init();

  bpo::variables_map commandline_map = parse_options(argc, argv);

  // check for loglevel
  if (commandline_map.count("loglevel") != 0) {
    // set the log level from command line option
    boost::log::trivial::severity_level severity =
        static_cast<boost::log::trivial::severity_level>(commandline_map["loglevel"].as<int>());
    if (severity < boost::log::trivial::trace) {
      LOG_DEBUG << "Invalid log level";
      severity = boost::log::trivial::trace;
    }
    if (boost::log::trivial::fatal < severity) {
      LOG_WARNING << "Invalid log level";
      severity = boost::log::trivial::fatal;
    }
    if (severity <= boost::log::trivial::debug) {
      SSL_load_error_strings();
    }
    logger_set_threshold(severity);
  }

  LOG_INFO << "Aktualizr-secondary version " AKTUALIZR_VERSION " starting";
  LOG_DEBUG << "Current directory: " << boost::filesystem::current_path().string();
  // Initialize config with default values, the update with config, then with cmd
  boost::filesystem::path secondary_config_path(commandline_map["config"].as<std::string>());
  if (!boost::filesystem::exists(secondary_config_path)) {
    std::cout << "aktualizr-secondary: configuration file " << boost::filesystem::absolute(secondary_config_path)
              << " not found. Exiting." << std::endl;
    exit(EXIT_FAILURE);
  }

  // TODO: do something
  return 0;
}
<|endoftext|>


<commit_before>/*
    openGCM, geometric constraint manager
    Copyright (C) 2012  Stefan Troeger <stefantroeger@gmx.net>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the 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 General Public License for more detemplate tails.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#ifndef GCM_PARALLEL_H
#define GCM_PARALLEL_H

#include "geometry.hpp"

namespace gcm {

//the possible directions
enum Direction { Same, Opposite };

//the calculations( same as we always calculate directions we can outsource the work to this functions)
namespace parallel {

template<typename Kernel, typename T>
inline typename Kernel::number_type calc(T d1,
        T d2,
        Direction dir)  {

    switch(dir) {
        case Same:
            return (d1-d2).norm();
        case Opposite:
            return (d1+d2).norm();
    }
};

template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradFirst(T d1,
        T d2,
        T dd1,
        Direction dir)  {

    switch(dir) {
        case Same:
            return (d1-d2).dot(dd1) / (d1-d2).norm();
        case Opposite:
            return (d1+d2).dot(dd1) / (d1+d2).norm();
    }
};

template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradSecond(T d1,
        T d2,
        T dd2,
        Direction dir)  {

    switch(dir) {
        case Same:
            return (d1-d2).dot(-dd2) / (d1-d2).norm();
        case Opposite:
            return (d1+d2).dot(dd2) / (d1+d2).norm();
    }
};

template<typename Kernel, typename T>
inline void calcGradFirstComp(T d1,
                              T d2,
                              T grad,
                              Direction dir)  {

    switch(dir) {
        case Same:
            grad = (d1-d2) / (d1-d2).norm();
            return;
        case Opposite:
            grad = (d1+d2) / (d1+d2).norm();
            return;
    }
};

template<typename Kernel, typename T>
inline void calcGradSecondComp(T d1,
                               T d2,
                               T grad,
                               Direction dir)  {

    switch(dir) {
        case Same:
            grad = (d2-d1) / (d1-d2).norm();
            return;
        case Opposite:
            grad = (d2+d1) / (d1+d2).norm();
            return;
    }
};

}

template< typename Kernel, typename Tag1, typename Tag2 >
struct Parallel3D {

    typedef typename Kernel::number_type Scalar;
    typedef typename Kernel::VectorMap   Vector;
    Direction m_dir;

    Parallel3D(Direction d = Same) : m_dir(d) {
    //  Base::Console().Message("choosen direction (0=same, 1=opposite): %d\n",m_dir);
    };

    //template definition
    Scalar calculate(Vector& param1,  Vector& param2) {
        assert(false);
    };
    Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
        assert(false);
    };
    Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
        assert(false);
    };
    void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
        assert(false);
    };
    void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
        assert(false);
    };
};

template< typename Kernel >
struct Parallel3D< Kernel, tag::line3D, tag::line3D > {

    typedef typename Kernel::number_type Scalar;
    typedef typename Kernel::VectorMap   Vector;

    Direction m_dir;

    Parallel3D(Direction d = Same) : m_dir(d) {};

    //template definition
    Scalar calculate(Vector& param1,  Vector& param2) {
        return parallel::calc<Kernel>(param1.template tail<3>(), param2.template tail<3>(), m_dir);
    };
    Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
        return parallel::calcGradFirst<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam1.template tail<3>(), m_dir);
    };
    Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
        return parallel::calcGradSecond<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam2.template tail<3>(), m_dir);
    };
    void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradFirstComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), m_dir);
    };
    void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradSecondComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), m_dir);
    };
};

//planes like lines have the direction as segment 3-5, so we can use the same implementations
template< typename Kernel >
struct Parallel3D< Kernel, tag::plane3D, tag::plane3D > : public Parallel3D<Kernel, tag::line3D, tag::line3D> {
  Parallel3D(Direction d = Same) : Parallel3D<Kernel, tag::line3D, tag::line3D>(d) {};
};
template< typename Kernel >
struct Parallel3D< Kernel, tag::line3D, tag::plane3D > : public Parallel3D<Kernel, tag::line3D, tag::line3D> {
  Parallel3D(Direction d = Same) : Parallel3D<Kernel, tag::line3D, tag::line3D>(d) {};
};

template< typename Kernel >
struct Parallel3D< Kernel, tag::cylinder3D, tag::cylinder3D > {

    typedef typename Kernel::number_type Scalar;
    typedef typename Kernel::VectorMap   Vector;

    Direction m_dir;

    Parallel3D(Direction d = Same) : m_dir(d) {
      Base::Console().Message("Create parrallel cylinder");
    };

    //template definition
    Scalar calculate(Vector& param1,  Vector& param2) {
        return parallel::calc<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), m_dir);
    };
    Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
        return parallel::calcGradFirst<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), dparam1.template segment<3>(3), m_dir);
    };
    Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
        return parallel::calcGradSecond<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), dparam2.template segment<3>(3), m_dir);
    };
    void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradFirstComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), gradient.template segment<3>(3), m_dir);
    };
    void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradSecondComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), gradient.template segment<3>(3), m_dir);
    };
};
}

#endif //GCM_ANGLE<commit_msg>first take on bidirectional parallel constraint<commit_after>/*
    openGCM, geometric constraint manager
    Copyright (C) 2012  Stefan Troeger <stefantroeger@gmx.net>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the 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 General Public License for more detemplate tails.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#ifndef GCM_PARALLEL_H
#define GCM_PARALLEL_H

#include "geometry.hpp"

namespace gcm {

//the possible directions
enum Direction { Same, Opposite, Both };

//the calculations( same as we always calculate directions we can outsource the work to this functions)
namespace parallel {

template<typename Kernel, typename T>
inline typename Kernel::number_type calc(T d1,
        T d2,
        Direction dir)  {

    switch(dir) {
        case Same:
            return (d1-d2).norm();
        case Opposite:
            return (d1+d2).norm();
        case Both:
            return (d1/d1.norm() + d2/d2.norm()).norm();
    }
};

template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradFirst(T d1,
        T d2,
        T dd1,
        Direction dir)  {

    switch(dir) {
        case Same:
            return (d1-d2).dot(dd1) / (d1-d2).norm();
        case Opposite:
            return (d1+d2).dot(dd1) / (d1+d2).norm();
        case Both:
            const typename Kernel::number_type nd1 = d1.norm();
            const typename Kernel::Vector3 f = d1/nd1 + d2/d2.norm();
            return f.dot(dd1/nd1 - d1*d1.dot(dd1)/std::pow(nd1,3)) / f.norm();
    }
};

template<typename Kernel, typename T>
inline typename Kernel::number_type calcGradSecond(T d1,
        T d2,
        T dd2,
        Direction dir)  {

    switch(dir) {
        case Same:
            return (d1-d2).dot(-dd2) / (d1-d2).norm();
        case Opposite:
            return (d1+d2).dot(dd2) / (d1+d2).norm();
        case Both:
            const typename Kernel::number_type nd2 = d2.norm();
            const typename Kernel::Vector3 f = d1/d1.norm() + d2/nd2;
            return f.dot(dd2/nd2 - d2*d2.dot(dd2)/std::pow(nd2,3)) / f.norm();
    }
};

template<typename Kernel, typename T>
inline void calcGradFirstComp(T d1,
                              T d2,
                              T grad,
                              Direction dir)  {

    switch(dir) {
        case Same:
            grad = (d1-d2) / (d1-d2).norm();
            return;
        case Opposite:
            grad = (d1+d2) / (d1+d2).norm();
            return;
        case Both:
            assert(false);
    }
};

template<typename Kernel, typename T>
inline void calcGradSecondComp(T d1,
                               T d2,
                               T grad,
                               Direction dir)  {

    switch(dir) {
        case Same:
            grad = (d2-d1) / (d1-d2).norm();
            return;
        case Opposite:
            grad = (d2+d1) / (d1+d2).norm();
            return;
        case Both:
            assert(false);
    }
};

}

template< typename Kernel, typename Tag1, typename Tag2 >
struct Parallel3D {

    typedef typename Kernel::number_type Scalar;
    typedef typename Kernel::VectorMap   Vector;
    Direction m_dir;

    Parallel3D(Direction d = Same) : m_dir(d) {
        //  Base::Console().Message("choosen direction (0=same, 1=opposite): %d\n",m_dir);
    };

    //template definition
    Scalar calculate(Vector& param1,  Vector& param2) {
        assert(false);
    };
    Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
        assert(false);
    };
    Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
        assert(false);
    };
    void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
        assert(false);
    };
    void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
        assert(false);
    };
};

template< typename Kernel >
struct Parallel3D< Kernel, tag::line3D, tag::line3D > {

    typedef typename Kernel::number_type Scalar;
    typedef typename Kernel::VectorMap   Vector;

    Direction m_dir;

    Parallel3D(Direction d = Same) : m_dir(d) {};

    //template definition
    Scalar calculate(Vector& param1,  Vector& param2) {
        return parallel::calc<Kernel>(param1.template tail<3>(), param2.template tail<3>(), m_dir);
    };
    Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
        return parallel::calcGradFirst<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam1.template tail<3>(), m_dir);
    };
    Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
        return parallel::calcGradSecond<Kernel>(param1.template tail<3>(), param2.template tail<3>(), dparam2.template tail<3>(), m_dir);
    };
    void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradFirstComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), m_dir);
    };
    void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradSecondComp<Kernel>(param1.template tail<3>(), param2.template tail<3>(), gradient.template tail<3>(), m_dir);
    };
};

//planes like lines have the direction as segment 3-5, so we can use the same implementations
template< typename Kernel >
struct Parallel3D< Kernel, tag::plane3D, tag::plane3D > : public Parallel3D<Kernel, tag::line3D, tag::line3D> {
    Parallel3D(Direction d = Same) : Parallel3D<Kernel, tag::line3D, tag::line3D>(d) {};
};
template< typename Kernel >
struct Parallel3D< Kernel, tag::line3D, tag::plane3D > : public Parallel3D<Kernel, tag::line3D, tag::line3D> {
    Parallel3D(Direction d = Same) : Parallel3D<Kernel, tag::line3D, tag::line3D>(d) {};
};

template< typename Kernel >
struct Parallel3D< Kernel, tag::cylinder3D, tag::cylinder3D > {

    typedef typename Kernel::number_type Scalar;
    typedef typename Kernel::VectorMap   Vector;

    Direction m_dir;

    Parallel3D(Direction d = Same) : m_dir(d) {
        Base::Console().Message("Create parrallel cylinder");
    };

    //template definition
    Scalar calculate(Vector& param1,  Vector& param2) {
        return parallel::calc<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), m_dir);
    };
    Scalar calculateGradientFirst(Vector& param1, Vector& param2, Vector& dparam1) {
        return parallel::calcGradFirst<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), dparam1.template segment<3>(3), m_dir);
    };
    Scalar calculateGradientSecond(Vector& param1, Vector& param2, Vector& dparam2) {
        return parallel::calcGradSecond<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), dparam2.template segment<3>(3), m_dir);
    };
    void calculateGradientFirstComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradFirstComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), gradient.template segment<3>(3), m_dir);
    };
    void calculateGradientSecondComplete(Vector& param1, Vector& param2, Vector& gradient) {
        gradient.template head<3>().setZero();
        parallel::calcGradSecondComp<Kernel>(param1.template segment<3>(3), param2.template segment<3>(3), gradient.template segment<3>(3), m_dir);
    };
};
}

#endif //GCM_ANGLE
<|endoftext|>


<commit_before>/** @file Tests to make sure that invalid certificates fail to connect */
#include "catch.hpp"

#include <communique/Client.h>
#include <communique/Server.h>
#include <thread>
#include <iostream>

#include "testinputs.h"

SCENARIO( "Test that server setup fails when given invalid security files", "[security][local]" )
{
	WHEN( "I create a server with invalid certificate file" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setCertificateChainFile( testinputs::testFileDirectory+"blahblahblah.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}

	WHEN( "I create a server with invalid key file" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setPrivateKeyFile( testinputs::testFileDirectory+"blahblahblah.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}

	WHEN( "I create a server with key in place of the certificate" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setCertificateChainFile( testinputs::testFileDirectory+"server_key.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}

	WHEN( "I create a server with certificate in place of the key" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_cert.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}
}

SCENARIO( "Test that client setup fails when given invalid security files", "[security][local]" )
{
	WHEN( "I create a client with invalid verification file" )
	{
		communique::Client myClient;
		REQUIRE_NOTHROW( myClient.setVerifyFile( testinputs::testFileDirectory+"blahblahblah.pem" ) );
		REQUIRE_THROWS( myClient.connect( "wss://echo.websocket.org" ) );
	}
}

SCENARIO( "Test that an incorrect server certificate fails ", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"old/server.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"old/server.pem" );

		communique::Client myClient;
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( !myClient.isConnected() );

			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}

SCENARIO( "Test that connection fails when server requires client authentication, and client doesn't authenticate", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"server_cert.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_key.pem" );
		myServer.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		communique::Client myClient;
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( !myClient.isConnected() );

			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}

SCENARIO( "Test that client authentication works", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"server_cert.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_key.pem" );
		myServer.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		communique::Client myClient;
		myClient.setCertificateChainFile( testinputs::testFileDirectory+"client_cert.pem" );
		myClient.setPrivateKeyFile( testinputs::testFileDirectory+"client_key.pem" );
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( myClient.isConnected() );

			REQUIRE_NOTHROW( myClient.disconnect() );
			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}
<commit_msg>Add a test with an expired server certificate<commit_after>/** @file Tests to make sure that invalid certificates fail to connect */
#include "catch.hpp"

#include <communique/Client.h>
#include <communique/Server.h>
#include <thread>
#include <iostream>

#include "testinputs.h"

SCENARIO( "Test that server setup fails when given invalid security files", "[security][local]" )
{
	WHEN( "I create a server with invalid certificate file" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setCertificateChainFile( testinputs::testFileDirectory+"blahblahblah.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}

	WHEN( "I create a server with invalid key file" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setPrivateKeyFile( testinputs::testFileDirectory+"blahblahblah.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}

	WHEN( "I create a server with key in place of the certificate" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setCertificateChainFile( testinputs::testFileDirectory+"server_key.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}

	WHEN( "I create a server with certificate in place of the key" )
	{
		communique::Server myServer;
		REQUIRE_NOTHROW( myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_cert.pem" ) );
		REQUIRE_THROWS( myServer.listen( ++testinputs::portNumber ) );
	}
}

SCENARIO( "Test that client setup fails when given invalid security files", "[security][local]" )
{
	WHEN( "I create a client with invalid verification file" )
	{
		communique::Client myClient;
		REQUIRE_NOTHROW( myClient.setVerifyFile( testinputs::testFileDirectory+"blahblahblah.pem" ) );
		REQUIRE_THROWS( myClient.connect( "wss://echo.websocket.org" ) );
	}
}

SCENARIO( "Test that an incorrect server certificate fails ", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"old/server.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"old/server.pem" );

		communique::Client myClient;
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( !myClient.isConnected() );

			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}

SCENARIO( "Test that connection fails when server requires client authentication, and client doesn't authenticate", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"server_cert.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_key.pem" );
		myServer.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		communique::Client myClient;
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( !myClient.isConnected() );

			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}

SCENARIO( "Test that client authentication works", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"server_cert.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_key.pem" );
		myServer.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		communique::Client myClient;
		myClient.setCertificateChainFile( testinputs::testFileDirectory+"client_cert.pem" );
		myClient.setPrivateKeyFile( testinputs::testFileDirectory+"client_key.pem" );
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( myClient.isConnected() );

			REQUIRE_NOTHROW( myClient.disconnect() );
			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}

SCENARIO( "Test that authentication fails when the server certificate has expired", "[security][local]" )
{
	GIVEN( "A Client and server" )
	{
		communique::Server myServer;
		myServer.setCertificateChainFile( testinputs::testFileDirectory+"expired_server_cert.pem" );
		myServer.setPrivateKeyFile( testinputs::testFileDirectory+"server_key.pem" );
		myServer.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		communique::Client myClient;
		myClient.setCertificateChainFile( testinputs::testFileDirectory+"client_cert.pem" );
		myClient.setPrivateKeyFile( testinputs::testFileDirectory+"client_key.pem" );
		myClient.setVerifyFile( testinputs::testFileDirectory+"certificateAuthority_cert.pem" );

		WHEN( "I start a server listening and try and connect a client to it" )
		{
			REQUIRE_NOTHROW( myServer.listen( ++testinputs::portNumber ) );
			std::this_thread::sleep_for( testinputs::shortWait );

			REQUIRE_NOTHROW( myClient.connect( "ws://localhost:"+std::to_string(testinputs::portNumber) ) );

			REQUIRE( !myClient.isConnected() );

			REQUIRE_NOTHROW( myClient.disconnect() );
			REQUIRE_NOTHROW( myServer.stop() );
		}
	}
}
<|endoftext|>


<commit_before>/**
 * SD Card Read/Write Test
 * 4/5/2017
 *
**/
//#include <stdlib.h>
//#include <stdio.h>

#include "mbed.h"
#include "pinout.h"

#include "SDFileSystem.h"

#include "imu.h"
#include "GPS.h"
#include "QEI.h"
#include "motor.h"
#include "PwmIn.h"

/* map structure */
typedef struct Point {
    int x; //need to change to GPS relevent values
    int y;
} Point;

typedef struct Map {

  Point nw; //NorthWest corner of the map
  Point ne; //NorthEast corner of the map
  Point sw; //southwest corner of the map
  Point se; //southEast corner of the map

  float mAccel; //Maximum allowed acceleration
  float mVel;   //Maximum allowed velocity

  int npoints; //number of map points to run
  Point *path; //array of map points to be run

} Map;

int readMap(FILE *fp, Map *mp) {
//    char *line;
//    int len;

    if (!fp)
        return -1;

/*    while(line = getline(&line, &len, fp) != -1) {
        printf("%s\r\n", line);
    }
*/
    return 0;
}
int checkMap(Map *mp) {
    return 0;
}

//Function allows driving by RC controler
//Note: this is bad organisation for the final, the only function
//  in the final program that can manipulate the motors directly
//  should be main or a function only called from main who's only job
//  is to set the motor speeds;
int modeRC(float throtle, float lrat, int *mr, int *ml) {
	throtle *= 1000000;
	throtle -= 1100;
	throtle /= 8;

	lrat *= 1000000;
	lrat -= 1000;
	lrat /= 1000;
	*ml = (int)((1 - lrat) * throtle);
	*mr = (int)(lrat * throtle);

	return 0;
}

/* Test Objects */
Serial Pc(USBTX,USBRX);

SDFileSystem sd(DI, DO, CLK, CS, "sd");
IMU imu(I2C_SDA, I2C_SCL, BNO055_G_CHIP_ADDR, true);

/* Motor Objects */
MOTOR MotorR(1, IN2A, IN1A, ENA, true);
MOTOR MotorL(2, IN2B, IN1B, ENB, true);

/* Encoder Objects */
QEI EncoderL(CHA1, CHB1, NC, 192, QEI::X4_ENCODING);
QEI EncoderR(CHA2, CHB2, NC, 192, QEI::X4_ENCODING);

/* IMU Objects */
IMU Imu(IMDA, IMCL, BNO055_G_CHIP_ADDR, true);

/* GPS Objects */
GPS Gps(GPTX, GPRX);

/* Radio Objects */
PwmIn Throt(THRO);
PwmIn Lr(LRIN);
PwmIn Mode(MODE);
PwmIn E_Stop(ESTO);

/* Input Buttons */
DigitalIn Btn(PC_0);
DigitalIn Dip(PB_6);

int main()
{
    Map mp;
    int pCount = 0;

    //radio variables
    float estop;
    int mr, ml, md;

    // Creates variables of reading data types
    IMU::imu_euler_t euler;
    IMU::imu_lin_accel_t linAccel;
    IMU::imu_gravity_t grav;

    //Mount the filesystem
    printf("Mounting SD card\r\n");
    sd.mount();
    FILE *ofp = fopen("/sd/data.txt", "w");
    FILE *ifp = fopen("/sd/map.mp", "r");

	//infinite loop if SD card not found
	if (ofp == NULL) {
		Pc.printf("SD card not found\r\n");
		while(1) ;
	}
    //open map file
	if (ifp == NULL) {
		Pc.printf("No Map File Found! ABORT!\r\n");
		while(1) ;
	}

    //parse map file into Map structure
    if (readMap(ifp, &mp)) {
        Pc.printf("Map parse failed\r\n");
        while(1);
    }

    //review the map structure for errors
    if (checkMap(&mp)) {
        Pc.printf("Map does not fit rules\r\n");
        while(1);
    }
    //close the map file, won't be using it anymore
    fclose(ifp);
    Pc.printf("FileSystem ready\r\n");

	//Initialise motors
	MotorL.start(0);
	MotorR.start(0);
    Pc.printf("Motors initialised\r\n");

    Pc.printf("Waiting on user GO\r\n");
	while ((estop = E_Stop.pulsewidth() * 1000000) < 1150 &&
            estop > 1090)
        ;
	while ((estop = E_Stop.pulsewidth() * 1000000) != 1096)
        ;

    Pc.printf("User GO accepted starting run\r\n");
	while((estop = E_Stop.pulsewidth() * 1000000) < 1150 &&
           estop > 1090) {
        fprintf(ofp, "Data Point: %d\r\n",pCount);
        //Check DIP2 to activate the motors
		if(Btn && (md = Mode.pulsewidth() * 1000000) > 1450 &&
                   md < 1550) {

			modeRC(Throt.pulsewidth(), Lr.pulsewidth(), &mr, &ml);
			MotorL.start(ml);
			MotorR.start(mr);

            Pc.printf("Motor ON\tMode:%f\t",Mode.pulsewidth() );
            Pc.printf("Motor Left: %d\tMotor Right: %d\r\n", ml, mr);
			fprintf(ofp, "Motor ON\t");
            Pc.printf("Motor Left: %d\tMotor Right: %d\r\n", ml, mr);
		} else {
			MotorL.start(0);
			MotorR.start(0);
            Pc.printf("Motor OFF\r\n");
			fprintf(ofp, "Motor OFF\r\n");
		}

        //Read data from GPS
        if(Gps.parseData()) {
            fprintf(ofp, "time: %f\r\n",Gps.time);
            fprintf(ofp, "latatude: %f\r\n", Gps.latitude);
            fprintf(ofp, "longitude: %f\r\n", Gps.longitude);
            fprintf(ofp, "Satilites: %d\r\n", Gps.satellites);
        } else {
            fprintf(ofp, "GPS: No LOCK\r\n");
        }

        //Read data from encoders
        fprintf(ofp, "EncoderL: %d\r\n", EncoderL.getPulses());
        fprintf(ofp, "EncoderR: %d\r\n", EncoderR.getPulses());
        EncoderL.reset();
        EncoderR.reset();

        //Read data from IMU
        imu.getEulerAng(&euler);
        imu.getLinAccel(&linAccel);
        imu.getGravity(&grav);

        fprintf(ofp, "Data Point %d\r\n", pCount);
        fprintf(ofp, "Heading: %f Pitch: %f Roll: %f\r\n",
                euler.heading, euler.pitch, euler.roll);
        fprintf(ofp, "LinX: %f LinY: %f LinZ: %f\r\n",
                linAccel.x, linAccel.y, linAccel.z);
        fprintf(ofp, "GravX: %f GravY: %f GravZ: %f\r\n",
                grav.x, grav.y, grav.z);
        fprintf(ofp, "\r\n");
        pCount++;
        wait_ms(10);
    }

    MotorL.start(0);
    MotorR.start(0);
    //Unmount the filesystem
    fprintf(ofp,"End of Program\r\n");
    fclose(ofp);
    printf("Unmounting SD card\r\n");
    sd.unmount();
    Pc.printf("SD card unmounted\r\n");

    Pc.printf("Program Terminated\r\n");
    while(1);
}

<commit_msg>cleaned up the data collection program, output should be csv (untested)<commit_after>/**
 * SD Card Read/Write Test
 * 4/5/2017
 *
**/
//#include <stdlib.h>
//#include <stdio.h>

#include "mbed.h"
#include "pinout.h"

#include "SDFileSystem.h"

#include "imu.h"
#include "GPS.h"
#include "QEI.h"
#include "motor.h"
#include "PwmIn.h"

/* map structure */
typedef struct Point {
    int time;
    int x; //need to change to GPS relevent values
    int y;
    int dir; //need to change to imu relevent values
    int vel;
} Point;

typedef struct Map {

  Point nw; //NorthWest corner of the map
  Point ne; //NorthEast corner of the map
  Point sw; //southwest corner of the map
  Point se; //southEast corner of the map

  float mAccel; //Maximum allowed acceleration
  float mVel;   //Maximum allowed velocity

  int npoints; //number of map points to run
  Point *path; //array of map points to be run

} Map;

int readMap(FILE *fp, Map *mp) {
//    char *line;
//    int len;

    if (!fp)
        return -1;

/*    while(line = getline(&line, &len, fp) != -1) {
        printf("%s\r\n", line);
    }
*/
    return 0;
}
int checkMap(Map *mp) {
    return 0;
}

//Function allows driving by RC controler
int modeRC(float throtle, float lrat, int *mr, int *ml) {
	throtle *= 1000000;
	throtle -= 1100;
	throtle /= 8;

	lrat *= 1000000;
	lrat -= 1000;
	lrat /= 1000;
	*ml = (int)((1 - lrat) * throtle);
	*mr = (int)(lrat * throtle);

	return 0;
}

/* Test Objects */
Serial Pc(USBTX,USBRX);

/* file system objects */
SDFileSystem sd(DI, DO, CLK, CS, "sd");

/* IMU objects */
IMU imu(I2C_SDA, I2C_SCL, BNO055_G_CHIP_ADDR, true);

/* Motor Objects */
MOTOR MotorR(1, IN2A, IN1A, ENA, true);
MOTOR MotorL(2, IN2B, IN1B, ENB, true);

/* Encoder Objects */
QEI EncoderL(CHA1, CHB1, NC, 192, QEI::X4_ENCODING);
QEI EncoderR(CHA2, CHB2, NC, 192, QEI::X4_ENCODING);

/* IMU Objects */
IMU Imu(IMDA, IMCL, BNO055_G_CHIP_ADDR, true);

/* GPS Objects */
GPS Gps(GPTX, GPRX);

/* Radio Objects */
PwmIn Throt(THRO);
PwmIn Lr(LRIN);
PwmIn Mode(MODE);
PwmIn E_Stop(ESTO);

/* Input Buttons */
DigitalIn Btn(PC_0);
DigitalIn Dip(PB_6);

int main()
{
    Map mp;
    int pCount = 0;

    //radio variables
    float throtle, leftright, mode, estop;

    //motor variables
    int mr, ml;

    //encoder variables
    int lenc, renc;

    //gps variables
    int lock;

    // Creates variables of reading data types
    IMU::imu_euler_t euler;
    IMU::imu_lin_accel_t linAccel;
    IMU::imu_gravity_t grav;

    //Mount the filesystem
    printf("Mounting SD card\r\n");
    sd.mount();
    FILE *ofp = fopen("/sd/data.txt", "w");
    FILE *ifp = fopen("/sd/map.mp", "r");

	//infinite loop if SD card not found
	if (ofp == NULL) {
		Pc.printf("SD card not found\r\n");
		while(1) ;
	}
    //open map file
	if (ifp == NULL) {
		Pc.printf("No Map File Found! ABORT!\r\n");
		while(1) ;
	}

    //parse map file into Map structure
    if (readMap(ifp, &mp)) {
        Pc.printf("Map parse failed\r\n");
        while(1);
    }

    //review the map structure for errors
    if (checkMap(&mp)) {
        Pc.printf("Map does not fit rules\r\n");
        while(1);
    }
    //close the map file, won't be using it anymore
    fclose(ifp);
    Pc.printf("FileSystem ready\r\n");

	//Initialise motors
	MotorL.start(0);
	MotorR.start(0);
    Pc.printf("Motors initialised\r\n");

    Pc.printf("Waiting on user GO\r\n");
    //estop must transition from low to high to activate vehical
	while ((estop = E_Stop.pulsewidth() * 1000000) < 1150 &&
            estop > 1090)
        ;
	while ((estop = E_Stop.pulsewidth() * 1000000) != 1096)
        ;

    //print collumn catagories
    Pc.printf("User GO accepted starting run\r\n");
    fprintf(ofp, "Point#, nearest waypoint, next waypoint, ");
    fprintf(ofp, "rcThrot, rcDir, rcE-stop, rcMode, ");
    fprintf(ofp, "time, lat, long, #sat, ");
    fprintf(ofp, "xAcc, yAcc, zAcc, heading, pitch, role, xGra, yGra, zGra, ");
    fprintf(ofp, "lEncoder, rEncoder, lMotor, rMotor\r\n");

    //main loop, breaks out if estop tripped
	while((estop = E_Stop.pulsewidth() * 1000000) < 1150 && estop > 1090) {

        ////////////////////////////Gather Data
        //get radio values
        throtle = Throt.pulsewidth();
        mode = Mode.pulsewidth();
        leftright = Lr.pulsewidth();
        //Read data from IMU
        imu.getEulerAng(&euler);
        imu.getLinAccel(&linAccel);
        imu.getGravity(&grav);
        //get encoder data and reset encoders
        lenc = EncoderL.getPulses();
        renc = EncoderR.getPulses();
        EncoderL.reset();
        EncoderR.reset();
        //get gps data
        lock = Gps.parseData();

        //////////////////////////Use Data to make decisions
        //Check Dip2 && radio state then set the motor variables accordingly
		if(Btn && (mode * 1000000) > 1450 && mode < 1550) {
            //Radio control mode
			modeRC(throtle, leftright, &mr, &ml);
		} else {
            //all other states atmo are just dead stop
			ml = 0;
		    mr = 0;
		}

        ///////////////////////////Record data and decisions to file
        //record map relevent data (not currently used)
        fprintf(ofp, "%d, %d, %d, ", pCount, 0, 0);
        //record radio values
        fprintf(ofp, "%f, %f, %f, %f, ", throtle, leftright, estop, mode);
        //record gps data if available
        if (lock) {
            fprintf(ofp, "%f, %f, %f, %d, ", Gps.time, Gps.latitude,
                Gps.longitude, Gps.satellites);
        } else {
            fprintf(ofp, "NL, NL, NL, NL, NL, ");
        }
        //record data from IMU
        fprintf(ofp, "%f, %f, %f, ", linAccel.x, linAccel.y, linAccel.z);
        fprintf(ofp, "%f, %f, %f, ", euler.heading, euler.pitch, euler.roll);
        fprintf(ofp, "%f, %f, %f, ", grav.x, grav.y, grav.z);
        //record encoder data
        fprintf(ofp, "%d, %d, ", lenc, renc);
        //record motor variables
        fprintf(ofp, "%d, %d\r\n", ml, mr);

        /////////////////////////////use decisions to change motors
        //Set motors
	    MotorL.start(ml);
		MotorR.start(mr);

        ////////////////////////////end of loop cleanup and multiloop funcs
        //Increment data point count
        pCount++;
        //delay 10ms, this may be removed in future if we use a heavy algorithm
        wait_ms(10);
    }

    //power down motors
    MotorL.start(0);
    MotorR.start(0);

    //Unmount the filesystem
    fprintf(ofp,"End of Program\r\n");
    fclose(ofp);
    printf("Unmounting SD card\r\n");
    sd.unmount();
    Pc.printf("SD card unmounted\r\n");

    Pc.printf("Program Terminated\r\n");
    while(1);
}

<|endoftext|>