<commit_before>/* indivudal.cpp - CS 472 Project #2: Genetic Programming
 * Copyright 2014 Andrew Schwartzmeyer
 *
 * Source file for individual namespace
 */

#include <algorithm>
#include <cassert>
#include <cmath>
#include <iostream>
#include <memory>
#include <tuple>
#include <vector>

#include "individual.hpp"
#include "../options/options.hpp"
#include "../random_generator/random_generator.hpp"

namespace individual
{
  using std::vector;
  using std::string;
  using namespace random_generator;

  // Default Size struct constructor.
  Size::Size(): internals{0}, leaves{0}, depth{0} {}

  using F = Function;
  // Vectors of same-arity function enums.
  vector<F> nullaries {F::left, F::right, F::forward};
  vector<F> binaries {F::prog2, F::iffoodahead};
  vector<F> trinaries {F::prog3};

  /* Vectors of available function enums.  Should be moved into
     Options struct. */
  vector<F> leaves = nullaries;
  vector<F> internals {F::prog2, F::prog3, F::iffoodahead};

  // Returns a random function from a given set of functions.
  Function
  get_function(const vector<Function>& functions)
  {
    int_dist dist{0, static_cast<int>(functions.size()) - 1}; // closed interval
    return functions[dist(rg.engine)];
  }

  // Returns bool of whether or not the item is in the set.
  template<typename I, typename S> bool
  contains(const I& item, const S& set)
  {
    return std::find(set.begin(), set.end(), item) != set.end();
  }

  // Returns the appropriate arity for a given function.
  unsigned int
  get_arity(const Function& function)
  {
    if (contains(function, nullaries))
      return 0;
    else if (contains(function, binaries))
      return 2;
    else if (contains(function, trinaries))
      return 3;
    assert(false);
  }

  // Default constructor for "empty" node
  Node::Node(): function{Function::nil}, arity{0} {}

  /* Recursively constructs a parse tree using the given method
     (either GROW or FULL). */
  Node::Node(const Method& method, const unsigned int& max_depth)
  {
    // Create leaf node if at the max depth or randomly (if growing).
    real_dist dist{0, 1};
    float grow_chance =
      static_cast<float>(leaves.size()) / (leaves.size() + internals.size());
    if (max_depth == 0
	or (method == Method::grow and dist(rg.engine) < grow_chance))
      {
	function = get_function(leaves);
	arity = 0; // get_arity(function); leaves are always zero
      }
    // Otherwise choose an internal node.
    else
      {
	function = get_function(internals);
	arity = get_arity(function);
	// Recursively create subtrees.
	children.reserve(arity);
	for (unsigned int i = 0; i < arity; ++i)
	  children.emplace_back(Node{method, max_depth - 1});
      }
    assert(function != Function::nil); // do not create null types
    assert(children.size() == arity); // ensure arity
  }

  Node create(const unsigned int& max_depth, const float& chance)
  {
    real_dist method_dist{0, 1};
    Method method = (method_dist(rg.engine) < chance)
      ? Method::grow : Method::full;

    int_dist depth_dist{0, static_cast<int>(max_depth)};
    unsigned int depth = depth_dist(rg.engine);

    return Node{method, depth};
  }

  // Returns a string visually representing a particular node.
  string
  Node::represent() const
  {
    switch (function)
      {
      case F::nil:
	assert(false); // Never represent empty node.
      case F::prog2:
	return "prog-2";
      case F::prog3:
	return "prog-3";
      case F::iffoodahead:
	return "if-food-ahead";
      case F::left:
	return "left";
      case F::right:
	return "right";
      case F::forward:
	return "forward";
      }
    assert(false); // Every node should have been matched.
  }

  /* Returns string representation of expression in Polish/prefix
     notation using a pre-order traversal. */
  string
  Node::print() const
  {
    if (children.empty())
      return represent();

    string formula = "(" + represent();

    for (const Node& child : children)
      formula += " " + child.print();

    return formula + ")";
  }

  /* Evaluates an ant over a given map using a depth-first post-order
     recursive continuous evaluation of a decision tree. */
  void
  Node::evaluate(options::Map& map) const
  {
    if (not map.active()) return;

    switch (function)
      {
      case F::left:
	map.left(); // Terminal case
	break;
      case F::right:
	map.right(); // Terminal case
	break;
      case F::forward:
	map.forward(); // Terminal case
	break;
      case F::iffoodahead:
	if (map.look()) // Do left or right depending on if food ahead
	  children[0].evaluate(map);
	else
	  children[1].evaluate(map);
	break;
      case F::prog2: // Falls through
      case F::prog3:
	for (const Node& child : children)
	  child.evaluate(map);
	break;
      case F::nil:
	assert(false); // Never evaluate empty node
      }
  }

  /* Recursively count children and find maximum depth of tree via
     post-order traversal.  Keep track of internals, leaves, and depth
     via Size struct */
  const Size
  Node::size() const
  {
    Size size;

    if (children.empty())
      ++size.leaves;
    else
      {
	vector<unsigned int> depths;
	depths.reserve(arity);
	for (const Node& child : children)
	  {
	    Size temp = child.size();
	    size.internals += temp.internals;
	    size.leaves += temp.leaves;
	    depths.emplace_back(temp.depth);
	  }
	size.depth = 1 + *std::max_element(depths.begin(), depths.end());
	++size.internals;
      }
    return size;
  }

  // Used to represent "not-found" (similar to a NULL pointer).
  Node empty;

  /* Depth-first search for taget node.  Must be seeking either
     internal or leaf, cannot be both. */
  Node&
  Node::visit(const Size& i, Size& visiting)
  {
    for (Node& child : children) {
      // Increase relevant count.
      if (child.children.empty())
	++visiting.leaves;
      else
	++visiting.internals;

      // Return node reference if found.
      if (visiting.internals == i.internals or visiting.leaves == i.leaves)
	return child;
      else
	{
	  Node& temp = child.visit(i, visiting); // Recursive search.
	  if (temp.function != Function::nil)
	    return temp;
	}
    }
    return empty;
  }

  void
  Node::mutate_self()
  {
    if (arity == 0)
      {
	const Function old = function;
	while (function == old)
	  function = get_function(leaves);
      }
    else
      {
	const Function old = function;
	while (function == old)
	  function = get_function(internals);
	arity = get_arity(function);
	// Fix arity mismatches caused by mutation
	if (arity == 2 and children.size() == 3)
	  children.pop_back();
	else if (arity == 3 and children.size() == 2)
	  children.emplace_back(create(4, 0.5));
      }
    assert(arity == children.size());
    assert(function != Function::nil);
  }

  // Recursively mutate nodes with given probability.
  void
  Node::mutate_tree(const float& chance)
  {
    real_dist dist{0, 1};
    for (Node& child : children)
      {
	if (dist(rg.engine) < chance)
	  child.mutate_self();
	child.mutate_tree(chance);
      }
  }

  // Default constructor for Individual
  Individual::Individual() {}

  /* Create an Individual tree by having a root node (to which the
     actual construction is delegated). The depth is passed by value
     as its creation elsewhere is temporary. */
  Individual::Individual(const unsigned int depth, const float& chance,
			 options::Map map): fitness{0}, adjusted{0}
  {
    root = create(depth, chance);
    /* The evaluate method updates the size and both raw and adjusted
       fitnesses. */
    evaluate(map);
  }

  // Return string representation of a tree's size and fitness.
  string
  Individual::print() const
  {
    using std::to_string;

    string info = "# Size " + to_string(get_total()) + ", with "
      + to_string(get_internals()) + " internals, "
      + to_string(get_leaves()) + " leaves, and depth of "
      + to_string(get_depth()) + ".\n"
      + "# Score: " + to_string(score)
      + ", and adjusted fitness: " + to_string(adjusted) + ".\n";

    return info;
  }

  // Return string represenation of tree's expression (delegated).
  string
  Individual::print_formula() const
  {
    return "# Formula: " + root.print() + "\n";
  }

  // Read-only "getters" for private data

  unsigned int
  Individual::get_internals() const
  {
    return size.internals;
  }

  unsigned int
  Individual::get_leaves() const
  {
    return size.leaves;
  }

  unsigned int
  Individual::get_total() const
  {
    return size.internals + size.leaves;
  }

  unsigned int
  Individual::get_depth() const
  {
    return size.depth;
  }

  unsigned int
  Individual::get_score() const
  {
    return score;
  }

  float
  Individual::get_fitness() const
  {
    return fitness;
  }

  float
  Individual::get_adjusted() const
  {
    return adjusted;
  }

  /* Evaluate Individual for given values and calculate size.  Update
     Individual's size and fitness accordingly. Return non-empty
     string if printing. */
  string
  Individual::evaluate(options::Map map, const float& penalty,
		       const bool& print)
  {
    // Update size on evaluation because it's incredibly convenient.
    size = root.size();

    while (map.active())
      root.evaluate(map);

    score = map.fitness();
    // Adjusted fitness does not have size penalty.
    adjusted = static_cast<float>(score) / map.max();
    // Apply size penalty if not printing.
    fitness = score - penalty * get_total();

    string evaluation;
    if (print) evaluation = map.print();

    return evaluation;
  }

  // Mutate each node with given probability.
  void
  Individual::mutate(const float& chance)
  {
    root.mutate_tree(chance);
  }

  // Safely return reference to desired node.
  Node&
  Individual::operator[](const Size& i)
  {
    assert(i.internals <= get_internals());
    assert(i.leaves <= get_leaves());

    Size visiting;
    // Return root node if that's what we're seeking.
    if (i.internals == 0 and i.leaves == 0)
      return root;
    else
      return root.visit(i, visiting);
  }

  {

    else
      {
      }

  Size
  Individual::get_node(const Type& type)
  {
    Size target;
    // Guaranteed to have at least 1 leaf, but may have 0 internals.
    if (type == Type::internal and get_internals() != 0)
      {
	// Choose an internal node.
	int_dist dist{0, static_cast<int>(get_internals()) - 1};
	target.internals = dist(rg.engine);
      }
    else
      {
	// Otherwise choose a leaf node.
	int_dist dist{0, static_cast<int>(get_leaves()) - 1};
	target.leaves = dist(rg.engine);
      }
    return target;
  }

  /* Swap two random subtrees between Individuals "a" and "b",
     selecting an internal node with chance probability. */
  void
  crossover(const float& chance, Individual& a, Individual& b)
  {
    real_dist probability{0, 1};

    Individual::Type type_a = (probability(rg.engine) < chance)
      ? Individual::Type::internal : Individual::Type::leaf;

    Individual::Type type_b = (probability(rg.engine) < chance)
      ? Individual::Type::internal : Individual::Type::leaf;

    std::swap(a[a.get_node(type_a)], b[b.get_node(type_b)]);
  }
  }
}
<commit_msg>Moving getter functions to bottom of source file<commit_after>/* indivudal.cpp - CS 472 Project #2: Genetic Programming
 * Copyright 2014 Andrew Schwartzmeyer
 *
 * Source file for individual namespace
 */

#include <algorithm>
#include <cassert>
#include <cmath>
#include <iostream>
#include <memory>
#include <tuple>
#include <vector>

#include "individual.hpp"
#include "../options/options.hpp"
#include "../random_generator/random_generator.hpp"

namespace individual
{
  using std::vector;
  using std::string;
  using namespace random_generator;

  // Default Size struct constructor.
  Size::Size(): internals{0}, leaves{0}, depth{0} {}

  using F = Function;
  // Vectors of same-arity function enums.
  vector<F> nullaries {F::left, F::right, F::forward};
  vector<F> binaries {F::prog2, F::iffoodahead};
  vector<F> trinaries {F::prog3};

  /* Vectors of available function enums.  Should be moved into
     Options struct. */
  vector<F> leaves = nullaries;
  vector<F> internals {F::prog2, F::prog3, F::iffoodahead};

  // Returns a random function from a given set of functions.
  Function
  get_function(const vector<Function>& functions)
  {
    int_dist dist{0, static_cast<int>(functions.size()) - 1}; // closed interval
    return functions[dist(rg.engine)];
  }

  // Returns bool of whether or not the item is in the set.
  template<typename I, typename S> bool
  contains(const I& item, const S& set)
  {
    return std::find(set.begin(), set.end(), item) != set.end();
  }

  // Returns the appropriate arity for a given function.
  unsigned int
  get_arity(const Function& function)
  {
    if (contains(function, nullaries))
      return 0;
    else if (contains(function, binaries))
      return 2;
    else if (contains(function, trinaries))
      return 3;
    assert(false);
  }

  // Default constructor for "empty" node
  Node::Node(): function{Function::nil}, arity{0} {}

  /* Recursively constructs a parse tree using the given method
     (either GROW or FULL). */
  Node::Node(const Method& method, const unsigned int& max_depth)
  {
    // Create leaf node if at the max depth or randomly (if growing).
    real_dist dist{0, 1};
    float grow_chance =
      static_cast<float>(leaves.size()) / (leaves.size() + internals.size());
    if (max_depth == 0
	or (method == Method::grow and dist(rg.engine) < grow_chance))
      {
	function = get_function(leaves);
	arity = 0; // get_arity(function); leaves are always zero
      }
    // Otherwise choose an internal node.
    else
      {
	function = get_function(internals);
	arity = get_arity(function);
	// Recursively create subtrees.
	children.reserve(arity);
	for (unsigned int i = 0; i < arity; ++i)
	  children.emplace_back(Node{method, max_depth - 1});
      }
    assert(function != Function::nil); // do not create null types
    assert(children.size() == arity); // ensure arity
  }

  Node create(const unsigned int& max_depth, const float& chance)
  {
    real_dist method_dist{0, 1};
    Method method = (method_dist(rg.engine) < chance)
      ? Method::grow : Method::full;

    int_dist depth_dist{0, static_cast<int>(max_depth)};
    unsigned int depth = depth_dist(rg.engine);

    return Node{method, depth};
  }

  // Returns a string visually representing a particular node.
  string
  Node::represent() const
  {
    switch (function)
      {
      case F::nil:
	assert(false); // Never represent empty node.
      case F::prog2:
	return "prog-2";
      case F::prog3:
	return "prog-3";
      case F::iffoodahead:
	return "if-food-ahead";
      case F::left:
	return "left";
      case F::right:
	return "right";
      case F::forward:
	return "forward";
      }
    assert(false); // Every node should have been matched.
  }

  /* Returns string representation of expression in Polish/prefix
     notation using a pre-order traversal. */
  string
  Node::print() const
  {
    if (children.empty())
      return represent();

    string formula = "(" + represent();

    for (const Node& child : children)
      formula += " " + child.print();

    return formula + ")";
  }

  /* Evaluates an ant over a given map using a depth-first post-order
     recursive continuous evaluation of a decision tree. */
  void
  Node::evaluate(options::Map& map) const
  {
    if (not map.active()) return;

    switch (function)
      {
      case F::left:
	map.left(); // Terminal case
	break;
      case F::right:
	map.right(); // Terminal case
	break;
      case F::forward:
	map.forward(); // Terminal case
	break;
      case F::iffoodahead:
	if (map.look()) // Do left or right depending on if food ahead
	  children[0].evaluate(map);
	else
	  children[1].evaluate(map);
	break;
      case F::prog2: // Falls through
      case F::prog3:
	for (const Node& child : children)
	  child.evaluate(map);
	break;
      case F::nil:
	assert(false); // Never evaluate empty node
      }
  }

  /* Recursively count children and find maximum depth of tree via
     post-order traversal.  Keep track of internals, leaves, and depth
     via Size struct */
  const Size
  Node::size() const
  {
    Size size;

    if (children.empty())
      ++size.leaves;
    else
      {
	vector<unsigned int> depths;
	depths.reserve(arity);
	for (const Node& child : children)
	  {
	    Size temp = child.size();
	    size.internals += temp.internals;
	    size.leaves += temp.leaves;
	    depths.emplace_back(temp.depth);
	  }
	size.depth = 1 + *std::max_element(depths.begin(), depths.end());
	++size.internals;
      }
    return size;
  }

  // Used to represent "not-found" (similar to a NULL pointer).
  Node empty;

  /* Depth-first search for taget node.  Must be seeking either
     internal or leaf, cannot be both. */
  Node&
  Node::visit(const Size& i, Size& visiting)
  {
    for (Node& child : children) {
      // Increase relevant count.
      if (child.children.empty())
	++visiting.leaves;
      else
	++visiting.internals;

      // Return node reference if found.
      if (visiting.internals == i.internals or visiting.leaves == i.leaves)
	return child;
      else
	{
	  Node& temp = child.visit(i, visiting); // Recursive search.
	  if (temp.function != Function::nil)
	    return temp;
	}
    }
    return empty;
  }

  void
  Node::mutate_self()
  {
    if (arity == 0)
      {
	const Function old = function;
	while (function == old)
	  function = get_function(leaves);
      }
    else
      {
	const Function old = function;
	while (function == old)
	  function = get_function(internals);
	arity = get_arity(function);
	// Fix arity mismatches caused by mutation
	if (arity == 2 and children.size() == 3)
	  children.pop_back();
	else if (arity == 3 and children.size() == 2)
	  children.emplace_back(create(4, 0.5));
      }
    assert(arity == children.size());
    assert(function != Function::nil);
  }

  // Recursively mutate nodes with given probability.
  void
  Node::mutate_tree(const float& chance)
  {
    real_dist dist{0, 1};
    for (Node& child : children)
      {
	if (dist(rg.engine) < chance)
	  child.mutate_self();
	child.mutate_tree(chance);
      }
  }

  // Default constructor for Individual
  Individual::Individual() {}

  /* Create an Individual tree by having a root node (to which the
     actual construction is delegated). The depth is passed by value
     as its creation elsewhere is temporary. */
  Individual::Individual(const unsigned int depth, const float& chance,
			 options::Map map): fitness{0}, adjusted{0}
  {
    root = create(depth, chance);
    /* The evaluate method updates the size and both raw and adjusted
       fitnesses. */
    evaluate(map);
  }

  // Return string representation of a tree's size and fitness.
  string
  Individual::print() const
  {
    using std::to_string;

    string info = "# Size " + to_string(get_total()) + ", with "
      + to_string(get_internals()) + " internals, "
      + to_string(get_leaves()) + " leaves, and depth of "
      + to_string(get_depth()) + ".\n"
      + "# Score: " + to_string(score)
      + ", and adjusted fitness: " + to_string(adjusted) + ".\n";

    return info;
  }

  // Return string represenation of tree's expression (delegated).
  string
  Individual::print_formula() const
  {
    return "# Formula: " + root.print() + "\n";
  }

  /* Evaluate Individual for given values and calculate size.  Update
     Individual's size and fitness accordingly. Return non-empty
     string if printing. */
  string
  Individual::evaluate(options::Map map, const float& penalty,
		       const bool& print)
  {
    // Update size on evaluation because it's incredibly convenient.
    size = root.size();

    while (map.active())
      root.evaluate(map);

    score = map.fitness();
    // Adjusted fitness does not have size penalty.
    adjusted = static_cast<float>(score) / map.max();
    // Apply size penalty if not printing.
    fitness = score - penalty * get_total();

    string evaluation;
    if (print) evaluation = map.print();

    return evaluation;
  }

  // Mutate each node with given probability.
  void
  Individual::mutate(const float& chance)
  {
    root.mutate_tree(chance);
  }

  // Safely return reference to desired node.
  Node&
  Individual::operator[](const Size& i)
  {
    assert(i.internals <= get_internals());
    assert(i.leaves <= get_leaves());

    Size visiting;
    // Return root node if that's what we're seeking.
    if (i.internals == 0 and i.leaves == 0)
      return root;
    else
      return root.visit(i, visiting);
  }

  {

    else
      {
      }

  Size
  Individual::get_node(const Type& type)
  {
    Size target;
    // Guaranteed to have at least 1 leaf, but may have 0 internals.
    if (type == Type::internal and get_internals() != 0)
      {
	// Choose an internal node.
	int_dist dist{0, static_cast<int>(get_internals()) - 1};
	target.internals = dist(rg.engine);
      }
    else
      {
	// Otherwise choose a leaf node.
	int_dist dist{0, static_cast<int>(get_leaves()) - 1};
	target.leaves = dist(rg.engine);
      }
    return target;
  }

  /* Swap two random subtrees between Individuals "a" and "b",
     selecting an internal node with chance probability. */
  void
  crossover(const float& chance, Individual& a, Individual& b)
  {
    real_dist probability{0, 1};

    Individual::Type type_a = (probability(rg.engine) < chance)
      ? Individual::Type::internal : Individual::Type::leaf;

    Individual::Type type_b = (probability(rg.engine) < chance)
      ? Individual::Type::internal : Individual::Type::leaf;

    std::swap(a[a.get_node(type_a)], b[b.get_node(type_b)]);
  }

  // Read-only "getters" for private data

  unsigned int
  Individual::get_internals() const
  {
    return size.internals;
  }

  unsigned int
  Individual::get_leaves() const
  {
    return size.leaves;
  }

  unsigned int
  Individual::get_total() const
  {
    return size.internals + size.leaves;
  }

  unsigned int
  Individual::get_depth() const
  {
    return size.depth;
  }

  unsigned int
  Individual::get_score() const
  {
    return score;
  }

  float
  Individual::get_fitness() const
  {
    return fitness;
  }

  float
  Individual::get_adjusted() const
  {
    return adjusted;
  }
}
<|endoftext|>


<commit_before>/* You can use one of the both BSD 3-Clause License or GNU Lesser General Public License 3.0 for this source. */
/* BSD 3-Clause License:
 * Copyright (c) 2013 - 2020, kazunobu watatsu.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation or other materials provided with the distribution.
 *    Neither the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#if !defined(_LINEAR_OPT_)

template <typename T> class Linner {
public:
#if defined(_WITHOUT_EIGEN_)
  typedef SimpleMatrix<T> Mat;
  typedef SimpleVector<T> Vec;
#else
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
#endif

  inline Linner();
  inline ~Linner();

         Vec inner(const Mat& A, const Vec& b) const;
  inline Mat roughQR(const Mat& At) const;
  T epsilon;
};

template <typename T> inline Linner<T>::Linner() {
#if defined(ACC_NO_FLOAT)
  epsilon = T(1) >> short(32);
#else
  epsilon = sqrt(std::numeric_limits<T>::epsilon());
#endif
  return;
}

template <typename T> inline Linner<T>::~Linner() {
  return;
}

template <typename T> typename Linner<T>::Vec Linner<T>::inner(const Mat& A, const Vec& b) const {
  assert(A.rows() == b.size() && 0 < A.cols() && 0 < b.size());
  // cout << A << endl << b << endl << c.transpose() << endl;
  cerr << " (" << A.rows() << ", " << A.cols() << ")";
  {
    bool trivial(true);
    for(int i = 0; i < b.size() && trivial; i ++)
      trivial = T(0) <= b[i];
    if(trivial) {
      Vec rvec(A.cols());
      for(int i = 0; i < rvec.size(); i ++)
        rvec[i] = T(0);
      return rvec;
    }
  }
  Mat AA(A.rows() + A.cols() * 2 + 1, A.cols() + 1);
  for(int i = 0; i < A.rows(); i ++) {
    for(int j = 0; j < A.cols(); j ++)
      AA(i, j) = A(i, j);
    AA(i, A.cols()) = - b[i];
    AA.row(i) /= sqrt(AA.row(i).dot(AA.row(i)));
  }
  T m(1);
  for(int i = 0; i < A.rows(); i ++)
    for(int j = 0; j < AA.cols(); j ++) {
      const auto buf(abs(AA(i, j)));
      if(buf != T(0)) m = min(m, buf);
      assert(isfinite(buf) && !isnan(buf));
    }
  cerr << " scale(" << m << ")" << flush;
  for(int i = A.rows(); i < AA.rows(); i ++)
    for(int j = 0; j < AA.cols(); j ++)
      AA(i, j) = (i - A.rows()) / 2 == j
        ? T((i - A.rows()) & 1 ? 1 : - 1)
        : T(0);
        auto Pt(roughQR(AA.transpose()));
  cerr << "Q" << flush;
  const Mat  R(Pt * AA);
  cerr << "R" << flush;

  Vec one(Pt.cols());
#if defined(_OPENMP)
#pragma omp simd
#endif
  for(int i = 0; i < Pt.cols(); i ++)
    one[i] = T(1);
  for(int n_fixed = 0; n_fixed < Pt.rows() - 1; n_fixed ++) {
#if defined(_WITHOUT_EIGEN_)
    const auto on(Pt.projectionPt(- one));
#else
    const Vec  on(Pt.transpose() * (Pt * (- one)));
#endif
    auto fidx(0);
    for( ; fidx < on.size() && on[fidx] <= T(0); fidx ++) ;
    for(int i = fidx + 1; i < on.size(); i ++)
      if(T(0) < on[i] && on[i] < on[fidx]) fidx = i;
    if(! n_fixed) fidx = Pt.cols() - 1;
    if(on.size() <= fidx || on[fidx] <= T(0)) break;

    // O(mn^2) over all in this function.
    const Vec  orth(Pt.col(fidx));
          Vec  intercept(Pt.cols());
    const auto orth2(orth.dot(orth));
#if defined(_OPENMP)
#pragma omp parallel for schedule(static, 1)
#endif
    for(int j = 0; j < Pt.cols(); j ++) {
      intercept[j] = sqrt(orth2 * Pt.col(j).dot(Pt.col(j)));
      if(intercept[j] == T(0)) continue;
      intercept[j] = T(1) / intercept[j];
      if(! isfinite(intercept[j]) || isnan(intercept[j])) intercept[j] = T(0);
    }
    const auto ninter(sqrt(intercept.dot(intercept)));
    if(! isfinite(ninter) || isnan(ninter) || ninter == T(0)) break;
    intercept *= epsilon / ninter;
    const auto norm2orth(orth.dot(orth) + intercept[fidx]);
#if defined(_OPENMP)
#pragma omp parallel for schedule(static, 1)
#endif
    for(int j = 0; j < Pt.cols(); j ++)
      if(intercept[j] == T(0))
#if defined(_WITHOUT_EIGEN_)
        Pt.setCol(j, Pt.col(j) * T(0));
#else
        Pt.col(j) *= T(0);
#endif
      else
#if defined(_WITHOUT_EIGEN_)
        Pt.setCol(j, Pt.col(j) - orth * (Pt.col(j).dot(orth) + intercept[j]) / norm2orth);
#else
        Pt.col(j) -= orth * (Pt.col(j).dot(orth) + intercept[j]) / norm2orth;
#endif
  }
  cerr << "G" << flush;
#if defined(_WITHOUT_EIGEN_)
  auto rvec(- R.solve(Pt * one));
#else
  auto rvec(- R.inverse() * (Pt * one));
#endif
  cerr << "I" << flush;
  Vec rrvec(rvec.size() - 1);
  for(int i = 0; i < rrvec.size(); i ++)
    rrvec[i] = rvec[i] / rvec[rvec.size() - 1];
  return rrvec;
}

template <typename T> inline typename Linner<T>::Mat Linner<T>::roughQR(const Mat& At) const {
  Mat Q(At.rows(), At.cols());
  for(int i = 0; i < Q.rows(); i ++)
    for(int j = 0; j < Q.cols(); j ++)
      Q(i, j) = T(0);
  for(int i = 0; i < At.rows(); i ++) {
    // N.B. in this case, At is full rank.
#if defined(_WITHOUT_EIGEN_)
    const Vec work(At.row(i) - Q.projectionPt(At.row(i)));
#else
    const Vec work(At.row(i) - (Q.transpose() * (Q * At.row(i).transpose())).transpose());
#endif
    // generally, assert norm > error is needed.
    // in this case, not.
    Q.row(i) = work / sqrt(work.dot(work));
  }
  return Q;
}

#define _LINEAR_OPT_
#endif

<commit_msg>it's better not to orthogonal \with intercept.<commit_after>/* You can use one of the both BSD 3-Clause License or GNU Lesser General Public License 3.0 for this source. */
/* BSD 3-Clause License:
 * Copyright (c) 2013 - 2020, kazunobu watatsu.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation or other materials provided with the distribution.
 *    Neither the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#if !defined(_LINEAR_OPT_)

template <typename T> class Linner {
public:
#if defined(_WITHOUT_EIGEN_)
  typedef SimpleMatrix<T> Mat;
  typedef SimpleVector<T> Vec;
#else
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
#endif

  inline Linner();
  inline ~Linner();

         Vec inner(const Mat& A, const Vec& b) const;
  inline Mat roughQR(const Mat& At) const;
  T epsilon;
};

template <typename T> inline Linner<T>::Linner() {
#if defined(ACC_NO_FLOAT)
  epsilon = T(1) >> short(32);
#else
  epsilon = sqrt(std::numeric_limits<T>::epsilon());
#endif
  return;
}

template <typename T> inline Linner<T>::~Linner() {
  return;
}

template <typename T> typename Linner<T>::Vec Linner<T>::inner(const Mat& A, const Vec& b) const {
  assert(A.rows() == b.size() && 0 < A.cols() && 0 < b.size());
  // cout << A << endl << b << endl << c.transpose() << endl;
  cerr << " (" << A.rows() << ", " << A.cols() << ")";
  {
    bool trivial(true);
    for(int i = 0; i < b.size() && trivial; i ++)
      trivial = T(0) <= b[i];
    if(trivial) {
      Vec rvec(A.cols());
      for(int i = 0; i < rvec.size(); i ++)
        rvec[i] = T(0);
      return rvec;
    }
  }
  Mat AA(A.rows() + A.cols() * 2 + 1, A.cols() + 1);
  for(int i = 0; i < A.rows(); i ++) {
    for(int j = 0; j < A.cols(); j ++)
      AA(i, j) = A(i, j);
    AA(i, A.cols()) = - b[i];
    AA.row(i) /= sqrt(AA.row(i).dot(AA.row(i)));
  }
  T m(1);
  for(int i = 0; i < A.rows(); i ++)
    for(int j = 0; j < AA.cols(); j ++) {
      const auto buf(abs(AA(i, j)));
      if(buf != T(0)) m = min(m, buf);
      assert(isfinite(buf) && !isnan(buf));
    }
  cerr << " scale(" << m << ")" << flush;
  for(int i = A.rows(); i < AA.rows(); i ++)
    for(int j = 0; j < AA.cols(); j ++)
      AA(i, j) = (i - A.rows()) / 2 == j
        ? T((i - A.rows()) & 1 ? 1 : - 1)
        : T(0);
        auto Pt(roughQR(AA.transpose()));
  cerr << "Q" << flush;
  const Mat  R(Pt * AA);
  cerr << "R" << flush;

  Vec one(Pt.cols());
#if defined(_OPENMP)
#pragma omp simd
#endif
  for(int i = 0; i < Pt.cols(); i ++)
    one[i] = T(1);
  for(int n_fixed = 0; n_fixed < Pt.rows() - 1; n_fixed ++) {
#if defined(_WITHOUT_EIGEN_)
    const auto on(Pt.projectionPt(- one));
#else
    const Vec  on(Pt.transpose() * (Pt * (- one)));
#endif
    auto fidx(0);
    for( ; fidx < on.size() && on[fidx] <= T(0); fidx ++) ;
    for(int i = fidx + 1; i < on.size(); i ++)
      if(T(0) < on[i] && on[i] < on[fidx]) fidx = i;
    if(on.size() <= fidx || on[fidx] <= T(0)) break;

    // O(mn^2) over all in this function.
    const Vec  orth(Pt.col(fidx));
    const auto norm2orth(orth.dot(orth));
#if defined(_OPENMP)
#pragma omp parallel for schedule(static, 1)
#endif
    for(int j = 0; j < Pt.cols(); j ++)
#if defined(_WITHOUT_EIGEN_)
      Pt.setCol(j, Pt.col(j) - orth * Pt.col(j).dot(orth) / norm2orth);
#else
      Pt.col(j) -= orth * Pt.col(j).dot(orth) / norm2orth;
#endif
  }
  cerr << "G" << flush;
#if defined(_WITHOUT_EIGEN_)
  auto rvec(- R.solve(Pt * one));
#else
  auto rvec(- R.inverse() * (Pt * one));
#endif
  cerr << "I" << flush;
  Vec rrvec(rvec.size() - 1);
  for(int i = 0; i < rrvec.size(); i ++)
    rrvec[i] = rvec[i] / rvec[rvec.size() - 1];
  return rrvec;
}

template <typename T> inline typename Linner<T>::Mat Linner<T>::roughQR(const Mat& At) const {
  Mat Q(At.rows(), At.cols());
  for(int i = 0; i < Q.rows(); i ++)
    for(int j = 0; j < Q.cols(); j ++)
      Q(i, j) = T(0);
  for(int i = 0; i < At.rows(); i ++) {
    // N.B. in this case, At is full rank.
#if defined(_WITHOUT_EIGEN_)
    const Vec work(At.row(i) - Q.projectionPt(At.row(i)));
#else
    const Vec work(At.row(i) - (Q.transpose() * (Q * At.row(i).transpose())).transpose());
#endif
    // generally, assert norm > error is needed.
    // in this case, not.
    Q.row(i) = work / sqrt(work.dot(work));
  }
  return Q;
}

#define _LINEAR_OPT_
#endif

<|endoftext|>


<commit_before>#include "sipmanager.h"

#include <QObject>

const quint32 FIRSTTRANSPORTID = 1;

SIPManager::SIPManager():
  tcpServer_(),
  sipPort_(5060), // default for SIP, use 5061 for tls encrypted
  transports_(),
  nextTransportID_(FIRSTTRANSPORTID),
  transactions_(),
  negotiation_(),
  state_(INDIVIDUAL)
{}

// start listening to incoming
void SIPManager::init(SIPTransactionUser* callControl)
{
  QObject::connect(&tcpServer_, &ConnectionServer::newConnection,
                   this, &SIPManager::receiveTCPConnection);

  tcpServer_.setProxy(QNetworkProxy::NoProxy);

  // listen to everything
  qDebug() << "Initiating," << metaObject()->className()
           << ": Listening to SIP TCP connections on port:" << sipPort_;
  if (!tcpServer_.listen(QHostAddress::Any, sipPort_))
  {
    printDebug(DEBUG_ERROR, this, DC_STARTUP,
               "Failed to listen to socket. Is it reserved?");

    // TODO announce it to user!
  }

  transactions_.init(callControl);

  QSettings settings("kvazzup.ini", QSettings::IniFormat);
  QString username = !settings.value("local/Username").isNull()
      ? settings.value("local/Username").toString() : "anonymous";

  negotiation_.setLocalInfo(username);

  QObject::connect(&transactions_, &SIPTransactions::transportRequest,
                   this, &SIPManager::transportRequest);
  QObject::connect(&transactions_, &SIPTransactions::transportResponse,
                   this, &SIPManager::transportResponse);
}

void SIPManager::uninit()
{
  transactions_.uninit();

  for(std::shared_ptr<SIPTransport> transport : transports_)
  {
    if(transport != nullptr)
    {
      transport->cleanup();
      transport.reset();
    }
  }
}

void SIPManager::bindToServer()
{
  // get server address from settings and bind to server.
  QSettings settings("kvazzup.ini", QSettings::IniFormat);

  QString serverAddress = settings.value("sip/ServerAddress").toString();

  if (serverAddress != "")
  {
    std::shared_ptr<SIPTransport> transport = createSIPTransport();
    transport->createConnection(TCP, serverAddress);

    sessionToTransportID_[transactions_.reserveSessionID()] = transport->getTransportID();

    waitingToBind_[transport->getTransportID()] = serverAddress;
  }
  else {
    qDebug() << "SIP server address was empty in settings";
  }
}

uint32_t SIPManager::startCall(Contact& address)
{
  state_ = INDIVIDUAL;
  quint32 transportID = 0;
  uint32_t sessionID = transactions_.reserveSessionID();

  // There should not exist a dialog for this user
  if(!negotiation_.canStartSession())
  {
    printDebug(DEBUG_WARNING, this, DC_START_CALL, "Not enough ports to start a call.");
    return 0;
  }

  // check if we are already connected to remoteaddress and set transportID
  if (!isConnected(address.remoteAddress, transportID))
  {
    // we are not yet connected to them. Form a connection by creating the transport layer
    std::shared_ptr<SIPTransport> transport = createSIPTransport();
    transportID = transport->getTransportID(); // Get new transportID
    sessionToTransportID_[sessionID] = transportID;
    transport->createConnection(TCP, address.remoteAddress);
    waitingToStart_[transportID] = {sessionID, address};
  }
  else {
    // associate this sessionID with existing transportID
    sessionToTransportID_[sessionID] = transportID;
    // we have an existing connection already. Send SIP message and start call.
    transactions_.startDirectCall(address,
                                  transports_[transportID]->getLocalAddress(),
                                  sessionID);
  }

  return sessionID;
}

void SIPManager::acceptCall(uint32_t sessionID)
{

  // Start candiate nomination. This function won't block,
  // negotiation happens in the background remoteFinalSDP()
  // makes sure that a connection was in fact nominated
  negotiation_.startICECandidateNegotiation(sessionID);

  transactions_.acceptCall(sessionID);
}

void SIPManager::rejectCall(uint32_t sessionID)
{
  transactions_.rejectCall(sessionID);
}

void SIPManager::cancelCall(uint32_t sessionID)
{
  transactions_.cancelCall(sessionID);
}

void SIPManager::endCall(uint32_t sessionID)
{
  transactions_.endCall(sessionID);
  negotiation_.endSession(sessionID);
}

void SIPManager::endAllCalls()
{
  transactions_.endAllCalls();
  negotiation_.endAllSessions();
}

void SIPManager::negotiateReceivePorts()
{
  state_ = RECEIVE_PORTS;
  transactions_.renegotiateAllCalls();
}

void SIPManager::negotiateConference()
{
  state_ = WHOLE_CONFERENCE;
  transactions_.renegotiateAllCalls();
}

void SIPManager::getSDPs(uint32_t sessionID,
             std::shared_ptr<SDPMessageInfo>& localSDP,
             std::shared_ptr<SDPMessageInfo>& remoteSDP) const
{
  Q_ASSERT(sessionID);
  localSDP = negotiation_.getLocalSDP(sessionID);

  if (state_ == WHOLE_CONFERENCE)
  {
    remoteSDP = negotiation_.getRemoteConferenceSDP(sessionID);
  }
  else {
    remoteSDP = negotiation_.getRemoteSDP(sessionID);
  }
}

void SIPManager::receiveTCPConnection(TCPConnection *con)
{
  qDebug() << "Received a TCP connection. Initializing dialog";
  Q_ASSERT(con);

  std::shared_ptr<SIPTransport> transport = createSIPTransport();
  transport->incomingTCPConnection(std::shared_ptr<TCPConnection> (con));
}

void SIPManager::connectionEstablished(quint32 transportID)
{
  if (waitingToStart_.find(transportID) != waitingToStart_.end())
  {
    transactions_.startDirectCall(waitingToStart_[transportID].contact,
                                  transports_[transportID]->getLocalAddress(),
                                  waitingToStart_[transportID].sessionID);
  }

  if(waitingToBind_.find(transportID) != waitingToBind_.end())
  {
    transactions_.bindToServer(waitingToBind_[transportID],
                               transports_[transportID]->getLocalAddress(),
                               transportID);
  }
}

void SIPManager::transportRequest(uint32_t sessionID, SIPRequest &request)
{
  if (sessionToTransportID_.find(sessionID) != sessionToTransportID_.end())
  {
    quint32 transportID = sessionToTransportID_[sessionID];

    if (transports_.find(transportID) != transports_.end())
    {
      QVariant content;
      if(request.type == SIP_INVITE)
      {
        request.message->content.length = 0;
        qDebug() << "Adding SDP content to request:" << request.type;
        request.message->content.type = APPLICATION_SDP;

        if (!SDPOfferToContent(content,
                               transports_[transportID]->getLocalAddress(),
                               sessionID))
        {
          return;
        }
      }

      transports_[transportID]->sendRequest(request, content);
    }
    else {
      printDebug(DEBUG_ERROR,  metaObject()->className(), DC_SEND_SIP_REQUEST,
                 "Tried to send request with invalid transportID");
    }
  }
  else {
    printDebug(DEBUG_ERROR, metaObject()->className(), DC_SEND_SIP_REQUEST,
                "No mapping from sessionID to transportID");
  }
}

void SIPManager::transportResponse(uint32_t sessionID, SIPResponse &response)
{
  if (sessionToTransportID_.find(sessionID) != sessionToTransportID_.end())
  {
    quint32 transportID = sessionToTransportID_[sessionID];

    if (transports_.find(transportID) != transports_.end())
    {
      QVariant content;
      if (response.type == SIP_OK
          && response.message->transactionRequest == SIP_INVITE)
      {
        response.message->content.length = 0;
        qDebug() << "Adding SDP content to request:" << response.type;
        response.message->content.type = APPLICATION_SDP;
        if (!SDPAnswerToContent(content, sessionID))
        {
          return;
        }
      }

      transports_[transportID]->sendResponse(response, content);
    }
    else {
      printDebug(DEBUG_ERROR, metaObject()->className(), DC_SEND_SIP_RESPONSE,
                 "Tried to send response with invalid.", {"transportID"},
      {QString::number(transportID)});
    }
  }
  else {
    printDebug(DEBUG_ERROR, metaObject()->className(), DC_SEND_SIP_RESPONSE,
                "No mapping from sessionID to transportID");
  }
}

void SIPManager::processSIPRequest(SIPRequest& request, QHostAddress localAddress,
                                   QVariant& content, quint32 transportID)
{
  if(request.type == SIP_INVITE && !negotiation_.canStartSession())
  {
    printDebug(DEBUG_ERROR, this, DC_RECEIVE_SIP_REQUEST,
               "Got INVITE, but unable to start a call");
    return;
  }

  uint32_t sessionID = 0;

  // sets the sessionID if session exists or creates a new session if INVITE
  // returns true if either was successful.
  if (transactions_.identifySession(request, localAddress, sessionID))
  {
    Q_ASSERT(sessionID);
    if (sessionID != 0)
    {
      sessionToTransportID_[sessionID] = transportID;

      // TODO: prechecks that the message is ok, then modify program state.
      if(request.type == SIP_INVITE)
      {
        if(request.message->content.type == APPLICATION_SDP)
        {
          if(request.type == SIP_INVITE)
          {
            if(!processOfferSDP(sessionID, content, localAddress))
            {
              qDebug() << "Failed to find suitable SDP.";

              // TODO: Announce to transactions that we could not process their SDP
              printDebug(DEBUG_PROGRAM_ERROR, this, DC_RECEIVE_SIP_REQUEST,
                         "Failure to process SDP offer not implemented.");

              //foundDialog->server->setCurrentRequest(request); // TODO
              //sendResponse(sessionID, SIP_DECLINE, request.type);

              return;
            }
          }
        }
        else
        {
          qDebug() << "PEER ERROR: No SDP in" << request.type;
          // TODO: tell SIP transactions that they did not have an SDP
          // sendResponse(sessionID, SIP_DECLINE, request.type);
          return;
        }
      }

      transactions_.processSIPRequest(request, sessionID);
    }
    else
    {
      printDebug(DEBUG_PROGRAM_ERROR, metaObject()->className(),
                 DC_RECEIVE_SIP_REQUEST, "transactions did not set new sessionID.");
    }
  }
  else
  {
    printDebug(DEBUG_PEER_ERROR, metaObject()->className(),
               DC_RECEIVE_SIP_REQUEST, "transactions could not identify session.");
  }
}

void SIPManager::processSIPResponse(SIPResponse &response, QVariant& content)
{
  uint32_t sessionID = 0;

  if(!transactions_.identifySession(response, sessionID))
  {
    printDebug(DEBUG_PEER_ERROR, this, DC_RECEIVE_SIP_RESPONSE,
               "Could not identify response session");
    return;
  }

  if(response.message->transactionRequest == SIP_INVITE && response.type == SIP_OK)
  {
    if(response.message->content.type == APPLICATION_SDP)
    {
      if(!processAnswerSDP(sessionID, content))
      {
        qDebug() << "PEER_ERROR:"
                 << "Their final sdp is not suitable. They should have followed our SDP!!!";
        return;
      }

      negotiation_.setICEPorts(sessionID);
    }
  }

  transactions_.processSIPResponse(response, sessionID);
}

std::shared_ptr<SIPTransport> SIPManager::createSIPTransport()
{
  quint32 transportID = nextTransportID_;
  ++nextTransportID_;

  std::shared_ptr<SIPTransport> connection =
      std::shared_ptr<SIPTransport>(new SIPTransport(transportID));

  QObject::connect(connection.get(), &SIPTransport::incomingSIPRequest,
                   this, &SIPManager::processSIPRequest);

  QObject::connect(connection.get(), &SIPTransport::incomingSIPResponse,
                   this, &SIPManager::processSIPResponse);

  QObject::connect(connection.get(), &SIPTransport::sipTransportEstablished,
                   this, &SIPManager::connectionEstablished);
  transports_[transportID] = connection;

  return connection;
}

bool SIPManager::isConnected(QString remoteAddress, quint32& outTransportID)
{
  for(auto transport : transports_)
  {
    if(transport != nullptr &&
       transport->getRemoteAddress().toString() == remoteAddress)
    {
      outTransportID = transport->getTransportID();
      return true;
    }
  }
  return false;
}

bool SIPManager::SDPOfferToContent(QVariant& content, QHostAddress localAddress,
                                   uint32_t sessionID)
{
  std::shared_ptr<SDPMessageInfo> pointer;
  switch (state_)
  {
    case INDIVIDUAL:
    {
      printDebug(DEBUG_NORMAL, this, DC_NEGOTIATING, "Adding one-to-one SDP.");
      if(!negotiation_.generateOfferSDP(localAddress, sessionID))
      {
        qDebug() << "Failed to generate first SDP offer while sending.";
        return false;
      }
       pointer = negotiation_.getLocalSDP(sessionID);
      break;
    }
    case RECEIVE_PORTS:
    {
      printDebug(DEBUG_NORMAL, this, DC_NEGOTIATING, "Adding conferencing receive SDP.");
      if(!negotiation_.initialConferenceOfferSDP(sessionID))
      {
        qDebug() << "Failed to generate initial conference SDP offer while sending.";
        return false;
      }
      pointer = negotiation_.getInitialConferenceOffer(sessionID);
      break;
    }
    case WHOLE_CONFERENCE:
    {
      printDebug(DEBUG_NORMAL, this, DC_NEGOTIATING, "Adding conferencing final SDP.");
      if(!negotiation_.finalConferenceOfferSDP(sessionID))
      {
        qDebug() << "Failed to generate final conference SDP offer while sending.";
        return false;
      }
      pointer = negotiation_.getFinalConferenceOffer(sessionID);
      break;
    }
  }
  Q_ASSERT(pointer != nullptr);

  SDPMessageInfo sdp = *pointer;
  content.setValue(sdp);
  return true;
}

bool SIPManager::processOfferSDP(uint32_t sessionID, QVariant& content,
                                 QHostAddress localAddress)
{
  if(!content.isValid())
  {
    printDebug(DEBUG_PROGRAM_ERROR, this, DC_SIP_CONTENT,
                     "The SDP content is not valid at processing. "
                     "Should be detected earlier.");
    return false;
  }

  SDPMessageInfo retrieved = content.value<SDPMessageInfo>();
  if(!negotiation_.generateAnswerSDP(retrieved, localAddress, sessionID))
  {
    qDebug() << "Remote SDP not suitable or we have no ports to assign";
    negotiation_.endSession(sessionID);
    return false;
  }
  return true;
}

bool SIPManager::SDPAnswerToContent(QVariant &content, uint32_t sessionID)
{
  SDPMessageInfo sdp;
  std::shared_ptr<SDPMessageInfo> pointer = negotiation_.getLocalSDP(sessionID);
  sdp = *pointer;
  content.setValue(sdp);
  return true;
}

bool SIPManager::processAnswerSDP(uint32_t sessionID, QVariant &content)
{
  SDPMessageInfo retrieved = content.value<SDPMessageInfo>();
  if (!content.isValid())
  {
    printDebug(DEBUG_PROGRAM_ERROR, this, DC_NEGOTIATING,
               "Content is not valid when processing SDP. "
               "Should be detected earlier.");
    return false;
  }

  if(!negotiation_.processAnswerSDP(retrieved, sessionID))
  {
    return false;
  }

  return true;
}
<commit_msg>feature(Negotiation): Take into account different places where SDP may be included.<commit_after>#include "sipmanager.h"

#include <QObject>

const quint32 FIRSTTRANSPORTID = 1;

SIPManager::SIPManager():
  tcpServer_(),
  sipPort_(5060), // default for SIP, use 5061 for tls encrypted
  transports_(),
  nextTransportID_(FIRSTTRANSPORTID),
  transactions_(),
  negotiation_(),
  state_(INDIVIDUAL)
{}

// start listening to incoming
void SIPManager::init(SIPTransactionUser* callControl)
{
  QObject::connect(&tcpServer_, &ConnectionServer::newConnection,
                   this, &SIPManager::receiveTCPConnection);

  tcpServer_.setProxy(QNetworkProxy::NoProxy);

  // listen to everything
  qDebug() << "Initiating," << metaObject()->className()
           << ": Listening to SIP TCP connections on port:" << sipPort_;
  if (!tcpServer_.listen(QHostAddress::Any, sipPort_))
  {
    printDebug(DEBUG_ERROR, this, DC_STARTUP,
               "Failed to listen to socket. Is it reserved?");

    // TODO announce it to user!
  }

  transactions_.init(callControl);

  QSettings settings("kvazzup.ini", QSettings::IniFormat);
  QString username = !settings.value("local/Username").isNull()
      ? settings.value("local/Username").toString() : "anonymous";

  negotiation_.setLocalInfo(username);

  QObject::connect(&transactions_, &SIPTransactions::transportRequest,
                   this, &SIPManager::transportRequest);
  QObject::connect(&transactions_, &SIPTransactions::transportResponse,
                   this, &SIPManager::transportResponse);
}

void SIPManager::uninit()
{
  transactions_.uninit();

  for(std::shared_ptr<SIPTransport> transport : transports_)
  {
    if(transport != nullptr)
    {
      transport->cleanup();
      transport.reset();
    }
  }
}

void SIPManager::bindToServer()
{
  // get server address from settings and bind to server.
  QSettings settings("kvazzup.ini", QSettings::IniFormat);

  QString serverAddress = settings.value("sip/ServerAddress").toString();

  if (serverAddress != "")
  {
    std::shared_ptr<SIPTransport> transport = createSIPTransport();
    transport->createConnection(TCP, serverAddress);

    sessionToTransportID_[transactions_.reserveSessionID()] = transport->getTransportID();

    waitingToBind_[transport->getTransportID()] = serverAddress;
  }
  else {
    qDebug() << "SIP server address was empty in settings";
  }
}

uint32_t SIPManager::startCall(Contact& address)
{
  state_ = INDIVIDUAL;
  quint32 transportID = 0;
  uint32_t sessionID = transactions_.reserveSessionID();

  // There should not exist a dialog for this user
  if(!negotiation_.canStartSession())
  {
    printDebug(DEBUG_WARNING, this, DC_START_CALL, "Not enough ports to start a call.");
    return 0;
  }

  // check if we are already connected to remoteaddress and set transportID
  if (!isConnected(address.remoteAddress, transportID))
  {
    // we are not yet connected to them. Form a connection by creating the transport layer
    std::shared_ptr<SIPTransport> transport = createSIPTransport();
    transportID = transport->getTransportID(); // Get new transportID
    sessionToTransportID_[sessionID] = transportID;
    transport->createConnection(TCP, address.remoteAddress);
    waitingToStart_[transportID] = {sessionID, address};
  }
  else {
    // associate this sessionID with existing transportID
    sessionToTransportID_[sessionID] = transportID;
    // we have an existing connection already. Send SIP message and start call.
    transactions_.startDirectCall(address,
                                  transports_[transportID]->getLocalAddress(),
                                  sessionID);
  }

  return sessionID;
}

void SIPManager::acceptCall(uint32_t sessionID)
{

  // Start candiate nomination. This function won't block,
  // negotiation happens in the background remoteFinalSDP()
  // makes sure that a connection was in fact nominated

  //negotiation_.startICECandidateNegotiation(sessionID);

  transactions_.acceptCall(sessionID);
}

void SIPManager::rejectCall(uint32_t sessionID)
{
  transactions_.rejectCall(sessionID);
}

void SIPManager::cancelCall(uint32_t sessionID)
{
  transactions_.cancelCall(sessionID);
}

void SIPManager::endCall(uint32_t sessionID)
{
  transactions_.endCall(sessionID);
  negotiation_.endSession(sessionID);
}

void SIPManager::endAllCalls()
{
  transactions_.endAllCalls();
  negotiation_.endAllSessions();
}

void SIPManager::negotiateReceivePorts()
{
  state_ = RECEIVE_PORTS;
  transactions_.renegotiateAllCalls();
}

void SIPManager::negotiateConference()
{
  state_ = WHOLE_CONFERENCE;
  transactions_.renegotiateAllCalls();
}

void SIPManager::getSDPs(uint32_t sessionID,
             std::shared_ptr<SDPMessageInfo>& localSDP,
             std::shared_ptr<SDPMessageInfo>& remoteSDP) const
{
  Q_ASSERT(sessionID);
  localSDP = negotiation_.getLocalSDP(sessionID);

  if (state_ == WHOLE_CONFERENCE)
  {
    remoteSDP = negotiation_.getRemoteConferenceSDP(sessionID);
  }
  else {
    remoteSDP = negotiation_.getRemoteSDP(sessionID);
  }
}

void SIPManager::receiveTCPConnection(TCPConnection *con)
{
  qDebug() << "Received a TCP connection. Initializing dialog";
  Q_ASSERT(con);

  std::shared_ptr<SIPTransport> transport = createSIPTransport();
  transport->incomingTCPConnection(std::shared_ptr<TCPConnection> (con));
}

void SIPManager::connectionEstablished(quint32 transportID)
{
  if (waitingToStart_.find(transportID) != waitingToStart_.end())
  {
    transactions_.startDirectCall(waitingToStart_[transportID].contact,
                                  transports_[transportID]->getLocalAddress(),
                                  waitingToStart_[transportID].sessionID);
  }

  if(waitingToBind_.find(transportID) != waitingToBind_.end())
  {
    transactions_.bindToServer(waitingToBind_[transportID],
                               transports_[transportID]->getLocalAddress(),
                               transportID);
  }
}

void SIPManager::transportRequest(uint32_t sessionID, SIPRequest &request)
{
  if (sessionToTransportID_.find(sessionID) != sessionToTransportID_.end())
  {
    quint32 transportID = sessionToTransportID_[sessionID];

    if (transports_.find(transportID) != transports_.end())
    {
      QVariant content;
      if(request.type == SIP_INVITE)
      {
        request.message->content.length = 0;
        qDebug() << "Adding SDP content to request:" << request.type;
        request.message->content.type = APPLICATION_SDP;

        if (!SDPOfferToContent(content,
                               transports_[transportID]->getLocalAddress(),
                               sessionID))
        {
          return;
        }
      }

      transports_[transportID]->sendRequest(request, content);
    }
    else {
      printDebug(DEBUG_ERROR,  metaObject()->className(), DC_SEND_SIP_REQUEST,
                 "Tried to send request with invalid transportID");
    }
  }
  else {
    printDebug(DEBUG_ERROR, metaObject()->className(), DC_SEND_SIP_REQUEST,
                "No mapping from sessionID to transportID");
  }
}

void SIPManager::transportResponse(uint32_t sessionID, SIPResponse &response)
{
  if (sessionToTransportID_.find(sessionID) != sessionToTransportID_.end())
  {
    quint32 transportID = sessionToTransportID_[sessionID];

    if (transports_.find(transportID) != transports_.end())
    {
      QVariant content;
      if (response.type == SIP_OK
          && response.message->transactionRequest == SIP_INVITE)
      {
        response.message->content.length = 0;
        qDebug() << "Adding SDP content to request:" << response.type;
        response.message->content.type = APPLICATION_SDP;
        if (!SDPAnswerToContent(content, sessionID))
        {
          return;
        }
      }

      transports_[transportID]->sendResponse(response, content);
    }
    else {
      printDebug(DEBUG_ERROR, metaObject()->className(), DC_SEND_SIP_RESPONSE,
                 "Tried to send response with invalid.", {"transportID"},
      {QString::number(transportID)});
    }
  }
  else {
    printDebug(DEBUG_ERROR, metaObject()->className(), DC_SEND_SIP_RESPONSE,
                "No mapping from sessionID to transportID");
  }
}

void SIPManager::processSIPRequest(SIPRequest& request, QHostAddress localAddress,
                                   QVariant& content, quint32 transportID)
{
  if(request.type == SIP_INVITE && !negotiation_.canStartSession())
  {
    printDebug(DEBUG_ERROR, this, DC_RECEIVE_SIP_REQUEST,
               "Got INVITE, but unable to start a call");
    return;
  }

  uint32_t sessionID = 0;

  // sets the sessionID if session exists or creates a new session if INVITE
  // returns true if either was successful.
  if (transactions_.identifySession(request, localAddress, sessionID))
  {
    Q_ASSERT(sessionID);
    if (sessionID != 0)
    {
      sessionToTransportID_[sessionID] = transportID;

      if((request.type == SIP_INVITE || request.type == SIP_ACK)
         && request.message->content.type == APPLICATION_SDP)
      {
        switch (negotiation_.getState(sessionID))
        {
        case NEG_NO_STATE:
        {
          printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_REQUEST,
                     "Got first SDP offer.");
          if(!processOfferSDP(sessionID, content, transports_[transportID]->getLocalAddress()))
          {
             printDebug(DEBUG_PROGRAM_ERROR, this, DC_RECEIVE_SIP_REQUEST,
                        "Failure to process SDP offer not implemented.");

             //foundDialog->server->setCurrentRequest(request); // TODO
             //sendResponse(sessionID, SIP_DECLINE, request.type);
             return;
          }
          break;
        }
        case NEG_OFFER_GENERATED:
        {
          printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_REQUEST,
                     "Got an SDP answer.");
          processAnswerSDP(sessionID, content);
          break;
        }
        case NEG_ANSWER_GENERATED: // TODO: Not sure if these make any sense
        {
          printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_REQUEST,
                     "They sent us another SDP offer.");
          processOfferSDP(sessionID, content, transports_[transportID]->getLocalAddress());
          break;
        }
        case NEG_FINISHED:
        {
          printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_REQUEST,
                     "Got a new SDP offer in response.");
          processOfferSDP(sessionID, content, transports_[transportID]->getLocalAddress());
          break;
        }
        }

      }

      transactions_.processSIPRequest(request, sessionID);
    }
    else
    {
      printDebug(DEBUG_PROGRAM_ERROR, metaObject()->className(),
                 DC_RECEIVE_SIP_REQUEST, "transactions did not set new sessionID.");
    }
  }
  else
  {
    printDebug(DEBUG_PEER_ERROR, metaObject()->className(),
               DC_RECEIVE_SIP_REQUEST, "transactions could not identify session.");
  }
}

void SIPManager::processSIPResponse(SIPResponse &response, QVariant& content)
{
  uint32_t sessionID = 0;

  if(!transactions_.identifySession(response, sessionID))
  {
    printDebug(DEBUG_PEER_ERROR, this, DC_RECEIVE_SIP_RESPONSE,
               "Could not identify response session");
    return;
  }

  if(response.message->transactionRequest == SIP_INVITE && response.type == SIP_OK)
  {
    if(response.message->content.type == APPLICATION_SDP)
    {
      if(sessionToTransportID_.find(sessionID) == sessionToTransportID_.end())
      {
        printDebug(DEBUG_WARNING, this, DC_RECEIVE_SIP_RESPONSE,
                   "Could not identify transport for session");
        return;
      }

      quint32 transportID = sessionToTransportID_[sessionID];

      switch (negotiation_.getState(sessionID))
      {
      case NEG_NO_STATE:
      {
        printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_RESPONSE,
                   "Got first SDP offer.");
        if(!processOfferSDP(sessionID, content, transports_[transportID]->getLocalAddress()))
        {
           printDebug(DEBUG_PROGRAM_ERROR, this, DC_RECEIVE_SIP_RESPONSE,
                      "Failure to process SDP offer not implemented.");

           //foundDialog->server->setCurrentRequest(request); // TODO
           //sendResponse(sessionID, SIP_DECLINE, request.type);
           return;
        }
        break;
      }
      case NEG_OFFER_GENERATED:
      {
        printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_RESPONSE,
                   "Got an SDP answer.");
        processAnswerSDP(sessionID, content);
        break;
      }
      case NEG_ANSWER_GENERATED: // TODO: Not sure if these make any sense
      {
        printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_RESPONSE,
                   "They sent us another SDP offer.");
        processOfferSDP(sessionID, content, transports_[transportID]->getLocalAddress());
        break;
      }
      case NEG_FINISHED:
      {
        printDebug(DEBUG_NORMAL, this, DC_RECEIVE_SIP_RESPONSE,
                   "Got a new SDP offer in response.");
        processOfferSDP(sessionID, content, transports_[transportID]->getLocalAddress());
        break;
      }
      }

      negotiation_.setICEPorts(sessionID);
    }
  }

  transactions_.processSIPResponse(response, sessionID);
}

std::shared_ptr<SIPTransport> SIPManager::createSIPTransport()
{
  quint32 transportID = nextTransportID_;
  ++nextTransportID_;

  std::shared_ptr<SIPTransport> connection =
      std::shared_ptr<SIPTransport>(new SIPTransport(transportID));

  QObject::connect(connection.get(), &SIPTransport::incomingSIPRequest,
                   this, &SIPManager::processSIPRequest);

  QObject::connect(connection.get(), &SIPTransport::incomingSIPResponse,
                   this, &SIPManager::processSIPResponse);

  QObject::connect(connection.get(), &SIPTransport::sipTransportEstablished,
                   this, &SIPManager::connectionEstablished);
  transports_[transportID] = connection;

  return connection;
}

bool SIPManager::isConnected(QString remoteAddress, quint32& outTransportID)
{
  for(auto transport : transports_)
  {
    if(transport != nullptr &&
       transport->getRemoteAddress().toString() == remoteAddress)
    {
      outTransportID = transport->getTransportID();
      return true;
    }
  }
  return false;
}

bool SIPManager::SDPOfferToContent(QVariant& content, QHostAddress localAddress,
                                   uint32_t sessionID)
{
  std::shared_ptr<SDPMessageInfo> pointer;
  switch (state_)
  {
    case INDIVIDUAL:
    {
      printDebug(DEBUG_NORMAL, this, DC_NEGOTIATING, "Adding one-to-one SDP.");
      if(!negotiation_.generateOfferSDP(localAddress, sessionID))
      {
        qDebug() << "Failed to generate first SDP offer while sending.";
        return false;
      }
       pointer = negotiation_.getLocalSDP(sessionID);
      break;
    }
    case RECEIVE_PORTS:
    {
      printDebug(DEBUG_NORMAL, this, DC_NEGOTIATING, "Adding conferencing receive SDP.");
      if(!negotiation_.initialConferenceOfferSDP(sessionID))
      {
        qDebug() << "Failed to generate initial conference SDP offer while sending.";
        return false;
      }
      pointer = negotiation_.getInitialConferenceOffer(sessionID);
      break;
    }
    case WHOLE_CONFERENCE:
    {
      printDebug(DEBUG_NORMAL, this, DC_NEGOTIATING, "Adding conferencing final SDP.");
      if(!negotiation_.finalConferenceOfferSDP(sessionID))
      {
        qDebug() << "Failed to generate final conference SDP offer while sending.";
        return false;
      }
      pointer = negotiation_.getFinalConferenceOffer(sessionID);
      break;
    }
  }
  Q_ASSERT(pointer != nullptr);

  SDPMessageInfo sdp = *pointer;
  content.setValue(sdp);
  return true;
}

bool SIPManager::processOfferSDP(uint32_t sessionID, QVariant& content,
                                 QHostAddress localAddress)
{
  if(!content.isValid())
  {
    printDebug(DEBUG_PROGRAM_ERROR, this, DC_SIP_CONTENT,
                     "The SDP content is not valid at processing. "
                     "Should be detected earlier.");
    return false;
  }

  SDPMessageInfo retrieved = content.value<SDPMessageInfo>();
  if(!negotiation_.generateAnswerSDP(retrieved, localAddress, sessionID))
  {
    qDebug() << "Remote SDP not suitable or we have no ports to assign";
    negotiation_.endSession(sessionID);
    return false;
  }
  return true;
}

bool SIPManager::SDPAnswerToContent(QVariant &content, uint32_t sessionID)
{
  SDPMessageInfo sdp;
  std::shared_ptr<SDPMessageInfo> pointer = negotiation_.getLocalSDP(sessionID);
  sdp = *pointer;
  content.setValue(sdp);
  return true;
}

bool SIPManager::processAnswerSDP(uint32_t sessionID, QVariant &content)
{
  SDPMessageInfo retrieved = content.value<SDPMessageInfo>();
  if (!content.isValid())
  {
    printDebug(DEBUG_PROGRAM_ERROR, this, DC_NEGOTIATING,
               "Content is not valid when processing SDP. "
               "Should be detected earlier.");
    return false;
  }

  if(!negotiation_.processAnswerSDP(retrieved, sessionID))
  {
    return false;
  }

  return true;
}
<|endoftext|>


<commit_before>#include "parser.h"

#include <assert.h>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <cstring>
#include "types.h"
#include "parser_definitions.h"

uint32 Parser::get() const {
  return *buffer;
}

bool Parser::end() const {
  return index >= length;
}

void Parser::eat() {
  index++;
  buffer++;
}

uint32 Parser::getAndEat() {
  uint32 result = get();
  eat();
  return result;
}

bool Parser::expectAndEat(uint32 e) {
  assert(e == get());
  eat();
  return true;
}

bool Parser::expect(uint32 e) const {
  assert(e == get());
  return true;
}

SOp Parser::readInstruction() {
  uint32* opData = buffer;
  uint32 word = getAndEat();
  spv::Op op = (spv::Op)(word & spv::OpCodeMask);
  uint32 wordCount = (uint32)(word >> spv::WordCountShift);

  WordType wordTypes[255];

  uint32 opWordCount = wordCount;

  if (sizeof(LUTOpWordTypes) / sizeof(void*) <= (int)op) {
    wordTypes[0] = WordType::TOp;
    for (int i = 1; i < wordCount; i++) {
      wordTypes[i] = WordType::TLiteralNumber;
    }
  }
  else {
    WordType* opWordTypes = (WordType*)LUTOpWordTypes[(int)op];
    opWordCount = LUTOpWordTypesCount[(int)op];

    for (int i = 0; i < wordCount; i++) {
      wordTypes[i] = opWordTypes[i > opWordCount - 1 ? opWordCount - 1 : i];
    }
  }

  uint32* opMem = new uint32[opWordCount + 1];
  memset(opMem, 0, sizeof(uint32) * (opWordCount + 1));
  SOp Result = { op, opMem };
  auto bufferBegin = buffer;
  int indexBegin = index;

  for (int i = 1; i < wordCount; i++) {
    word = getAndEat();
    void* currMem = opMem + i - 1;
    uint32* currMemU32 = (uint32*)currMem;
    if (wordTypes[i] == WordType::TIdList || wordTypes[i] == WordType::TLiteralNumberList) {
      assert(i + 1 == opWordCount);
      *currMemU32++ = (uint32) (wordCount - opWordCount + 1);
      currMem = currMemU32;
      *(uint32 **) currMem = (uint32 *) (buffer - 1);
      break;
    } else if (wordTypes[i] == WordType::TLiteralString) {
      assert(i + 1 == opWordCount);
      *(char **) currMem = (char *) (buffer - 1);
      break;
    } else {
      *currMemU32 = word;
    }
  }

  buffer = bufferBegin + wordCount - 1;
  index = indexBegin + wordCount - 1;

  return Result;
}

bool Parser::Parse(Program *outProg) {
  if (!expectAndEat(spv::MagicNumber)) {
    return false;
  }

  ParseProgram prog;
  prog.Version = getAndEat();
  prog.GeneratorMagic = getAndEat();
  prog.IDBound = getAndEat();
  prog.InstructionSchema = getAndEat();

  int instructionIndex = 0;
  prog.NextOp = readInstruction();

  do {
    SOp op = prog.NextOp;
    instructionIndex++;
    if (!end()) {
      prog.NextOp = readInstruction();
    } else {
      prog.NextOp = SOp{ Op::OpNop, nullptr };
    }

    LUTHandlerMethods[(int)op.Op]((void*)op.Memory, &prog);
    prog.Ops.push_back(op);

    if (prog.InFunction && prog.CurrentFunction->InBlock) {
      addOp(&prog, op);
    }
  } while (prog.NextOp.Op != Op::OpNop);

  *outProg = (Program)prog;
  return true;
}

std::string writeProgram(const Program& prog) {
  std::stringstream progStream;
  progStream << "Version: " << prog.Version << std::endl;
  progStream << "Generator Magic: " << prog.GeneratorMagic << std::endl;
  progStream << "ID Bound: " << prog.IDBound << std::endl;
  progStream << "Instruction Schema: " << prog.InstructionSchema << std::endl;
  progStream << "=================================================" << std::endl;
  int instructionIndex = 0;
  for(auto& op : prog.Ops) {
    progStream << std::setw(3) << instructionIndex << ": " << writeOp(op);
    instructionIndex++;
  }
  return progStream.str();
}

std::string writeOp(SOp op) {
  std::stringstream opline;

  opline << OpStrings[(int)op.Op];

  WordType wordTypes[255];

  uint32 opWordCount = 0;

  if (sizeof(LUTOpWordTypes) / sizeof(void*) <= (int)op.Op) {
    return "";
  }
  else {
    WordType* opWordTypes = (WordType*)LUTOpWordTypes[(int)op.Op];
    opWordCount = LUTOpWordTypesCount[(int)op.Op];

    for (int i = 0; i < opWordCount; i++) {
      wordTypes[i] = opWordTypes[i];
    }
  }

  for (int i = 1; i < opWordCount; i++) {
    uint32 word = *((uint32*)op.Memory + i - 1);

    if (wordTypes[i] == WordType::TLiteralNumber) {
      opline << " " << word;
    }
    else if (wordTypes[i] == WordType::TId) {
      opline << " [" << word << "]";
    }
    else if (wordTypes[i] == WordType::TIdList) {
      uint32 count = word;
      uint32* ptr = *(uint32**)((uint32*)op.Memory + i);
      opline << " [";
      for (int j = 0; j < count; j++) {
        word = ptr[j];
        opline << "[" << word << "]" << (j != count - 1 ? ", " : "");
      }
      opline << "]";

      break;
    }
    else if (wordTypes[i] == WordType::TLiteralNumberList) {
      uint32 count = word;
      uint32* ptr = *(uint32**)((uint32*)op.Memory + i);
      opline << " [";
      for (int j = 0; j < count; j++) {
        word = ptr[j];
        opline << word << (j != count - 1 ? ", " : "");
      }
      opline << "]";

      break;
    }
    else if (wordTypes[i] == WordType::TLiteralString) {
      if (wordTypes[i - 1] != WordType::TLiteralString) {
        char* str = *((char**)((uint32*)op.Memory + i - 1));
        opline << " " << str;
      }
    }
    else {
      std::string* lutPointer = *((std::string**)LUTPointers + (uint32)wordTypes[i]);
      std::string name = lutPointer[word];
      opline << " " << name;
    }
  }

  opline << std::endl;
  return opline.str();
}
<commit_msg>improved disassembler output<commit_after>#include "parser.h"

#include <assert.h>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <cstring>
#include "types.h"
#include "parser_definitions.h"

uint32 Parser::get() const {
  return *buffer;
}

bool Parser::end() const {
  return index >= length;
}

void Parser::eat() {
  index++;
  buffer++;
}

uint32 Parser::getAndEat() {
  uint32 result = get();
  eat();
  return result;
}

bool Parser::expectAndEat(uint32 e) {
  assert(e == get());
  eat();
  return true;
}

bool Parser::expect(uint32 e) const {
  assert(e == get());
  return true;
}

SOp Parser::readInstruction() {
  uint32* opData = buffer;
  uint32 word = getAndEat();
  spv::Op op = (spv::Op)(word & spv::OpCodeMask);
  uint32 wordCount = (uint32)(word >> spv::WordCountShift);

  WordType wordTypes[255];

  uint32 opWordCount = wordCount;

  if (sizeof(LUTOpWordTypes) / sizeof(void*) <= (int)op) {
    wordTypes[0] = WordType::TOp;
    for (int i = 1; i < wordCount; i++) {
      wordTypes[i] = WordType::TLiteralNumber;
    }
  }
  else {
    WordType* opWordTypes = (WordType*)LUTOpWordTypes[(int)op];
    opWordCount = LUTOpWordTypesCount[(int)op];

    for (int i = 0; i < wordCount; i++) {
      wordTypes[i] = opWordTypes[i > opWordCount - 1 ? opWordCount - 1 : i];
    }
  }

  uint32* opMem = new uint32[opWordCount + 1];
  memset(opMem, 0, sizeof(uint32) * (opWordCount + 1));
  SOp Result = { op, opMem };
  auto bufferBegin = buffer;
  int indexBegin = index;

  for (int i = 1; i < wordCount; i++) {
    word = getAndEat();
    void* currMem = opMem + i - 1;
    uint32* currMemU32 = (uint32*)currMem;
    if (wordTypes[i] == WordType::TIdList || wordTypes[i] == WordType::TLiteralNumberList) {
      assert(i + 1 == opWordCount);
      *currMemU32++ = (uint32) (wordCount - opWordCount + 1);
      currMem = currMemU32;
      *(uint32 **) currMem = (uint32 *) (buffer - 1);
      break;
    } else if (wordTypes[i] == WordType::TLiteralString) {
      assert(i + 1 == opWordCount);
      *(char **) currMem = (char *) (buffer - 1);
      break;
    } else {
      *currMemU32 = word;
    }
  }

  buffer = bufferBegin + wordCount - 1;
  index = indexBegin + wordCount - 1;

  return Result;
}

bool Parser::Parse(Program *outProg) {
  if (!expectAndEat(spv::MagicNumber)) {
    return false;
  }

  ParseProgram prog;
  prog.Version = getAndEat();
  prog.GeneratorMagic = getAndEat();
  prog.IDBound = getAndEat();
  prog.InstructionSchema = getAndEat();

  int instructionIndex = 0;
  prog.NextOp = readInstruction();

  do {
    SOp op = prog.NextOp;
    instructionIndex++;
    if (!end()) {
      prog.NextOp = readInstruction();
    } else {
      prog.NextOp = SOp{ Op::OpNop, nullptr };
    }

    LUTHandlerMethods[(int)op.Op]((void*)op.Memory, &prog);
    prog.Ops.push_back(op);

    if (prog.InFunction && prog.CurrentFunction->InBlock) {
      addOp(&prog, op);
    }
  } while (prog.NextOp.Op != Op::OpNop);

  *outProg = (Program)prog;
  return true;
}

std::string getDescriptor(uint32 id, const Program* prog) {
  if(prog->Names.find(id) != prog->Names.end()) {
    return prog->Names.at(id).Name;
  } else if(prog->Variables.find(id) != prog->Variables.end()) {
    SVariable variable = prog->Variables.at(id);
    return getDescriptor(variable.ResultId, prog);
  }
  return  "";
}

std::string writeOp(SOp op, const Program* prog) {
  std::stringstream opline;

  opline << OpStrings[(int)op.Op];

  WordType wordTypes[255];

  uint32 opWordCount = 0;

  if (sizeof(LUTOpWordTypes) / sizeof(void*) <= (int)op.Op) {
    return "";
  }
  else {
    WordType* opWordTypes = (WordType*)LUTOpWordTypes[(int)op.Op];
    opWordCount = LUTOpWordTypesCount[(int)op.Op];

    for (int i = 0; i < opWordCount; i++) {
      wordTypes[i] = opWordTypes[i];
    }
  }

  for (int i = 1; i < opWordCount; i++) {
    uint32 word = *((uint32*)op.Memory + i - 1);

    if (wordTypes[i] == WordType::TLiteralNumber) {
      opline << " " << word;
    }
    else if (wordTypes[i] == WordType::TId) {
      opline << " [" << word;
      if(prog != nullptr && op.Op != Op::OpName && op.Op != Op::OpMemberName) {
        auto desc = getDescriptor(word, prog);
        if(desc.size() > 0) {
          opline << "(" << getDescriptor(word, prog) << ")";
        }
      }
      opline << "]";
    }
    else if (wordTypes[i] == WordType::TIdList) {
      uint32 count = word;
      uint32* ptr = *(uint32**)((uint32*)op.Memory + i);
      opline << " [";
      for (int j = 0; j < count; j++) {
        word = ptr[j];
        opline << "[" << word;
        if(prog != nullptr && op.Op != Op::OpName && op.Op != Op::OpMemberName) {
          auto desc = getDescriptor(word, prog);
          if(desc.size() > 0) {
            opline << "(" << getDescriptor(word, prog) << ")";
          }
        }
        opline << "]" << (j != count - 1 ? ", " : "");
      }
      opline << "]";

      break;
    }
    else if (wordTypes[i] == WordType::TLiteralNumberList) {
      uint32 count = word;
      uint32* ptr = *(uint32**)((uint32*)op.Memory + i);
      opline << " [";
      for (int j = 0; j < count; j++) {
        word = ptr[j];
        opline << word << (j != count - 1 ? ", " : "");
      }
      opline << "]";

      break;
    }
    else if (wordTypes[i] == WordType::TLiteralString) {
      if (wordTypes[i - 1] != WordType::TLiteralString) {
        char* str = *((char**)((uint32*)op.Memory + i - 1));
        opline << " " << str;
      }
    }
    else {
      std::string* lutPointer = *((std::string**)LUTPointers + (uint32)wordTypes[i]);
      std::string name = lutPointer[word];
      opline << " " << name;
    }
  }

  opline << std::endl;
  return opline.str();
}

std::string writeProgram(const Program& prog) {
  std::stringstream progStream;
  progStream << "Version: " << prog.Version << std::endl;
  progStream << "Generator Magic: " << prog.GeneratorMagic << std::endl;
  progStream << "ID Bound: " << prog.IDBound << std::endl;
  progStream << "Instruction Schema: " << prog.InstructionSchema << std::endl;
  progStream << "=================================================" << std::endl;
  int instructionIndex = 0;
  for(auto& op : prog.Ops) {
    progStream << std::setw(3) << instructionIndex << ": " << writeOp(op, &prog);
    instructionIndex++;
  }
  return progStream.str();
}

std::string writeOp(SOp op) {
  return writeOp(op, nullptr);
}
<|endoftext|>


<commit_before>#include "port_win.h"

#include <Windows.h>

namespace leveldb {
namespace port {

Mutex::
	Mutex()
#ifndef NDEBUG
	: holding_thread_id_(0)
#endif
{
	static_assert(sizeof(CRITICAL_SECTION) <= SIZEOF_CRITICAL_SECTION, "update Mutex::SIZEOF_CRITICAL_SECTION");
	InitializeCriticalSection(_Out_ (LPCRITICAL_SECTION)cs_);
}

Mutex::
	~Mutex()
{
	DeleteCriticalSection(_Inout_ (LPCRITICAL_SECTION)cs_);
}

void Mutex::
	Lock()
{
	EnterCriticalSection(_Inout_ (LPCRITICAL_SECTION)cs_);
#ifndef NDEBUG
	holding_thread_id_ = GetCurrentThreadId();
#endif
}

void Mutex::
	Unlock()
{
#ifndef NDEBUG
	holding_thread_id_ = 0;
#endif
	LeaveCriticalSection(_Inout_ (LPCRITICAL_SECTION)cs_);
}

#ifndef NDEBUG
void Mutex::
	AssertHeld()
{
	if ( holding_thread_id_ != GetCurrentThreadId() ) {
		fprintf(stderr, "Mutex::AssertHeld failed, current thread: %d, holding thread: %d\n", GetCurrentThreadId(), holding_thread_id_);
		abort();
	}
}
#endif

CondVar::
	CondVar(Mutex* mu)
	: mu_(mu)
{
	static_assert(sizeof(CONDITION_VARIABLE) >= SIZEOF_CONDITION_VARIABLE, "update Mutex::SIZEOF_CONDITION_VARIABLE");
	InitializeConditionVariable((PCONDITION_VARIABLE)cv_);
}

CondVar::
	~CondVar()
{
}

void CondVar::
	Wait()
{
	BOOL b = SleepConditionVariableCS((PCONDITION_VARIABLE)cv_, (PCRITICAL_SECTION)(mu_->cs_), INFINITE);
	if ( !b )
		fprintf(stderr, "CondVar::Wait, SleepConditionVariableCS returned failure");
}

void CondVar::
	Signal()
{
	WakeConditionVariable((PCONDITION_VARIABLE)cv_);
}

void CondVar::
	SignalAll()
{
	WakeAllConditionVariable((PCONDITION_VARIABLE)cv_);
}

  // Read and return the stored pointer with the guarantee that no
  // later memory access (read or write) by this thread can be
  // reordered ahead of this read.
void* AtomicPointer::
	Acquire_Load() const
{
	//Perform an exchange operation on the condition if the value is 0
	//If it's zero, exchange with zero, return original value (0)
	//If it's nonzero, don't exchange, return original value.
	//So the exchange operation won't change the value, this is a const read operation
	AtomicPointer* mutable_this = const_cast<AtomicPointer*>(this);

	return InterlockedCompareExchangePointerAcquire(&mutable_this->rep_, 0, 0);
}

void AtomicPointer::
	Release_Store(void* v)
{
    PVOID old;

    do {
        old = rep_;
    } while (InterlockedCompareExchangePointerRelease(&rep_,
                                          v,
                                          old) != old);
}

BOOL CALLBACK InitOnceCallback(
  _Inout_      PINIT_ONCE InitOnce,
  _Inout_opt_  PVOID Parameter,
  _Out_opt_    PVOID *Context
)
{
	typedef void (*initializer_fn)();
	((initializer_fn)Parameter)();
	return TRUE;
}

void InitOnce(port::OnceType* once, void (*initializer)())
{
	INIT_ONCE* real_once = (INIT_ONCE*)once;
	static_assert(sizeof(*real_once) == sizeof(*once), "port::OnceType must be the same as INIT_ONCE");
	BOOL b = InitOnceExecuteOnce(
		real_once,
		InitOnceCallback,
		initializer,
		0);
}

#if 0
// ------------------ Compression -------------------

// Store the snappy compression of "input[0,input_length-1]" in *output.
// Returns false if snappy is not supported by this port.
extern bool Snappy_Compress(const char* input, size_t input_length,
                            std::string* output);

// If input[0,input_length-1] looks like a valid snappy compressed
// buffer, store the size of the uncompressed data in *result and
// return true.  Else return false.
extern bool Snappy_GetUncompressedLength(const char* input, size_t length,
                                         size_t* result);

// Attempt to snappy uncompress input[0,input_length-1] into *output.
// Returns true if successful, false if the input is invalid lightweight
// compressed data.
//
// REQUIRES: at least the first "n" bytes of output[] must be writable
// where "n" is the result of a successful call to
// Snappy_GetUncompressedLength.
extern bool Snappy_Uncompress(const char* input_data, size_t input_length,
                              char* output);

// ------------------ Miscellaneous -------------------

// If heap profiling is not supported, returns false.
// Else repeatedly calls (*func)(arg, data, n) and then returns true.
// The concatenation of all "data[0,n-1]" fragments is the heap profile.
extern bool GetHeapProfile(void (*func)(void*, const char*, int), void* arg);

#endif  // STORAGE_LEVELDB_PORT_PORT_EXAMPLE_H_

}  // namespace port
}  // namespace leveldb

<commit_msg>Mutex holding_thread_id restored after woken up in CondVar<commit_after>#include "port_win.h"

#include <Windows.h>

namespace leveldb {
namespace port {

Mutex::
	Mutex()
#ifndef NDEBUG
	: holding_thread_id_(0)
#endif
{
	static_assert(sizeof(CRITICAL_SECTION) <= SIZEOF_CRITICAL_SECTION, "update Mutex::SIZEOF_CRITICAL_SECTION");
	InitializeCriticalSection(_Out_ (LPCRITICAL_SECTION)cs_);
}

Mutex::
	~Mutex()
{
	DeleteCriticalSection(_Inout_ (LPCRITICAL_SECTION)cs_);
}

void Mutex::
	Lock()
{
	EnterCriticalSection(_Inout_ (LPCRITICAL_SECTION)cs_);
#ifndef NDEBUG
	holding_thread_id_ = GetCurrentThreadId();
#endif
}

void Mutex::
	Unlock()
{
#ifndef NDEBUG
	if ( holding_thread_id_ != 0 )
		AssertHeld();
	holding_thread_id_ = 0;
#endif
	LeaveCriticalSection(_Inout_ (LPCRITICAL_SECTION)cs_);
}

#ifndef NDEBUG
void Mutex::
	AssertHeld()
{
	if ( holding_thread_id_ != GetCurrentThreadId() ) {
		fprintf(stderr, "Mutex::AssertHeld failed, current thread: %d, holding thread: %d\n", GetCurrentThreadId(), holding_thread_id_);
		abort();
	}
}
#endif

CondVar::
	CondVar(Mutex* mu)
	: mu_(mu)
{
	static_assert(sizeof(CONDITION_VARIABLE) >= SIZEOF_CONDITION_VARIABLE, "update Mutex::SIZEOF_CONDITION_VARIABLE");
	InitializeConditionVariable((PCONDITION_VARIABLE)cv_);
}

CondVar::
	~CondVar()
{
}

void CondVar::
	Wait()
{
	mu_->AssertHeld();
	BOOL b = SleepConditionVariableCS((PCONDITION_VARIABLE)cv_, (PCRITICAL_SECTION)(mu_->cs_), INFINITE);
	if ( !b )
		fprintf(stderr, "CondVar::Wait, SleepConditionVariableCS returned failure");
#ifndef NDEBUG
	mu_->holding_thread_id_ = GetCurrentThreadId();
#endif
}

void CondVar::
	Signal()
{
	WakeConditionVariable((PCONDITION_VARIABLE)cv_);
}

void CondVar::
	SignalAll()
{
	WakeAllConditionVariable((PCONDITION_VARIABLE)cv_);
}

  // Read and return the stored pointer with the guarantee that no
  // later memory access (read or write) by this thread can be
  // reordered ahead of this read.
void* AtomicPointer::
	Acquire_Load() const
{
	//Perform an exchange operation on the condition if the value is 0
	//If it's zero, exchange with zero, return original value (0)
	//If it's nonzero, don't exchange, return original value.
	//So the exchange operation won't change the value, this is a const read operation
	AtomicPointer* mutable_this = const_cast<AtomicPointer*>(this);

	return InterlockedCompareExchangePointerAcquire(&mutable_this->rep_, 0, 0);
}

void AtomicPointer::
	Release_Store(void* v)
{
    PVOID old;

    do {
        old = rep_;
    } while (InterlockedCompareExchangePointerRelease(&rep_,
                                          v,
                                          old) != old);
}

BOOL CALLBACK InitOnceCallback(
  _Inout_      PINIT_ONCE InitOnce,
  _Inout_opt_  PVOID Parameter,
  _Out_opt_    PVOID *Context
)
{
	typedef void (*initializer_fn)();
	((initializer_fn)Parameter)();
	return TRUE;
}

void InitOnce(port::OnceType* once, void (*initializer)())
{
	INIT_ONCE* real_once = (INIT_ONCE*)once;
	static_assert(sizeof(*real_once) == sizeof(*once), "port::OnceType must be the same as INIT_ONCE");
	BOOL b = InitOnceExecuteOnce(
		real_once,
		InitOnceCallback,
		initializer,
		0);
}

#if 0
// ------------------ Compression -------------------

// Store the snappy compression of "input[0,input_length-1]" in *output.
// Returns false if snappy is not supported by this port.
extern bool Snappy_Compress(const char* input, size_t input_length,
                            std::string* output);

// If input[0,input_length-1] looks like a valid snappy compressed
// buffer, store the size of the uncompressed data in *result and
// return true.  Else return false.
extern bool Snappy_GetUncompressedLength(const char* input, size_t length,
                                         size_t* result);

// Attempt to snappy uncompress input[0,input_length-1] into *output.
// Returns true if successful, false if the input is invalid lightweight
// compressed data.
//
// REQUIRES: at least the first "n" bytes of output[] must be writable
// where "n" is the result of a successful call to
// Snappy_GetUncompressedLength.
extern bool Snappy_Uncompress(const char* input_data, size_t input_length,
                              char* output);

// ------------------ Miscellaneous -------------------

// If heap profiling is not supported, returns false.
// Else repeatedly calls (*func)(arg, data, n) and then returns true.
// The concatenation of all "data[0,n-1]" fragments is the heap profile.
extern bool GetHeapProfile(void (*func)(void*, const char*, int), void* arg);

#endif  // STORAGE_LEVELDB_PORT_PORT_EXAMPLE_H_

}  // namespace port
}  // namespace leveldb

<|endoftext|>