<commit_before>///
/// @file  pi_deleglise_rivat3.cpp
/// @brief Implementation of the Deleglise-Rivat prime counting
///        algorithm. This implementation is identical to
///        pi_deleglise_rivat2(x) but uses 128-bit integers.
///
///        This implementation is based on the paper:
///        Tomás Oliveira e Silva, Computing pi(x): the combinatorial
///        method, Revista do DETUA, vol. 4, no. 6, March 2006,
///        pp. 759-768.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <PiTable.hpp>
#include <FactorTable.hpp>
#include <primecount.hpp>
#include <primecount-internal.hpp>
#include <BitSieve.hpp>
#include <generate.hpp>
#include <pmath.hpp>
#include <PhiTiny.hpp>
#include <int128.hpp>
#include <S1.hpp>
#include <tos_counters.hpp>

#include <stdint.h>
#include <algorithm>
#include <vector>

using namespace std;
using namespace primecount;

namespace {

/// Cross-off the multiples of prime in the sieve array.
/// For each element that is unmarked the first time update
/// the special counters tree data structure.
///
template <typename T>
void cross_off(int64_t prime,
               int64_t low,
               int64_t high,
               int64_t& next_multiple,
               BitSieve& sieve,
               T& counters)
{
  int64_t segment_size = sieve.size();
  int64_t k = next_multiple;

  for (; k < high; k += prime * 2)
  {
    if (sieve[k - low])
    {
      sieve.unset(k - low);
      cnt_update(counters, k - low, segment_size);
    }
  }
  next_multiple = k;
}

/// Calculate the contribution of the trivial leaves.
///
template <typename P>
int128_t S2_trivial(uint128_t x,
                   int64_t y,
                   int64_t z,
                   int64_t c,
                   PiTable& pi,
                   vector<P>& primes)
{
  int64_t pi_y = pi(y);
  int64_t pi_sqrtz = pi(min(isqrt(z), y));
  int128_t S2_result = 0;

  // Find all trivial leaves: n = primes[b] * primes[l]
  // which satisfy phi(x / n), b - 1) = 1
  for (int64_t b = max(c, pi_sqrtz + 1); b < pi_y; b++)
  {
    uint128_t prime = primes[b];
    uint64_t xn = (uint64_t) max(x / (prime * prime), prime);
    S2_result += pi_y - pi(xn);
  }

  return S2_result;
}

/// Calculate the contribution of the trivial leaves, the clustered
/// easy leaves and the sparse easy leaves.
///
template <typename P>
int128_t S2_easy(uint128_t x,
                 int64_t y,
                 int64_t z,
                 int64_t c,
                 PiTable& pi,
                 vector<P>& primes)
{
  int64_t pi_sqrty = pi(isqrt(y));
  int64_t pi_x13 = pi(iroot<3>(x));
  int128_t S2_result = 0;

  for (int64_t b = max(c, pi_sqrty) + 1; b <= pi_x13; b++)
  {
    int128_t prime128 = primes[b];
    int64_t prime = primes[b];
    int64_t min_trivial_leaf = min(x / (prime128 * prime), y);
    int64_t min_clustered_easy_leaf = isqrt(x / prime);
    int64_t min_sparse_easy_leaf = z / prime;
    int64_t min_hard_leaf = max(y / prime, prime);

    min_sparse_easy_leaf = max(min_sparse_easy_leaf, min_hard_leaf);
    min_clustered_easy_leaf = max(min_clustered_easy_leaf, min_hard_leaf);
    int64_t l = pi(min_trivial_leaf);

    // Find all clustered easy leaves:
    // x / n <= y and phi(x / n, b - 1) == phi(x / m, b - 1)
    // where phi(x / n, b - 1) = pi(x / n) - b + 2
    while (primes[l] > min_clustered_easy_leaf)
    {
      int128_t n = prime128 * primes[l];
      int64_t xn = (int64_t) (x / n);
      int64_t phi_xn = pi(xn) - b + 2;
      int128_t m = prime128 * primes[b + phi_xn - 1];
      int64_t xm = max((int64_t) (x / m), min_clustered_easy_leaf);
      int64_t l2 = pi(xm);
      int128_t phi_factor = l - l2;
      S2_result += phi_xn * phi_factor;
      l = l2;
    }

    // Find all sparse easy leaves:
    // x / n <= y and phi(x / n, b - 1) = pi(x / n) - b + 2
    for (; primes[l] > min_sparse_easy_leaf; l--)
    {
      int128_t n = prime128 * primes[l];
      int64_t xn = (int64_t) (x / n);
      S2_result += pi(xn) - b + 2;
    }
  }

  return S2_result;
}

/// Calculate the contribution of the special leaves which require
/// a sieve (in order to reduce the memory usage).
///
template <typename P, typename F>
int128_t S2_sieve(uint128_t x,
                  int64_t y,
                  int64_t z,
                  int64_t c,
                  PiTable& pi,
                  vector<P>& primes,
                  FactorTable<F>& factors)
{
  int64_t limit = z + 1;
  int64_t segment_size = next_power_of_2(isqrt(limit));
  int64_t pi_sqrty = pi(isqrt(y));
  int64_t pi_sqrtz = pi(min(isqrt(z), y));
  int128_t S2_result = 0;

  BitSieve sieve(segment_size);
  vector<int32_t> counters(segment_size);
  vector<int64_t> next(primes.begin(), primes.end());
  vector<int64_t> phi(primes.size(), 0);

  // Segmented sieve of Eratosthenes
  for (int64_t low = 1; low < limit; low += segment_size)
  {
    // Current segment = interval [low, high[
    int64_t high = min(low + segment_size, limit);
    int64_t b = 2;

    sieve.fill(low, high);

    // phi(y, b) nodes with b <= c do not contribute to S2, so we
    // simply sieve out the multiples of the first c primes
    for (; b <= c; b++)
    {
      int64_t k = next[b];
      for (int64_t prime = primes[b]; k < high; k += prime * 2)
        sieve.unset(k - low);
      next[b] = k;
    }

    // Initialize special tree data structure from sieve
    cnt_finit(sieve, counters, segment_size);

    // For c + 1 <= b <= pi_sqrty
    // Find all special leaves: n = primes[b] * m, with mu[m] != 0 and primes[b] < lpf[m]
    // which satisfy: low <= (x / n) < high
    for (; b <= pi_sqrty; b++)
    {
      int128_t prime128 = primes[b];
      int64_t prime = primes[b];
      int64_t min_m = max(min(x / (prime128 * high), y), y / prime);
      int64_t max_m = min(x / (prime128 * low), y);

      if (prime >= max_m)
        goto next_segment;

      factors.to_index(&min_m);
      factors.to_index(&max_m);

      for (int64_t m = max_m; m > min_m; m--)
      {
        if (prime < factors.lpf(m))
        {
          int64_t n = prime * factors.get_number(m);
          int64_t count = cnt_query(counters, (x / n) - low);
          int64_t phi_xn = phi[b] + count;
          S2_result -= factors.mu(m) * phi_xn;
        }
      }

      phi[b] += cnt_query(counters, (high - 1) - low);
      cross_off(prime, low, high, next[b], sieve, counters);
    }

    // For pi_sqrty <= b <= pi_sqrtz
    // Find all hard special leaves: n = primes[b] * primes[l]
    // which satisfy: low <= (x / n) < high
    for (; b <= pi_sqrtz; b++)
    {
      int128_t prime128 = primes[b];
      int64_t prime = primes[b];
      int64_t l = pi(min(min(x / (prime128 * low), y), z / prime));
      int64_t min_hard_leaf = max3(min(x / (prime128 * high), y), y / prime, prime);

      if (prime >= primes[l])
        goto next_segment;

      for (; primes[l] > min_hard_leaf; l--)
      {
        int64_t n = prime * primes[l];
        int64_t xn = x / n;
        int64_t count = cnt_query(counters, xn - low);
        int64_t phi_xn = phi[b] + count;
        S2_result += phi_xn;
      }

      phi[b] += cnt_query(counters, (high - 1) - low);
      cross_off(prime, low, high, next[b], sieve, counters);
    }

    next_segment:;
  }

  return S2_result;
}

/// Calculate the contribution of the special leaves.
/// @pre y > 0 && c > 1
///
template <typename P, typename F>
int128_t S2(uint128_t x,
            int64_t y,
            int64_t z,
            int64_t c,
            vector<P>& primes,
            FactorTable<F>& factors)
{
  int128_t S2_total = 0;
  PiTable pi(y);

  S2_total += S2_trivial(x, y, z, c, pi, primes);
  S2_total += S2_easy(x, y, z, c, pi, primes);
  S2_total += S2_sieve(x, y, z, c, pi, primes, factors);

  return S2_total;
}

/// alpha is a tuning factor which should grow like (log(x))^3
/// for the Deleglise-Rivat prime counting algorithm.
///
double compute_alpha(uint128_t x)
{
  double d = (double) x;
  double alpha = log(d) * log(d) * log(d) / 1200;
  return in_between(1, alpha, iroot<6>(x));
}

} // namespace

namespace primecount {

/// Calculate the number of primes below x using the
/// Deleglise-Rivat algorithm.
/// Run time: O(x^(2/3) / (log x)^2) operations, O(x^(1/3) * (log x)^3) space.
///
int128_t pi_deleglise_rivat3(int128_t x)
{
  if (x < 2)
    return 0;

  if (x > to_maxint(primecount::max()))
    throw primecount_error("pi(x): x must be <= " + max());

  double alpha = compute_alpha(x);
  int64_t y = (int64_t) (alpha * iroot<3>(x));
  int64_t z = (int64_t) (x / y);
  int64_t pi_y;
  int64_t c;

  int128_t p2 = P2(x, y, 1);
  int128_t s1, s2;

  if (y <= FactorTable<uint16_t>::max())
  {
    // if y < 2^32 we can use 32-bit primes and a 16-bit FactorTable
    // which uses ~ (y / 2) bytes of memory

    vector<uint32_t> primes = generate_primes<uint32_t>(y);
    FactorTable<uint16_t> factors(y);

    pi_y = primes.size() - 1;
    c = min(pi_y, PhiTiny::max_a());
    s1 = S1(x, y, c, primes[c], factors, 1);
    s2 = S2(x, y, z, c, primes, factors);
  }
  else
  {
    // if y >= 2^32 we need to use 64-bit primes and a 32-bit
    // FactorTable which uses ~ y bytes of memory

    vector<int64_t> primes = generate_primes<int64_t>(y);
    FactorTable<uint32_t> factors(y);

    pi_y = primes.size() - 1;
    c = min(pi_y, PhiTiny::max_a());
    s1 = S1(x, y, c, primes[c], factors, 1);
    s2 = S2(x, y, z, c, primes, factors);
  }

  int128_t phi = s1 + s2;
  int128_t sum = phi + pi_y - 1 - p2;

  return sum;
}

} // namespace primecount
<commit_msg>Update pi_deleglise_rivat3.cpp<commit_after>///
/// @file  pi_deleglise_rivat3.cpp
/// @brief Implementation of the Deleglise-Rivat prime counting
///        algorithm. This implementation is identical to
///        pi_deleglise_rivat2(x) but uses 128-bit integers.
///
///        This implementation is based on the paper:
///        Tomás Oliveira e Silva, Computing pi(x): the combinatorial
///        method, Revista do DETUA, vol. 4, no. 6, March 2006,
///        pp. 759-768.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <PiTable.hpp>
#include <FactorTable.hpp>
#include <primecount.hpp>
#include <primecount-internal.hpp>
#include <BitSieve.hpp>
#include <generate.hpp>
#include <pmath.hpp>
#include <PhiTiny.hpp>
#include <int128.hpp>
#include <S1.hpp>
#include <tos_counters.hpp>

#include <stdint.h>
#include <algorithm>
#include <vector>

using namespace std;
using namespace primecount;

namespace {

/// Cross-off the multiples of prime in the sieve array.
/// For each element that is unmarked the first time update
/// the special counters tree data structure.
///
template <typename T>
void cross_off(int64_t prime,
               int64_t low,
               int64_t high,
               int64_t& next_multiple,
               BitSieve& sieve,
               T& counters)
{
  int64_t segment_size = sieve.size();
  int64_t k = next_multiple;

  for (; k < high; k += prime * 2)
  {
    if (sieve[k - low])
    {
      sieve.unset(k - low);
      cnt_update(counters, k - low, segment_size);
    }
  }
  next_multiple = k;
}

/// Calculate the contribution of the trivial leaves.
///
template <typename P>
int128_t S2_trivial(uint128_t x,
                   int64_t y,
                   int64_t z,
                   int64_t c,
                   PiTable& pi,
                   vector<P>& primes)
{
  int64_t pi_y = pi(y);
  int64_t pi_sqrtz = pi(min(isqrt(z), y));
  int128_t S2_result = 0;

  // Find all trivial leaves: n = primes[b] * primes[l]
  // which satisfy phi(x / n), b - 1) = 1
  for (int64_t b = max(c, pi_sqrtz + 1); b < pi_y; b++)
  {
    uint128_t prime = primes[b];
    uint64_t xn = (uint64_t) max(x / (prime * prime), prime);
    S2_result += pi_y - pi(xn);
  }

  return S2_result;
}

/// Calculate the contribution of the clustered easy
/// leaves and the sparse easy leaves.
///
template <typename P>
int128_t S2_easy(uint128_t x,
                 int64_t y,
                 int64_t z,
                 int64_t c,
                 PiTable& pi,
                 vector<P>& primes)
{
  int64_t pi_sqrty = pi(isqrt(y));
  int64_t pi_x13 = pi(iroot<3>(x));
  int128_t S2_result = 0;

  for (int64_t b = max(c, pi_sqrty) + 1; b <= pi_x13; b++)
  {
    int128_t prime128 = primes[b];
    int64_t prime = primes[b];
    int64_t min_trivial_leaf = min(x / (prime128 * prime), y);
    int64_t min_clustered_easy_leaf = isqrt(x / prime);
    int64_t min_sparse_easy_leaf = z / prime;
    int64_t min_hard_leaf = max(y / prime, prime);

    min_sparse_easy_leaf = max(min_sparse_easy_leaf, min_hard_leaf);
    min_clustered_easy_leaf = max(min_clustered_easy_leaf, min_hard_leaf);
    int64_t l = pi(min_trivial_leaf);

    // Find all clustered easy leaves:
    // x / n <= y and phi(x / n, b - 1) == phi(x / m, b - 1)
    // where phi(x / n, b - 1) = pi(x / n) - b + 2
    while (primes[l] > min_clustered_easy_leaf)
    {
      int128_t n = prime128 * primes[l];
      int64_t xn = (int64_t) (x / n);
      int64_t phi_xn = pi(xn) - b + 2;
      int128_t m = prime128 * primes[b + phi_xn - 1];
      int64_t xm = max((int64_t) (x / m), min_clustered_easy_leaf);
      int64_t l2 = pi(xm);
      int128_t phi_factor = l - l2;
      S2_result += phi_xn * phi_factor;
      l = l2;
    }

    // Find all sparse easy leaves:
    // x / n <= y and phi(x / n, b - 1) = pi(x / n) - b + 2
    for (; primes[l] > min_sparse_easy_leaf; l--)
    {
      int128_t n = prime128 * primes[l];
      int64_t xn = (int64_t) (x / n);
      S2_result += pi(xn) - b + 2;
    }
  }

  return S2_result;
}

/// Calculate the contribution of the special leaves which require
/// a sieve (in order to reduce the memory usage).
///
template <typename P, typename F>
int128_t S2_sieve(uint128_t x,
                  int64_t y,
                  int64_t z,
                  int64_t c,
                  PiTable& pi,
                  vector<P>& primes,
                  FactorTable<F>& factors)
{
  int64_t limit = z + 1;
  int64_t segment_size = next_power_of_2(isqrt(limit));
  int64_t pi_sqrty = pi(isqrt(y));
  int64_t pi_sqrtz = pi(min(isqrt(z), y));
  int128_t S2_result = 0;

  BitSieve sieve(segment_size);
  vector<int32_t> counters(segment_size);
  vector<int64_t> next(primes.begin(), primes.end());
  vector<int64_t> phi(primes.size(), 0);

  // Segmented sieve of Eratosthenes
  for (int64_t low = 1; low < limit; low += segment_size)
  {
    // Current segment = interval [low, high[
    int64_t high = min(low + segment_size, limit);
    int64_t b = 2;

    sieve.fill(low, high);

    // phi(y, b) nodes with b <= c do not contribute to S2, so we
    // simply sieve out the multiples of the first c primes
    for (; b <= c; b++)
    {
      int64_t k = next[b];
      for (int64_t prime = primes[b]; k < high; k += prime * 2)
        sieve.unset(k - low);
      next[b] = k;
    }

    // Initialize special tree data structure from sieve
    cnt_finit(sieve, counters, segment_size);

    // For c + 1 <= b <= pi_sqrty
    // Find all special leaves: n = primes[b] * m, with mu[m] != 0 and primes[b] < lpf[m]
    // which satisfy: low <= (x / n) < high
    for (; b <= pi_sqrty; b++)
    {
      int128_t prime128 = primes[b];
      int64_t prime = primes[b];
      int64_t min_m = max(min(x / (prime128 * high), y), y / prime);
      int64_t max_m = min(x / (prime128 * low), y);

      if (prime >= max_m)
        goto next_segment;

      factors.to_index(&min_m);
      factors.to_index(&max_m);

      for (int64_t m = max_m; m > min_m; m--)
      {
        if (prime < factors.lpf(m))
        {
          int64_t n = prime * factors.get_number(m);
          int64_t count = cnt_query(counters, (x / n) - low);
          int64_t phi_xn = phi[b] + count;
          S2_result -= factors.mu(m) * phi_xn;
        }
      }

      phi[b] += cnt_query(counters, (high - 1) - low);
      cross_off(prime, low, high, next[b], sieve, counters);
    }

    // For pi_sqrty <= b <= pi_sqrtz
    // Find all hard special leaves: n = primes[b] * primes[l]
    // which satisfy: low <= (x / n) < high
    for (; b <= pi_sqrtz; b++)
    {
      int128_t prime128 = primes[b];
      int64_t prime = primes[b];
      int64_t l = pi(min(min(x / (prime128 * low), y), z / prime));
      int64_t min_hard_leaf = max3(min(x / (prime128 * high), y), y / prime, prime);

      if (prime >= primes[l])
        goto next_segment;

      for (; primes[l] > min_hard_leaf; l--)
      {
        int64_t n = prime * primes[l];
        int64_t xn = x / n;
        int64_t count = cnt_query(counters, xn - low);
        int64_t phi_xn = phi[b] + count;
        S2_result += phi_xn;
      }

      phi[b] += cnt_query(counters, (high - 1) - low);
      cross_off(prime, low, high, next[b], sieve, counters);
    }

    next_segment:;
  }

  return S2_result;
}

/// Calculate the contribution of the special leaves.
/// @pre y > 0 && c > 1
///
template <typename P, typename F>
int128_t S2(uint128_t x,
            int64_t y,
            int64_t z,
            int64_t c,
            vector<P>& primes,
            FactorTable<F>& factors)
{
  int128_t S2_total = 0;
  PiTable pi(y);

  S2_total += S2_trivial(x, y, z, c, pi, primes);
  S2_total += S2_easy(x, y, z, c, pi, primes);
  S2_total += S2_sieve(x, y, z, c, pi, primes, factors);

  return S2_total;
}

/// alpha is a tuning factor which should grow like (log(x))^3
/// for the Deleglise-Rivat prime counting algorithm.
///
double compute_alpha(uint128_t x)
{
  double d = (double) x;
  double alpha = log(d) * log(d) * log(d) / 1200;
  return in_between(1, alpha, iroot<6>(x));
}

} // namespace

namespace primecount {

/// Calculate the number of primes below x using the
/// Deleglise-Rivat algorithm.
/// Run time: O(x^(2/3) / (log x)^2) operations, O(x^(1/3) * (log x)^3) space.
///
int128_t pi_deleglise_rivat3(int128_t x)
{
  if (x < 2)
    return 0;

  if (x > to_maxint(primecount::max()))
    throw primecount_error("pi(x): x must be <= " + max());

  double alpha = compute_alpha(x);
  int64_t y = (int64_t) (alpha * iroot<3>(x));
  int64_t z = (int64_t) (x / y);
  int64_t pi_y;
  int64_t c;

  int128_t p2 = P2(x, y, 1);
  int128_t s1, s2;

  if (y <= FactorTable<uint16_t>::max())
  {
    // if y < 2^32 we can use 32-bit primes and a 16-bit FactorTable
    // which uses ~ (y / 2) bytes of memory

    vector<uint32_t> primes = generate_primes<uint32_t>(y);
    FactorTable<uint16_t> factors(y);

    pi_y = primes.size() - 1;
    c = min(pi_y, PhiTiny::max_a());
    s1 = S1(x, y, c, primes[c], factors, 1);
    s2 = S2(x, y, z, c, primes, factors);
  }
  else
  {
    // if y >= 2^32 we need to use 64-bit primes and a 32-bit
    // FactorTable which uses ~ y bytes of memory

    vector<int64_t> primes = generate_primes<int64_t>(y);
    FactorTable<uint32_t> factors(y);

    pi_y = primes.size() - 1;
    c = min(pi_y, PhiTiny::max_a());
    s1 = S1(x, y, c, primes[c], factors, 1);
    s2 = S2(x, y, z, c, primes, factors);
  }

  int128_t phi = s1 + s2;
  int128_t sum = phi + pi_y - 1 - p2;

  return sum;
}

} // namespace primecount
<|endoftext|>


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY 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 OWNER 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.
 *********************************************************************/

/* Author: Ioan Sucan */

#include <moveit/ompl_interface/detail/state_validity_checker.h>
#include <moveit/ompl_interface/model_based_planning_context.h>
#include <moveit/profiler/profiler.h>
#include <ros/ros.h>

ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext *pc) :
  ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup().getSpaceInformation()), planning_context_(pc),
  group_name_(pc->getJointModelGroupName()), tss_(pc->getCompleteInitialRobotState()), verbose_(false)
{
  specs_.clearanceComputationType = ompl::base::StateValidityCheckerSpecs::APPROXIMATE;
  specs_.hasValidDirectionComputation = false;

  collision_request_with_distance_.distance = true;
  collision_request_with_cost_.cost = true;

  collision_request_simple_.group_name = planning_context_->getJointModelGroupName();
  collision_request_with_distance_.group_name = planning_context_->getJointModelGroupName();
  collision_request_with_cost_.group_name = planning_context_->getJointModelGroupName();

  collision_request_simple_verbose_ = collision_request_simple_;
  collision_request_simple_verbose_.verbose = true;

  collision_request_with_distance_verbose_ = collision_request_with_distance_;
  collision_request_with_distance_verbose_.verbose = true;
}

void ompl_interface::StateValidityChecker::setVerbose(bool flag)
{
  verbose_ = flag;
}

bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State *state, bool verbose) const
{
  //  moveit::Profiler::ScopedBlock sblock("isValid");
  return planning_context_->useStateValidityCache() ? isValidWithCache(state, verbose) : isValidWithoutCache(state, verbose);
}

bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State *state, double &dist, bool verbose) const
{
  //  moveit::Profiler::ScopedBlock sblock("isValid");
  return planning_context_->useStateValidityCache() ? isValidWithCache(state, dist, verbose) : isValidWithoutCache(state, dist, verbose);
}

double ompl_interface::StateValidityChecker::cost(const ompl::base::State *state) const
{
  static std::vector<double> prev_joint_values;
  static bool first_time = true;
  static boost::thread::id thread_id = boost::this_thread::get_id();

  // Check that we are still on same thread
  if( thread_id != boost::this_thread::get_id() )
    ROS_ERROR_STREAM("Multiple threads are not implemented here...");

  // Get current robot state
  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // Cost
  double cost = 0.0;
  std::vector<double> new_joint_values;

  // get joint state values
  kstate->getStateValues(new_joint_values);

  // calculate cost
  for (std::size_t i = 0; i < new_joint_values.size(); ++i)
  {
    double value = 0;
    // Check if first time
    if (first_time)
    {
      first_time = false;
      prev_joint_values.resize(new_joint_values.size());
    }
    else
    {
      // get abs of difference in each joint value
      value = fabs(prev_joint_values[i] - new_joint_values[i]);
    }

    cost += value;

    // copy new value to old vector
    prev_joint_values[i] = new_joint_values[i];
  }

  return cost;

  /*
    robot_state::RobotState *kstate = tss_.getStateStorage();
    planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

    collision_detection::CollisionResult res;
    planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *kstate);

    double c = 0.0;
    for (std::set<collision_detection::CostSource>::const_iterator it = res.cost_sources.begin() ; it != res.cost_sources.end() ; ++it)
    c += it->cost * it->getVolume();

    return c;
  */
}

double ompl_interface::StateValidityChecker::clearance(const ompl::base::State *state) const
{
  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *kstate);
  return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits<double>::infinity() : res.distance);
}

bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State *state, bool verbose) const
{
  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset && !kset->decide(*kstate, verbose).satisfied)
    return false;

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
    return false;

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *kstate);
  return res.collision == false;
}

bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State *state, double &dist, bool verbose) const
{
  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset)
  {
    kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
    if (!cer.satisfied)
    {
      dist = cer.distance;
      return false;
    }
  }

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
  {
    dist = 0.0;
    return false;
  }

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *kstate);
  dist = res.distance;
  return res.collision == false;
}

bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State *state, bool verbose) const
{
  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown())
    return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();

  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset && !kset->decide(*kstate, verbose).satisfied)
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *kstate);
  if (res.collision == false)
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markValid();
    return true;
  }
  else
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }
}

bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State *state, double &dist, bool verbose) const
{
  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown() && state->as<ModelBasedStateSpace::StateType>()->isGoalDistanceKnown())
  {
    dist = state->as<ModelBasedStateSpace::StateType>()->distance;
    return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
  }

  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(0.0);
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset)
  {
    kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
    if (!cer.satisfied)
    {
      dist = cer.distance;
      const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(dist);
      return false;
    }
  }

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
  {
    dist = 0.0;
    return false;
  }

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *kstate);
  dist = res.distance;
  return res.collision == false;
}
<commit_msg>Removed formatting changes<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY 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 OWNER 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.
*********************************************************************/

/* Author: Ioan Sucan */

#include <moveit/ompl_interface/detail/state_validity_checker.h>
#include <moveit/ompl_interface/model_based_planning_context.h>
#include <moveit/profiler/profiler.h>
#include <ros/ros.h>

ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext *pc) :
  ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup().getSpaceInformation()), planning_context_(pc),
  group_name_(pc->getJointModelGroupName()), tss_(pc->getCompleteInitialRobotState()), verbose_(false)
{
  specs_.clearanceComputationType = ompl::base::StateValidityCheckerSpecs::APPROXIMATE;
  specs_.hasValidDirectionComputation = false;

  collision_request_with_distance_.distance = true;
  collision_request_with_cost_.cost = true;

  collision_request_simple_.group_name = planning_context_->getJointModelGroupName();
  collision_request_with_distance_.group_name = planning_context_->getJointModelGroupName();
  collision_request_with_cost_.group_name = planning_context_->getJointModelGroupName();

  collision_request_simple_verbose_ = collision_request_simple_;
  collision_request_simple_verbose_.verbose = true;

  collision_request_with_distance_verbose_ = collision_request_with_distance_;
  collision_request_with_distance_verbose_.verbose = true;
}

void ompl_interface::StateValidityChecker::setVerbose(bool flag)
{
  verbose_ = flag;
}

bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State *state, bool verbose) const
{
  //  moveit::Profiler::ScopedBlock sblock("isValid");
  return planning_context_->useStateValidityCache() ? isValidWithCache(state, verbose) : isValidWithoutCache(state, verbose);
}

bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State *state, double &dist, bool verbose) const
{
  //  moveit::Profiler::ScopedBlock sblock("isValid");
  return planning_context_->useStateValidityCache() ? isValidWithCache(state, dist, verbose) : isValidWithoutCache(state, dist, verbose);
}

double ompl_interface::StateValidityChecker::cost(const ompl::base::State *state) const
{
  bool useCollisionDistanceCost = false; // TODO - set this somewhere else
  double cost = 0.0;

  if( useCollisionDistanceCost ) // proximity to collisions cost
  {
    robot_state::RobotState *kstate = tss_.getStateStorage();
    planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

    collision_detection::CollisionResult res;
    planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *kstate);

    for (std::set<collision_detection::CostSource>::const_iterator it = res.cost_sources.begin() ; it != res.cost_sources.end() ; ++it)
      cost += it->cost * it->getVolume();
  }
  else // use distance between joints cost
  {
    static std::vector<double> prev_joint_values;
    static bool first_time = true;
    static boost::thread::id thread_id = boost::this_thread::get_id();

    // Check that we are still on same thread
    if( thread_id != boost::this_thread::get_id() )
      ROS_ERROR_STREAM("Multiple threads are not implemented here...");

    // Get current robot state
    robot_state::RobotState *kstate = tss_.getStateStorage();
    planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

    std::vector<double> new_joint_values;

    // get joint state values
    kstate->getStateValues(new_joint_values);

    // calculate cost
    for (std::size_t i = 0; i < new_joint_values.size(); ++i)
    {
      double value = 0;
      // Check if first time
      if (first_time)
      {
        first_time = false;
        prev_joint_values.resize(new_joint_values.size());
      }
      else
      {
        // get abs of difference in each joint value
        value = fabs(prev_joint_values[i] - new_joint_values[i]);
      }

      cost += value;

      // copy new value to old vector
      prev_joint_values[i] = new_joint_values[i];
    }
  }

  return cost;
}

double ompl_interface::StateValidityChecker::clearance(const ompl::base::State *state) const
{
  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *kstate);
  return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits<double>::infinity() : res.distance);
}

bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State *state, bool verbose) const
{
  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset && !kset->decide(*kstate, verbose).satisfied)
    return false;

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
    return false;

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *kstate);
  return res.collision == false;
}

bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State *state, double &dist, bool verbose) const
{
  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset)
  {
    kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
    if (!cer.satisfied)
    {
      dist = cer.distance;
      return false;
    }
  }

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
  {
    dist = 0.0;
    return false;
  }

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *kstate);
  dist = res.distance;
  return res.collision == false;
}

bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State *state, bool verbose) const
{
  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown())
    return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();

  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset && !kset->decide(*kstate, verbose).satisfied)
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *kstate);
  if (res.collision == false)
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markValid();
    return true;
  }
  else
  {
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
    return false;
  }
}

bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State *state, double &dist, bool verbose) const
{
  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown() && state->as<ModelBasedStateSpace::StateType>()->isGoalDistanceKnown())
  {
    dist = state->as<ModelBasedStateSpace::StateType>()->distance;
    return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
  }

  if (!si_->satisfiesBounds(state))
  {
    if (verbose)
      logInform("State outside bounds");
    const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(0.0);
    return false;
  }

  robot_state::RobotState *kstate = tss_.getStateStorage();
  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);

  // check path constraints
  const kinematic_constraints::KinematicConstraintSetPtr &kset = planning_context_->getPathConstraints();
  if (kset)
  {
    kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
    if (!cer.satisfied)
    {
      dist = cer.distance;
      const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(dist);
      return false;
    }
  }

  // check feasibility
  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
  {
    dist = 0.0;
    return false;
  }

  // check collision avoidance
  collision_detection::CollisionResult res;
  planning_context_->getPlanningScene()->checkCollision(verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *kstate);
  dist = res.distance;
  return res.collision == false;
}
<|endoftext|>


<commit_before>#include "Controller.h"
#include <iostream>
#include <chrono>

typedef std::chrono::high_resolution_clock Clock;
typedef std::chrono::duration<double, std::milli> millisec_t;

Controller::Controller() :
	m_ballPosOptiTrack(0, 0),
	m_ballVelOptiTrack(0, 0),
	m_initialized(false)
{
	sigmaRef = 0.0;
	psiRef = 0.0;

	JRigidInv << 0, 0,
		0, 0;

	xip(0) = 0.0;
	xip(1) = 0.0;

	qpRef = JRigidInv*xip;

	psipRef = qpRef(0);
	sigmapRef = qpRef(1);

	setReferenceEnergy(0.0);
	//H = 1 / 2 * pow(0.0, 2) + g*sin(beta)*0.0;
	//Href = getReferenceEnergy();
	//Htilde = H - Href;
	//reflection gain of mirror law 0.075, // restoring of the energy href 0.01
	//this->setGains(0.075, 0.1, 0.05, 0.05);

	/* Following set of gains seems to work alright
	this->setGains(0.05, 0.001, 0.1, 0.05) // also k1 = 1e-4
	*/

	double k0 = 0.285;
	double k1 = 0.593;
	double k00 = 0.718;
	double k01 = 0.155;
	// For heavier puck
	this->setDefaultGains(k0, k1, k00, k01);
	this->setGains(k0, k1, k00, k01);
}

Controller::~Controller()
{
	delete m_serialComm;
}

int Controller::initialize(OptiTrack* optiTrackPointer)
{
	if (m_initialized)
	{
		printf("Controller already initialized.  Exiting");
	}

	m_optiTrackPointer = optiTrackPointer; //ToDo: check if pointer was initilaized
	try {
		m_serialComm = new SerialCommunicator();
	}
	catch(...)
	{
		// catch any serial errors!
		printf("COM Port not open!");
		return 1;
	}

	// create a motor object

	std::cout << "Controller Init, Thread :: ID = " << std::this_thread::get_id() << std::endl;

	int motorSetPosition = 0;
	//std::cout << "In Main Thread : Before Thread Start motorSetPosition = " << motorSetPosition << std::endl;

	// ToDo: get a handle on that thread
	m_initialized = true;

	// Start the Thread!

	std::thread threadObj(&Controller::controlArmThread, this);
	if (threadObj.joinable())
	{
		//threadObj.join();
		//std::cout << "Joined Thread " << std::endl;
		std::cout << "Detaching Control Arm Thread " << std::endl;
		threadObj.detach();
	}
}

void Controller::getDefaultGains(double* gainReflection, double* gainEnergy, double* gainHoriz, double* gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	*gainReflection = kappa0Default;
	*gainEnergy = kappa1Default;
	*gainHoriz = kappa00Default;
	*gainHorizDer = kappa01Default;
}

void Controller::setDefaultGains(const double& gainReflection, const double& gainEnergy, const double& gainHoriz, const double& gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	kappa0Default = gainReflection;
	kappa1Default = gainEnergy;
	kappa00Default = gainHoriz;
	kappa01Default = gainHorizDer;
}

void Controller::getGains(double* gainReflection, double* gainEnergy, double* gainHoriz, double* gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	*gainReflection = kappa0;
	*gainEnergy = kappa1;
	*gainHoriz = kappa00;
	*gainHorizDer = kappa01;
}

void Controller::setGains(const double& gainReflection, const double& gainEnergy, const double& gainHoriz, const double& gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	kappa0 = gainReflection;
	kappa1 = gainEnergy;
	kappa00 = gainHoriz;
	kappa01 = gainHorizDer;
}

double Controller::getReferenceEnergy()
{
	return Href;
}

void Controller::setReferenceEnergy(double referenceEnergy)
{
	Href = referenceEnergy;
}

double Controller::computeVerticalEnergy()
{
	return 1 / 2 * pow(zp, 2) + g*sin(beta)*z;
}

void Controller::computeSigmaRef()
{
	ballDistanceSquaredFromHinge = pow(ox - x, 2) + pow(oz - z, 2);
	sigmaRef = sqrt(-pow(r + rz, 2) + ballDistanceSquaredFromHinge) - rx;
}

void Controller::updateReferencePosition()
{
	/* Old calculation
	psiRef = atan2(-(oz - z)*sigmaRef + r*(ox - x), (ox - x)*sigmaRef - r*(oz - z)) - M_PI;
	psiRef = remainder(-psiRef, (2 * M_PI));
	*/
	//psiRef = atan2( (r+rz)*(ox - x) + (sigmaRef+rx)*(oz-z), (sigmaRef+rx)*(ox-x) - (r+rz)*(oz-z) );
	psiRef = atan2((r + rz)*(ox - x) + (sigmaRef + rx)*(oz - z), (sigmaRef + rx)*(ox - x) - (r + rz)*(oz - z));
}

void Controller::updateReferenceVelocity()
{
	computeJacobianInverse();
	Vector2d xip(xp, zp);
	Vector2d refVel = JRigidInv*xip;

	psipRef = refVel(0);
	sigmapRef = refVel(1);
}

void Controller::computeJacobianInverse()
{
	/*Old Jinv
	JRigidInv(0, 0) = (r*(x - ox) + (z - oz)*sigmaRef) / ballDistanceSquaredFromHinge / sigmaRef;
	JRigidInv(0, 1) = (r*(z - oz) + (ox - x)*sigmaRef) / ballDistanceSquaredFromHinge / sigmaRef;
	JRigidInv(1, 0) = (ox - x) / sigmaRef;
	JRigidInv(1, 1) = (oz - z) / sigmaRef;
	*/

	JRigidInv(0, 0) = ((r + rz)*(ox - x) + (sigmaRef + rx)*(oz - z)) / ballDistanceSquaredFromHinge / (sigmaRef + rx);
	JRigidInv(0, 1) = ((r + rz)*(oz - z) - (sigmaRef + rx)*(ox - x)) / ballDistanceSquaredFromHinge / (sigmaRef + rx);
	JRigidInv(1, 0) = -(ox - x) / (sigmaRef + rx);
	JRigidInv(1, 1) = -(oz - z) / (sigmaRef + rx);
}

double Controller::computeDesiredPaddlePosition()
{
	// control vertically
	H = computeVerticalEnergy();
	//Htilde = H - Href;
	Htilde = Href - H;

	computeSigmaRef();
	updateReferencePosition();
	updateReferenceVelocity();

	// Control horizontally
	double rhoBar = rubberLength / 4; //in the rubber frame
	double rhoRef = sigmaRef*cos(psiRef);
	double rhopRef = sigmapRef*cos(psiRef) - sigmaRef*psipRef*sin(psiRef);

	std::lock_guard<std::mutex> guard(m_mutexGains);

	return  -(kappa0 + kappa1*Htilde)*psiRef - ( kappa00*(rhoRef - rhoBar) + kappa01*rhopRef);
}

double Controller::computeDesiredPaddleVelocity()
{
	return  -m_positionGain * (m_currentPaddlePositionRad - m_desiredPaddlePositionRad);
}

void Controller::controlArmThread()
{
	//double gaink0, gaink1, gaink00, gaink01;

	while (true)
	{
		// Ensure it was initialized!
		if (!m_initialized)
		{
			printf("please init() controller!");
			return;
		}

		// printf("I'm in the controller thread.\n");

		// Velocity of Motor from Mbed
		m_motorVelMeasRadS = this->m_serialComm->readMotorRadPerSec();

		//Position of Paddle as set by OptiTrack
		m_currentPaddlePositionRad = m_optiTrackPointer->getPaddlePosition();

		//Position and Velocity of Puck

		Vector2d ballPos = m_optiTrackPointer->getBallPosition();
		x = ballPos(0);
		z = ballPos(1);
		Vector2d ballVel = m_optiTrackPointer->getBallVelocity();
		xp = ballVel(0);
		zp = ballVel(1);

		m_desiredPaddlePositionRad = this->computeDesiredPaddlePosition();

		m_desiredPaddleVelocityRad = this->computeDesiredPaddleVelocity();

		this->m_serialComm->sendMotorRadPerSec((float)m_desiredPaddleVelocityRad);
		m_controlThreadCounter++;
		std::this_thread::sleep_for(std::chrono::milliseconds(LOOP_PERIOD_MS));
		if (m_debug)
		{
			if (m_controlThreadCounter % LOOPCOUNTS_INT == 0)
			{
				printf("sigRef= %3.2f, psiRef= %3.2f, psiDes= %3.2f, psipDes= %3.2f\n",
					sigmaRef, psiRef, m_desiredPaddlePositionRad, m_desiredPaddleVelocityRad);
				//printf("H = %3.2f, \t Href = %3.2f \t Htilde = %3.2f \t \n", H, Href, Htilde);
			}
			/*if (m_controlThreadCounter % LOOPCOUNTS_INT == 150)
			{
				this->getGains(&gaink0, &gaink1, &gaink00, &gaink01);
				printf("k0 = %4.3f, k1 = %4.3f, k00 = %4.3f, k01 = %4.3f\n",
					gaink0, gaink1, gaink00, gaink01);
			}*/
		}
		//double incline = computeIncline();
		//printf("The inline is %3.2f degrees.\n", incline);
	}
}

double sgn(double d) {
	double eps = 1e-16;
	return d<-eps ? -1 : d>eps;
}

//double Controller::computeIncline()
//{
//	Vector2d ballPos = m_optiTrackPointer->getBallPosition();
//	z = ballPos(1);
//
//	while (abs(z) > 0.25) {
//		m_optiTrackPointer->getBallPosition();
//		z = ballPos(1);
//		printf("z-coordinate is = %3.2f\n", z);
//	}
//
//	auto t1 = Clock::now();
//	while (abs(z) < 0.25) {
//		m_optiTrackPointer->getBallPosition();
//		z = ballPos(1);
//	}
//	auto t2 = Clock::now();
//
//	millisec_t clkDuration(std::chrono::duration_cast<millisec_t>(t2 - t1));
//
//	double deltat= clkDuration.count();;
//	double deltaz = 0.5;
//	double incline = ( asin((-2 * deltaz) / g / deltat / deltat) ) * 180 / M_PI;
//
//	return incline;
//}
<commit_msg>Went above 30 degree inclination with this.<commit_after>#include "Controller.h"
#include <iostream>
#include <chrono>

typedef std::chrono::high_resolution_clock Clock;
typedef std::chrono::duration<double, std::milli> millisec_t;

Controller::Controller() :
	m_ballPosOptiTrack(0, 0),
	m_ballVelOptiTrack(0, 0),
	m_initialized(false)
{
	sigmaRef = 0.0;
	psiRef = 0.0;

	JRigidInv << 0, 0,
		0, 0;

	xip(0) = 0.0;
	xip(1) = 0.0;

	qpRef = JRigidInv*xip;

	psipRef = qpRef(0);
	sigmapRef = qpRef(1);

	setReferenceEnergy(0.0);

	//reflection gain of mirror law 0.075, // restoring of the energy href 0.01
	//this->setGains(0.075, 0.1, 0.05, 0.05);

	/* Following set of gains seems to work alright
	this->setGains(0.05, 0.001, 0.1, 0.05) // also k1 = 1e-4
	*/

	// These are for inclination, beta = 15.6 degrees.
	/*double k0 = 0.285;
	double k1 = 0.593;
	double k00 = 0.718;
	double k01 = 0.155;*/
	// For heavier puck

	// Increase inclination to, beta = ~33 degrees
	double k0 = 0.315;
	double k1 = 0.593;
	double k00 = 0.869;
	double k01 = 0.170;

	this->setDefaultGains(k0, k1, k00, k01);
	this->setGains(k0, k1, k00, k01);
}

Controller::~Controller()
{
	delete m_serialComm;
}

int Controller::initialize(OptiTrack* optiTrackPointer)
{
	if (m_initialized)
	{
		printf("Controller already initialized.  Exiting");
	}

	m_optiTrackPointer = optiTrackPointer; //ToDo: check if pointer was initilaized
	try {
		m_serialComm = new SerialCommunicator();
	}
	catch(...)
	{
		// catch any serial errors!
		printf("COM Port not open!");
		return 1;
	}

	// create a motor object

	std::cout << "Controller Init, Thread :: ID = " << std::this_thread::get_id() << std::endl;

	int motorSetPosition = 0;
	//std::cout << "In Main Thread : Before Thread Start motorSetPosition = " << motorSetPosition << std::endl;

	// ToDo: get a handle on that thread
	m_initialized = true;

	// Start the Thread!

	std::thread threadObj(&Controller::controlArmThread, this);
	if (threadObj.joinable())
	{
		//threadObj.join();
		//std::cout << "Joined Thread " << std::endl;
		std::cout << "Detaching Control Arm Thread " << std::endl;
		threadObj.detach();
	}
}

void Controller::getDefaultGains(double* gainReflection, double* gainEnergy, double* gainHoriz, double* gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	*gainReflection = kappa0Default;
	*gainEnergy = kappa1Default;
	*gainHoriz = kappa00Default;
	*gainHorizDer = kappa01Default;
}

void Controller::setDefaultGains(const double& gainReflection, const double& gainEnergy, const double& gainHoriz, const double& gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	kappa0Default = gainReflection;
	kappa1Default = gainEnergy;
	kappa00Default = gainHoriz;
	kappa01Default = gainHorizDer;
}

void Controller::getGains(double* gainReflection, double* gainEnergy, double* gainHoriz, double* gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	*gainReflection = kappa0;
	*gainEnergy = kappa1;
	*gainHoriz = kappa00;
	*gainHorizDer = kappa01;
}

void Controller::setGains(const double& gainReflection, const double& gainEnergy, const double& gainHoriz, const double& gainHorizDer)
{
	std::lock_guard<std::mutex> guard(m_mutexGains);
	kappa0 = gainReflection;
	kappa1 = gainEnergy;
	kappa00 = gainHoriz;
	kappa01 = gainHorizDer;
}

double Controller::getReferenceEnergy()
{
	return Href;
}

void Controller::setReferenceEnergy(double referenceEnergy)
{
	Href = referenceEnergy;
}

double Controller::computeVerticalEnergy()
{
	return 1 / 2 * pow(zp, 2) + g*sin(beta)*z;
}

void Controller::computeSigmaRef()
{
	ballDistanceSquaredFromHinge = pow(ox - x, 2) + pow(oz - z, 2);
	sigmaRef = sqrt(-pow(r + rz, 2) + ballDistanceSquaredFromHinge) - rx;
}

void Controller::updateReferencePosition()
{
	/* Old calculation
	psiRef = atan2(-(oz - z)*sigmaRef + r*(ox - x), (ox - x)*sigmaRef - r*(oz - z)) - M_PI;
	psiRef = remainder(-psiRef, (2 * M_PI));
	*/
	//psiRef = atan2( (r+rz)*(ox - x) + (sigmaRef+rx)*(oz-z), (sigmaRef+rx)*(ox-x) - (r+rz)*(oz-z) );
	psiRef = atan2((r + rz)*(ox - x) + (sigmaRef + rx)*(oz - z), (sigmaRef + rx)*(ox - x) - (r + rz)*(oz - z));
}

void Controller::updateReferenceVelocity()
{
	computeJacobianInverse();
	Vector2d xip(xp, zp);
	Vector2d refVel = JRigidInv*xip;

	psipRef = refVel(0);
	sigmapRef = refVel(1);
}

void Controller::computeJacobianInverse()
{
	/*Old Jinv
	JRigidInv(0, 0) = (r*(x - ox) + (z - oz)*sigmaRef) / ballDistanceSquaredFromHinge / sigmaRef;
	JRigidInv(0, 1) = (r*(z - oz) + (ox - x)*sigmaRef) / ballDistanceSquaredFromHinge / sigmaRef;
	JRigidInv(1, 0) = (ox - x) / sigmaRef;
	JRigidInv(1, 1) = (oz - z) / sigmaRef;
	*/

	JRigidInv(0, 0) = ((r + rz)*(ox - x) + (sigmaRef + rx)*(oz - z)) / ballDistanceSquaredFromHinge / (sigmaRef + rx);
	JRigidInv(0, 1) = ((r + rz)*(oz - z) - (sigmaRef + rx)*(ox - x)) / ballDistanceSquaredFromHinge / (sigmaRef + rx);
	JRigidInv(1, 0) = -(ox - x) / (sigmaRef + rx);
	JRigidInv(1, 1) = -(oz - z) / (sigmaRef + rx);
}

double Controller::computeDesiredPaddlePosition()
{
	// control vertically
	H = computeVerticalEnergy();
	//Htilde = H - Href;
	Htilde = Href - H;

	computeSigmaRef();
	updateReferencePosition();
	updateReferenceVelocity();

	// Control horizontally
	double rhoBar = rubberLength / 4; //in the rubber frame
	double rhoRef = sigmaRef*cos(psiRef);
	double rhopRef = sigmapRef*cos(psiRef) - sigmaRef*psipRef*sin(psiRef);

	std::lock_guard<std::mutex> guard(m_mutexGains);

	return  -(kappa0 + kappa1*Htilde)*psiRef - ( kappa00*(rhoRef - rhoBar) + kappa01*rhopRef);
}

double Controller::computeDesiredPaddleVelocity()
{
	return  -m_positionGain * (m_currentPaddlePositionRad - m_desiredPaddlePositionRad);
}

void Controller::controlArmThread()
{
	//double gaink0, gaink1, gaink00, gaink01;

	while (true)
	{
		// Ensure it was initialized!
		if (!m_initialized)
		{
			printf("please init() controller!");
			return;
		}

		// printf("I'm in the controller thread.\n");

		// Velocity of Motor from Mbed
		m_motorVelMeasRadS = this->m_serialComm->readMotorRadPerSec();

		//Position of Paddle as set by OptiTrack
		m_currentPaddlePositionRad = m_optiTrackPointer->getPaddlePosition();

		//Position and Velocity of Puck

		Vector2d ballPos = m_optiTrackPointer->getBallPosition();
		x = ballPos(0);
		z = ballPos(1);
		Vector2d ballVel = m_optiTrackPointer->getBallVelocity();
		xp = ballVel(0);
		zp = ballVel(1);

		m_desiredPaddlePositionRad = this->computeDesiredPaddlePosition();

		m_desiredPaddleVelocityRad = this->computeDesiredPaddleVelocity();

		this->m_serialComm->sendMotorRadPerSec((float)m_desiredPaddleVelocityRad);
		m_controlThreadCounter++;
		std::this_thread::sleep_for(std::chrono::milliseconds(LOOP_PERIOD_MS));
		if (m_debug)
		{
			if (m_controlThreadCounter % LOOPCOUNTS_INT == 0)
			{
				printf("sigRef= %3.2f, psiRef= %3.2f, psiDes= %3.2f, psipDes= %3.2f\n",
					sigmaRef, psiRef, m_desiredPaddlePositionRad, m_desiredPaddleVelocityRad);
				//printf("H = %3.2f, \t Href = %3.2f \t Htilde = %3.2f \t \n", H, Href, Htilde);
			}
			/*if (m_controlThreadCounter % LOOPCOUNTS_INT == 150)
			{
				this->getGains(&gaink0, &gaink1, &gaink00, &gaink01);
				printf("k0 = %4.3f, k1 = %4.3f, k00 = %4.3f, k01 = %4.3f\n",
					gaink0, gaink1, gaink00, gaink01);
			}*/
		}
		//double incline = computeIncline();
		//printf("The inline is %3.2f degrees.\n", incline);
	}
}

double sgn(double d) {
	double eps = 1e-16;
	return d<-eps ? -1 : d>eps;
}

//double Controller::computeIncline()
//{
//	Vector2d ballPos = m_optiTrackPointer->getBallPosition();
//	z = ballPos(1);
//
//	while (abs(z) > 0.25) {
//		m_optiTrackPointer->getBallPosition();
//		z = ballPos(1);
//		printf("z-coordinate is = %3.2f\n", z);
//	}
//
//	auto t1 = Clock::now();
//	while (abs(z) < 0.25) {
//		m_optiTrackPointer->getBallPosition();
//		z = ballPos(1);
//	}
//	auto t2 = Clock::now();
//
//	millisec_t clkDuration(std::chrono::duration_cast<millisec_t>(t2 - t1));
//
//	double deltat= clkDuration.count();;
//	double deltaz = 0.5;
//	double incline = ( asin((-2 * deltaz) / g / deltat / deltat) ) * 180 / M_PI;
//
//	return incline;
//}
<|endoftext|>


<commit_before>/* -*- Mode: C++; c-basic-offset: 2; tab-width: 2; indent-tabs-mode: nil -*-
 *
 * Quadra, an action puzzle game
 * Copyright (C) 1998-2000  Ludus Design
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <stdio.h>
#include <string.h>

#include "input.h"
#include "cursor.h"
#include "listbox.h"

RCSID("$Id$")

Zone_listbox::Zone_listbox(Inter* in, Bitmap *fond, Font *f, int *pval, int px, int py, int pw, int ph):
	Zone_watch_int(in, pval, px, py, pw, ph) {
	if(fond)
		back = new Bitmap((*fond)[py+1]+px+1, pw-2, ph-2, fond->realwidth);
	else
		back = NULL;
	screen = Video_bitmap::New(px+1, py+1, pw-2, ph-2);
	font2 = f;
	zup = new Zone_listup(this);
	zdown = new Zone_listdown(this);
	for(int i=y+18; i<y+h-18-f->height(); i+=f->height()) {
		list.add(new Zone_listtext(this, i));
	}
	first_item = 0;
	if(val)
		select(*val);
}

Zone_listbox::~Zone_listbox() {
	empty();
	while(list.size()) {
		delete list.last();
		list.removelast();
	}
	delete zdown;
	delete zup;
	if(back)
		delete back;
	delete screen;
}

void Zone_listbox::draw() {
	screen->setmem();
	if(back)
		back->draw(screen, 0, 0);
	video->vb->hline(y, x, w, 210);
	video->vb->hline(y+h-1, x, w, 210);
	video->vb->vline(x, y, h, 210);
	video->vb->vline(x+w-1, y, h, 210);
	//video->vb->hline(y+20, x, w, 210);
	//video->vb->hline(y+h-1-20, x, w, 210);
}

void Zone_listbox::dirt() {
	if(dirty != 2) {
		Zone_watch_int::dirt();
		zup->dirt();
		zdown->dirt();
		for(int i=0; i<list.size(); i++)
			list[i]->dirt();
	}
}

void Zone_listbox::enable() {
	Zone_watch_int::enable();
	zup->enable();
	zdown->enable();
	for(int i=0; i<list.size(); i++)
		list[i]->enable();
}

void Zone_listbox::disable() {
	Zone_watch_int::disable();
	zup->disable();
	zdown->disable();
	for(int i=0; i<list.size(); i++)
		list[i]->disable();
}

void Zone_listbox::init_sort() {
	sort_list.clear();
}

void Zone_listbox::add_sort(Listable *l) {
	sort_list.add(l);
}

void Zone_listbox::end_sort() {
	qsort((void *) &sort_list[0], sort_list.size(), sizeof(sort_list[0]), compare_sort);
	for(int i=0; i<sort_list.size(); i++)
		add_item(sort_list[i]);
	sort_list.clear();
}

int Zone_listbox::compare_sort(const void *arg1, const void *arg2) {
	char *s1 = (*(Listable **) arg1)->list_name;
	char *s2 = (*(Listable **) arg2)->list_name;
  return strcasecmp(s1, s2);
}

void Zone_listbox::add_item(Listable *e) {
	elements.add(e);
	sync_list();
}

void Zone_listbox::replace_item(int i, Listable *e) {
	delete elements[i];
	elements.replace(i, e);
	sync_list();
}

void Zone_listbox::remove_item(Listable *e) {
	int i;
	for(i=0; i<elements.size(); i++)
		if(elements[i]==e)
			break;
	if(i==elements.size())
		return;
	if(get_selected()==e)
		unselect();
	delete elements[i];
	elements.remove(i);
	sync_list();
}

void Zone_listbox::remove_item(int i) {
	delete elements[i];
	elements.remove(i);
	sync_list();
}

Listable *Zone_listbox::get_selected() {
	if(!val || *val == -1)
		return NULL;
	else
		return elements[*val];
}

void Zone_listbox::process() {
	Zone_watch_int::process();
	if(input) {
		if(cursor->x > x && cursor->x < x+w && cursor->y > y && cursor->y < y+h) {
			int z = input->mouse.dz;
			if(z > 0)
				zup->clicked(0);
			if(z < 0)
				zdown->clicked(0);
		}
	}
}

int Zone_listbox::search(Listable *source) {
	for(int i=0; i<elements.size(); i++)
		if(elements[i]->is_equal(source))
			return i;
	return -1;
}

bool Zone_listbox::in_listbox(const Zone *z) {
	for(int i=0; i<list.size(); i++)
		if(list[i] == z)
			return true;
	return false;
}

void Zone_listbox::sync_list() {
	for(int i=0; i<list.size(); i++) {
		Font *f = inter->font;
		list[i]->kb_focusable = false;
		if(i+first_item >= elements.size()) {
			list[i]->set_text("");
		} else {
			if(val)
				list[i]->kb_focusable = true;
			Listable *li = elements[i+first_item];
			list[i]->set_text(li->list_name);
			if(li->font)
				f = li->font;
		}
		list[i]->set_font(f);
	}
	if(val)
		select(*val);
	dirt();
}

void Zone_listbox::empty() {
	while(elements.size()) {
		delete elements.last();
		elements.removelast();
	}
}

void Zone_listbox::clear() {
	empty();
	first_item = 0;
	if(val)
		*val = -1;
	sync_list();
}

void Zone_listbox::unselect() {
	if(!val)
		return;
	if(*val >= first_item && *val < first_item+list.size()) {
		Font *f = inter->font;
		if(elements[*val]->font)
			f = elements[*val]->font;
		list[*val-first_item]->set_font(f);
	}
	*val = -1;
}

void Zone_listbox::select(int q) {
	if(!val)
		return;
	*val = q;
	if(*val >= first_item && *val < first_item+list.size()) {
		list[*val-first_item]->set_font(font2);
	}
}

Zone_listupdown::Zone_listupdown(Zone_listbox *par, const char *s, int py):
	Zone_text_select(par->inter, par->font2, s, par->x, py, par->w) {
	parent = par;
	leaved();
	kb_focusable = true;
}

void Zone_listupdown::waiting() {
	Zone_text_select::waiting();
	if(input->mouse.button[0] & PRESSED) {
		count--;
		if(count < 0) {
			clicked(0);
			count = 5;
		}
	}
}

void Zone_listupdown::leaved() {
	Zone_text_select::leaved();
	count = 40;
}

void Zone_listupdown::dirt() {
	Zone_text_select::dirt();
	parent->dirt();
}

Zone_listup::Zone_listup(Zone_listbox *par):
	Zone_listupdown(par, "3", par->y) {
}

void Zone_listup::clicked(int quel) {
	if(parent->first_item > 0) {
		parent->first_item--;
		parent->sync_list();
		parent->clicked(quel);
	}
}

Zone_listdown::Zone_listdown(Zone_listbox *par):
	Zone_listupdown(par, "4", par->y+par->h-18) {
}

void Zone_listdown::clicked(int quel) {
	if(parent->first_item < parent->elements.size() - parent->list.size()) {
		parent->first_item++;
		parent->sync_list();
		parent->clicked(quel);
	}
}

Listable::Listable(const char *s, Font *f) {
	strcpy(list_name, s);
	font = f;
}

bool Listable::is_equal(Listable *source) {
	return !strcmp(list_name, source->list_name);
}

Zone_listtext::Zone_listtext(Zone_listbox *par, int i):
	Zone_text(par->inter, "", par->x+2, i, par->w-4) {
	parent = par;
	quel = par->list.size();
	high = false;
}

void Zone_listtext::clicked(int quel) {
	//Watch out! Param 'quel' is mouse button; this->quel is...
	//  hmmm... something else... Ask Remz
	parent->unselect();
	if(this->quel < parent->elements.size()) {
		parent->select(this->quel + parent->first_item);
		//inter->clicked = parent; // eww!
		parent->clicked(quel);
	}
}

void Zone_listtext::draw() {
	parent->screen->setmem();
	font->draw(st, parent->screen, text_x-parent->x, y-parent->y);
	if(high) {
		if(!kb_focusable)
			high=false;
		else
			video->vb->box(x, y, w, h, 255);
	}
}

void Zone_listtext::dirt() {
	Zone_text::dirt();
	parent->dirt();
}

void Zone_listtext::entered() {
	Zone_text::entered();
	if(parent->val && kb_focusable) {
        /* kb_focusable also indicates that this zone_listtext
           currently contains something */
		high=true;
		dirt();
	}
}

void Zone_listtext::leaved() {
	Zone_text::leaved();
	if(parent->val && kb_focusable) {
        /* kb_focusable also indicates that this zone_listtext
           currently contains something */
		high=false;
		dirt();
	}
}
<commit_msg>Attempt at a fix for the crash in Demo Central.<commit_after>/* -*- Mode: C++; c-basic-offset: 2; tab-width: 2; indent-tabs-mode: nil -*-
 *
 * Quadra, an action puzzle game
 * Copyright (C) 1998-2000  Ludus Design
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <stdio.h>
#include <string.h>

#include "input.h"
#include "cursor.h"
#include "listbox.h"

RCSID("$Id$")

Zone_listbox::Zone_listbox(Inter* in, Bitmap *fond, Font *f, int *pval, int px, int py, int pw, int ph):
	Zone_watch_int(in, pval, px, py, pw, ph) {
	if(fond)
		back = new Bitmap((*fond)[py+1]+px+1, pw-2, ph-2, fond->realwidth);
	else
		back = NULL;
	screen = Video_bitmap::New(px+1, py+1, pw-2, ph-2);
	font2 = f;
	zup = new Zone_listup(this);
	zdown = new Zone_listdown(this);
	for(int i=y+18; i<y+h-18-f->height(); i+=f->height()) {
		list.add(new Zone_listtext(this, i));
	}
	first_item = 0;
	if(val)
		select(*val);
}

Zone_listbox::~Zone_listbox() {
	empty();
	while(list.size()) {
		delete list.last();
		list.removelast();
	}
	delete zdown;
	delete zup;
	if(back)
		delete back;
	delete screen;
}

void Zone_listbox::draw() {
	screen->setmem();
	if(back)
		back->draw(screen, 0, 0);
	video->vb->hline(y, x, w, 210);
	video->vb->hline(y+h-1, x, w, 210);
	video->vb->vline(x, y, h, 210);
	video->vb->vline(x+w-1, y, h, 210);
	//video->vb->hline(y+20, x, w, 210);
	//video->vb->hline(y+h-1-20, x, w, 210);
}

void Zone_listbox::dirt() {
	if(dirty != 2) {
		Zone_watch_int::dirt();
		zup->dirt();
		zdown->dirt();
		for(int i=0; i<list.size(); i++)
			list[i]->dirt();
	}
}

void Zone_listbox::enable() {
	Zone_watch_int::enable();
	zup->enable();
	zdown->enable();
	for(int i=0; i<list.size(); i++)
		list[i]->enable();
}

void Zone_listbox::disable() {
	Zone_watch_int::disable();
	zup->disable();
	zdown->disable();
	for(int i=0; i<list.size(); i++)
		list[i]->disable();
}

void Zone_listbox::init_sort() {
	sort_list.clear();
}

void Zone_listbox::add_sort(Listable *l) {
	sort_list.add(l);
}

void Zone_listbox::end_sort() {
  if (sort_list.size() == 0)
    return;

  qsort((void *) &sort_list[0], sort_list.size(), sizeof(sort_list[0]),
        compare_sort);

	for (int i = 0; i < sort_list.size(); ++i)
		add_item(sort_list[i]);

	sort_list.clear();
}

int Zone_listbox::compare_sort(const void *arg1, const void *arg2) {
	char *s1 = (*(Listable **) arg1)->list_name;
	char *s2 = (*(Listable **) arg2)->list_name;
  return strcasecmp(s1, s2);
}

void Zone_listbox::add_item(Listable *e) {
	elements.add(e);
	sync_list();
}

void Zone_listbox::replace_item(int i, Listable *e) {
	delete elements[i];
	elements.replace(i, e);
	sync_list();
}

void Zone_listbox::remove_item(Listable *e) {
	int i;
	for(i=0; i<elements.size(); i++)
		if(elements[i]==e)
			break;
	if(i==elements.size())
		return;
	if(get_selected()==e)
		unselect();
	delete elements[i];
	elements.remove(i);
	sync_list();
}

void Zone_listbox::remove_item(int i) {
	delete elements[i];
	elements.remove(i);
	sync_list();
}

Listable *Zone_listbox::get_selected() {
	if(!val || *val == -1)
		return NULL;
	else
		return elements[*val];
}

void Zone_listbox::process() {
	Zone_watch_int::process();
	if(input) {
		if(cursor->x > x && cursor->x < x+w && cursor->y > y && cursor->y < y+h) {
			int z = input->mouse.dz;
			if(z > 0)
				zup->clicked(0);
			if(z < 0)
				zdown->clicked(0);
		}
	}
}

int Zone_listbox::search(Listable *source) {
	for(int i=0; i<elements.size(); i++)
		if(elements[i]->is_equal(source))
			return i;
	return -1;
}

bool Zone_listbox::in_listbox(const Zone *z) {
	for(int i=0; i<list.size(); i++)
		if(list[i] == z)
			return true;
	return false;
}

void Zone_listbox::sync_list() {
	for(int i=0; i<list.size(); i++) {
		Font *f = inter->font;
		list[i]->kb_focusable = false;
		if(i+first_item >= elements.size()) {
			list[i]->set_text("");
		} else {
			if(val)
				list[i]->kb_focusable = true;
			Listable *li = elements[i+first_item];
			list[i]->set_text(li->list_name);
			if(li->font)
				f = li->font;
		}
		list[i]->set_font(f);
	}
	if(val)
		select(*val);
	dirt();
}

void Zone_listbox::empty() {
	while(elements.size()) {
		delete elements.last();
		elements.removelast();
	}
}

void Zone_listbox::clear() {
	empty();
	first_item = 0;
	if(val)
		*val = -1;
	sync_list();
}

void Zone_listbox::unselect() {
	if(!val)
		return;
	if(*val >= first_item && *val < first_item+list.size()) {
		Font *f = inter->font;
		if(elements[*val]->font)
			f = elements[*val]->font;
		list[*val-first_item]->set_font(f);
	}
	*val = -1;
}

void Zone_listbox::select(int q) {
	if(!val)
		return;
	*val = q;
	if(*val >= first_item && *val < first_item+list.size()) {
		list[*val-first_item]->set_font(font2);
	}
}

Zone_listupdown::Zone_listupdown(Zone_listbox *par, const char *s, int py):
	Zone_text_select(par->inter, par->font2, s, par->x, py, par->w) {
	parent = par;
	leaved();
	kb_focusable = true;
}

void Zone_listupdown::waiting() {
	Zone_text_select::waiting();
	if(input->mouse.button[0] & PRESSED) {
		count--;
		if(count < 0) {
			clicked(0);
			count = 5;
		}
	}
}

void Zone_listupdown::leaved() {
	Zone_text_select::leaved();
	count = 40;
}

void Zone_listupdown::dirt() {
	Zone_text_select::dirt();
	parent->dirt();
}

Zone_listup::Zone_listup(Zone_listbox *par):
	Zone_listupdown(par, "3", par->y) {
}

void Zone_listup::clicked(int quel) {
	if(parent->first_item > 0) {
		parent->first_item--;
		parent->sync_list();
		parent->clicked(quel);
	}
}

Zone_listdown::Zone_listdown(Zone_listbox *par):
	Zone_listupdown(par, "4", par->y+par->h-18) {
}

void Zone_listdown::clicked(int quel) {
	if(parent->first_item < parent->elements.size() - parent->list.size()) {
		parent->first_item++;
		parent->sync_list();
		parent->clicked(quel);
	}
}

Listable::Listable(const char *s, Font *f) {
	strcpy(list_name, s);
	font = f;
}

bool Listable::is_equal(Listable *source) {
	return !strcmp(list_name, source->list_name);
}

Zone_listtext::Zone_listtext(Zone_listbox *par, int i):
	Zone_text(par->inter, "", par->x+2, i, par->w-4) {
	parent = par;
	quel = par->list.size();
	high = false;
}

void Zone_listtext::clicked(int quel) {
	//Watch out! Param 'quel' is mouse button; this->quel is...
	//  hmmm... something else... Ask Remz
	parent->unselect();
	if(this->quel < parent->elements.size()) {
		parent->select(this->quel + parent->first_item);
		//inter->clicked = parent; // eww!
		parent->clicked(quel);
	}
}

void Zone_listtext::draw() {
	parent->screen->setmem();
	font->draw(st, parent->screen, text_x-parent->x, y-parent->y);
	if(high) {
		if(!kb_focusable)
			high=false;
		else
			video->vb->box(x, y, w, h, 255);
	}
}

void Zone_listtext::dirt() {
	Zone_text::dirt();
	parent->dirt();
}

void Zone_listtext::entered() {
	Zone_text::entered();
	if(parent->val && kb_focusable) {
        /* kb_focusable also indicates that this zone_listtext
           currently contains something */
		high=true;
		dirt();
	}
}

void Zone_listtext::leaved() {
	Zone_text::leaved();
	if(parent->val && kb_focusable) {
        /* kb_focusable also indicates that this zone_listtext
           currently contains something */
		high=false;
		dirt();
	}
}
<|endoftext|>


<commit_before>#include "CopyOperation.h"
#include <boost/filesystem.hpp>
#include <sstream>

using file_system::operations::CopyOperation;
using file_system::Path;

CopyOperation::CopyOperation()
{
}

CopyOperation::~CopyOperation()
{
}

void CopyOperation::copy(const Path& sourceFile, const Path& destinationFolder)
{
	boost::filesystem::path sourceFilePath(sourceFile.stringValue());
	boost::filesystem::path destinationFilePath(destinationFolder.stringValue());

	destinationFilePath.append(sourceFilePath.filename().c_str());

	boost::filesystem::copy_file(sourceFilePath, destinationFilePath);
}
<commit_msg>make it work o nlinux<commit_after>#include "CopyOperation.h"
#include <boost/filesystem.hpp>
#include <sstream>

using file_system::operations::CopyOperation;
using file_system::Path;

CopyOperation::CopyOperation()
{
}

CopyOperation::~CopyOperation()
{
}

void CopyOperation::copy(const Path& sourceFile, const Path& destinationFolder)
{
	boost::filesystem::path sourceFilePath(sourceFile.stringValue());
	boost::filesystem::path destinationFilePath(destinationFolder.stringValue());

	destinationFilePath /= sourceFilePath.filename();

	boost::filesystem::copy_file(sourceFilePath, destinationFilePath);
}
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2021.  All rights reserved
 */
#include "../../StroikaPreComp.h"

#if qHasFeature_OpenSSL
#include <openssl/evp.h>
#if OPENSSL_VERSION_MAJOR >= 3
#include <openssl/provider.h>
#endif
#endif

#include "../../Debug/Assertions.h"
#include "../../Execution/Exceptions.h"

#include "LibraryContext.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Cryptography;
using namespace Stroika::Foundation::Cryptography::OpenSSL;

#if qHasFeature_OpenSSL && defined(_MSC_VER)
// Use #pragma comment lib instead of explicit entry in the lib entry of the project file
#if OPENSSL_VERSION_NUMBER < 0x1010000fL
#pragma comment(lib, "libeay32.lib")
#pragma comment(lib, "ssleay32.lib")
#else
#pragma comment(lib, "libcrypto.lib")
#pragma comment(lib, "libssl.lib")
#pragma comment(lib, "ws2_32.lib")
#pragma comment(lib, "crypt32.lib")
#endif
#endif

#if qHasFeature_OpenSSL

namespace {
    void AccumulateIntoSetOfCipherNames_ (const ::EVP_CIPHER* ciph, Set<String>* ciphers)
    {
        RequireNotNull (ciphers);
        if (ciph != nullptr) {
#if OPENSSL_VERSION_MAJOR >= 3
            DbgTrace (L"cipher: %p (name: %s), provider: %p", ciph, CipherAlgorithm{ciph}.pName ().c_str (), ::EVP_CIPHER_get0_provider (ciph));
#else
            DbgTrace (L"cipher: %p (name: %s)", ciph, CipherAlgorithm{ciph}.pName ().c_str ());
#endif
#if OPENSSL_VERSION_MAJOR >= 3
            if (auto provider = ::EVP_CIPHER_get0_provider (ciph)) {
                DbgTrace ("providername = %s", ::OSSL_PROVIDER_get0_name (provider));
            }
#endif
#if 0
            int flags = ::EVP_CIPHER_flags (ciph);
            DbgTrace ("flags=%x", flags);
#endif

            ciphers->Add (CipherAlgorithm{ciph}.pName ());
        }
    };
    void AccumulateIntoSetOfDigestNames_ (const ::EVP_MD* digest, Set<String>* digestNames)
    {
        RequireNotNull (digestNames);
        if (digest != nullptr) {
#if OPENSSL_VERSION_MAJOR >= 3
            DbgTrace (L"digest: %p (name: %s), provider: %p", digest, DigestAlgorithm{digest}.pName ().c_str (), ::EVP_MD_get0_provider (digest));
#else
            DbgTrace (L"digest: %p (name: %s)", digest, DigestAlgorithm{digest}.pName ().c_str ());
#endif
#if OPENSSL_VERSION_MAJOR >= 3
            if (auto provider = ::EVP_MD_get0_provider (digest)) {
                DbgTrace ("providername = %s", ::OSSL_PROVIDER_get0_name (provider));
            }
#endif
            digestNames->Add (DigestAlgorithm{digest}.pName ());
        }
    };
}

/*
 ********************************************************************************
 ******************* Cryptography::OpenSSL::LibraryContext **********************
 ********************************************************************************
 */
#if qCompiler_cpp17InlineStaticMemberOfClassDoubleDeleteAtExit_Buggy
LibraryContext LibraryContext::sDefault;
#endif

LibraryContext::LibraryContext ()
    : pAvailableCipherAlgorithms{
          [qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<CipherAlgorithm> {
              const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pAvailableCipherAlgorithms);
              shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};
              Set<String>                                         cipherNames;
#if OPENSSL_VERSION_MAJOR >= 3
              ::EVP_CIPHER_do_all_provided (
                  nullptr,
                  [] (::EVP_CIPHER* ciph, void* arg) { AccumulateIntoSetOfCipherNames_ (ciph, reinterpret_cast<Set<String>*> (arg)); },
                  &cipherNames);
#else
              ::EVP_CIPHER_do_all_sorted (
#if qCompilerAndStdLib_maybe_unused_in_lambda_ignored_Buggy
                  [] (const ::EVP_CIPHER* ciph, const char*, const char*, void* arg) { AccumulateIntoSetOfCipherNames_ (ciph, reinterpret_cast<Set<String>*> (arg)); },
#else
                  [] (const ::EVP_CIPHER* ciph, [[maybe_unused]] const char* from, [[maybe_unused]] const char* to, void* arg) { AccumulateIntoSetOfCipherNames_ (ciph, reinterpret_cast<Set<String>*> (arg)); },
#endif
                  &cipherNames);
#endif
              DbgTrace (L"Found pAvailableCipherAlgorithms-FIRST-PASS (cnt=%d): %s", cipherNames.size (), Characters::ToString (cipherNames).c_str ());

              Set<CipherAlgorithm> results{cipherNames.Select<CipherAlgorithm> ([] (const String& n) -> optional<CipherAlgorithm> { return OpenSSL::CipherAlgorithm::GetByNameQuietly (n); })};
              DbgTrace (L"Found pAvailableCipherAlgorithms (cnt=%d): %s", results.size (), Characters::ToString (results).c_str ());
              return results;
          }}
    , pStandardCipherAlgorithms{[qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<CipherAlgorithm> {
        const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pStandardCipherAlgorithms);
        shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};
        Set<CipherAlgorithm>                                results;

        results += CipherAlgorithms::kAES_128_CBC;
        results += CipherAlgorithms::kAES_128_ECB;
        results += CipherAlgorithms::kAES_128_OFB;
        results += CipherAlgorithms::kAES_128_CFB1;
        results += CipherAlgorithms::kAES_128_CFB8;
        results += CipherAlgorithms::kAES_128_CFB128;
        results += CipherAlgorithms::kAES_192_CBC;
        results += CipherAlgorithms::kAES_192_ECB;
        results += CipherAlgorithms::kAES_192_OFB;
        results += CipherAlgorithms::kAES_192_CFB1;
        results += CipherAlgorithms::kAES_192_CFB8;
        results += CipherAlgorithms::kAES_192_CFB128;
        results += CipherAlgorithms::kAES_256_CBC;
        results += CipherAlgorithms::kAES_256_ECB;
        results += CipherAlgorithms::kAES_256_OFB;
        results += CipherAlgorithms::kAES_256_CFB1;
        results += CipherAlgorithms::kAES_256_CFB8;
        results += CipherAlgorithms::kAES_256_CFB128;

    /*
     * @todo mark these below as deprecated...??? in openssl3?
     */
#if OPENSSL_VERSION_MAJOR < 3
        results += CipherAlgorithms::kBlowfish_CBC;
        results += CipherAlgorithms::kBlowfish_ECB;
        results += CipherAlgorithms::kBlowfish_CFB;
        results += CipherAlgorithms::kBlowfish_OFB;
        results += CipherAlgorithms::kBlowfish;
        results += CipherAlgorithms::kRC2_CBC;
        results += CipherAlgorithms::kRC2_ECB;
        results += CipherAlgorithms::kRC2_CFB;
        results += CipherAlgorithms::kRC2_OFB;
        results += CipherAlgorithms::kRC4;
#endif

        return results;
    }}
    , pAvailableDigestAlgorithms{[qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<DigestAlgorithm> {
        const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pAvailableDigestAlgorithms);
        shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};

        Set<String> digestNames;
#if OPENSSL_VERSION_MAJOR >= 3
        ::EVP_MD_do_all_provided (
            nullptr,
            [] (::EVP_MD* md, void* arg) { AccumulateIntoSetOfDigestNames_ (md, reinterpret_cast<Set<String>*> (arg)); },
            &digestNames);
#else
        ::EVP_MD_do_all_sorted (
#if qCompilerAndStdLib_maybe_unused_in_lambda_ignored_Buggy
            [] (const ::EVP_MD* md, const char*, const char*, void* arg) { AccumulateIntoSetOfDigestNames_ (md, reinterpret_cast<Set<String>*> (arg)); },
#else
            [] (const ::EVP_MD* md, [[maybe_unused]] const char* from, [[maybe_unused]] const char* to, void* arg) { AccumulateIntoSetOfDigestNames_ (md, reinterpret_cast<Set<String>*> (arg)); },
#endif
            &digestNames);
#endif
        DbgTrace (L"Found pAvailableDigestAlgorithms-FIRST-PASS (cnt=%d): %s", digestNames.size (), Characters::ToString (digestNames).c_str ());

        Set<DigestAlgorithm> results{digestNames.Select<DigestAlgorithm> ([] (const String& n) -> optional<DigestAlgorithm> { return OpenSSL::DigestAlgorithm::GetByNameQuietly (n); })};
        DbgTrace (L"Found pAvailableDigestAlgorithms (cnt=%d): %s", results.size (), Characters::ToString (results).c_str ());

        return results;
    }}
    , pStandardDigestAlgorithms{[qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<DigestAlgorithm> {
        const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pStandardDigestAlgorithms);
        shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};
        Set<DigestAlgorithm>                                results;
        results += DigestAlgorithms::kMD5;
        results += DigestAlgorithms::kSHA1;
        results += DigestAlgorithms::kSHA224;
        results += DigestAlgorithms::kSHA256;
        results += DigestAlgorithms::kSHA384;
        results += DigestAlgorithms::kSHA512;
        return results;
    }}
{
    LoadProvider (kDefaultProvider);
}

LibraryContext ::~LibraryContext ()
{
    lock_guard<const AssertExternallySynchronizedLock> critSec{*this};
#if OPENSSL_VERSION_MAJOR >= 3
    for (auto i : fLoadedProviders_) {
        Verify (::OSSL_PROVIDER_unload (i.fValue.first) == 1);
    }
#endif
}

void LibraryContext::LoadProvider ([[maybe_unused]] const String& providerName)
{
    Debug::TraceContextBumper                          ctx{Stroika_Foundation_Debug_OptionalizeTraceArgs (L"OpenSSL::LibraryContext::LoadProvider", L"%s", providerName.c_str ())};
    lock_guard<const AssertExternallySynchronizedLock> critSec{*this};
#if OPENSSL_VERSION_MAJOR >= 3
    if (auto l = fLoadedProviders_.Lookup (providerName)) {
        l->second++;
        fLoadedProviders_.Add (providerName, *l);
    }
    else {
        OSSL_PROVIDER*                                p = ::OSSL_PROVIDER_load (nullptr, providerName.AsNarrowSDKString ().c_str ());
        static const Execution::RuntimeErrorException kErr_{L"No such SSL provider"sv};
        Execution::ThrowIfNull (p, kErr_);
        fLoadedProviders_.Add (providerName, {p, 1});
        DbgTrace (L"real load");
    }
#else
    Require (providerName == kDefaultProvider or providerName == kLegacyProvider);
#endif
}

void LibraryContext ::UnLoadProvider ([[maybe_unused]] const String& providerName)
{
    Debug::TraceContextBumper                          ctx{Stroika_Foundation_Debug_OptionalizeTraceArgs (L"OpenSSL::LibraryContext::UnLoadProvider", L"%s", providerName.c_str ())};
    lock_guard<const AssertExternallySynchronizedLock> critSec{*this};
#if OPENSSL_VERSION_MAJOR >= 3
    Require (fLoadedProviders_.ContainsKey (providerName));
    auto l = fLoadedProviders_.Lookup (providerName);
    Assert (l);
    l->second--;
    if (l->second == 0) {
        fLoadedProviders_.Remove (providerName);
        Verify (::OSSL_PROVIDER_unload (l->first) == 1);
        DbgTrace (L"real unload");
    }
    else {
        fLoadedProviders_.Add (providerName, *l);
    }
#endif
}
#endif
<commit_msg>cleanup debugtrace calls in openssl wrapper code<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2021.  All rights reserved
 */
#include "../../StroikaPreComp.h"

#if qHasFeature_OpenSSL
#include <openssl/evp.h>
#if OPENSSL_VERSION_MAJOR >= 3
#include <openssl/provider.h>
#endif
#endif

#include "../../Debug/Assertions.h"
#include "../../Execution/Exceptions.h"

#include "LibraryContext.h"

using namespace Stroika::Foundation;
using namespace Stroika::Foundation::Cryptography;
using namespace Stroika::Foundation::Cryptography::OpenSSL;

#if qHasFeature_OpenSSL && defined(_MSC_VER)
// Use #pragma comment lib instead of explicit entry in the lib entry of the project file
#if OPENSSL_VERSION_NUMBER < 0x1010000fL
#pragma comment(lib, "libeay32.lib")
#pragma comment(lib, "ssleay32.lib")
#else
#pragma comment(lib, "libcrypto.lib")
#pragma comment(lib, "libssl.lib")
#pragma comment(lib, "ws2_32.lib")
#pragma comment(lib, "crypt32.lib")
#endif
#endif

#if qHasFeature_OpenSSL

namespace {
    void AccumulateIntoSetOfCipherNames_ (const ::EVP_CIPHER* ciph, Set<String>* ciphers)
    {
        RequireNotNull (ciphers);
        if (ciph != nullptr) {
#if OPENSSL_VERSION_MAJOR >= 3
            DbgTrace (L"cipher: %p (name: %s), provider: %p (name %s)", ciph,
                      CipherAlgorithm{ciph}.pName ().c_str (),
                      ::EVP_CIPHER_get0_provider (ciph),
                      (::EVP_CIPHER_get0_provider (ciph) == nullptr ? L"null" : String::FromNarrowSDKString (::OSSL_PROVIDER_get0_name (::EVP_CIPHER_get0_provider (ciph))).c_str ()));
#else
            DbgTrace (L"cipher: %p (name: %s)", ciph, CipherAlgorithm{ciph}.pName ().c_str ());
#endif
#if 0
            int flags = ::EVP_CIPHER_flags (ciph);
            DbgTrace ("flags=%x", flags);
#endif

            ciphers->Add (CipherAlgorithm{ciph}.pName ());
        }
    };
    void AccumulateIntoSetOfDigestNames_ (const ::EVP_MD* digest, Set<String>* digestNames)
    {
        RequireNotNull (digestNames);
        if (digest != nullptr) {
#if OPENSSL_VERSION_MAJOR >= 3
            DbgTrace (L"digest: %p (name: %s), provider: %p (name %s)",
                digest, DigestAlgorithm{digest}.pName ().c_str (),
                ::EVP_MD_get0_provider (digest),
                      (::EVP_MD_get0_provider (digest) == nullptr ? L"null" : String::FromNarrowSDKString (::OSSL_PROVIDER_get0_name (::EVP_MD_get0_provider (digest))).c_str ()));
#else
            DbgTrace (L"digest: %p (name: %s)", digest, DigestAlgorithm{digest}.pName ().c_str ());
#endif
            digestNames->Add (DigestAlgorithm{digest}.pName ());
        }
    };
}

/*
 ********************************************************************************
 ******************* Cryptography::OpenSSL::LibraryContext **********************
 ********************************************************************************
 */
#if qCompiler_cpp17InlineStaticMemberOfClassDoubleDeleteAtExit_Buggy
LibraryContext LibraryContext::sDefault;
#endif

LibraryContext::LibraryContext ()
    : pAvailableCipherAlgorithms{
          [qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<CipherAlgorithm> {
              const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pAvailableCipherAlgorithms);
              shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};
              Set<String>                                         cipherNames;
#if OPENSSL_VERSION_MAJOR >= 3
              ::EVP_CIPHER_do_all_provided (
                  nullptr,
                  [] (::EVP_CIPHER* ciph, void* arg) { AccumulateIntoSetOfCipherNames_ (ciph, reinterpret_cast<Set<String>*> (arg)); },
                  &cipherNames);
#else
              ::EVP_CIPHER_do_all_sorted (
#if qCompilerAndStdLib_maybe_unused_in_lambda_ignored_Buggy
                  [] (const ::EVP_CIPHER* ciph, const char*, const char*, void* arg) { AccumulateIntoSetOfCipherNames_ (ciph, reinterpret_cast<Set<String>*> (arg)); },
#else
                  [] (const ::EVP_CIPHER* ciph, [[maybe_unused]] const char* from, [[maybe_unused]] const char* to, void* arg) { AccumulateIntoSetOfCipherNames_ (ciph, reinterpret_cast<Set<String>*> (arg)); },
#endif
                  &cipherNames);
#endif
              DbgTrace (L"Found pAvailableCipherAlgorithms-FIRST-PASS (cnt=%d): %s", cipherNames.size (), Characters::ToString (cipherNames).c_str ());

              Set<CipherAlgorithm> results{cipherNames.Select<CipherAlgorithm> ([] (const String& n) -> optional<CipherAlgorithm> { return OpenSSL::CipherAlgorithm::GetByNameQuietly (n); })};
              DbgTrace (L"Found pAvailableCipherAlgorithms (cnt=%d): %s", results.size (), Characters::ToString (results).c_str ());
              return results;
          }}
    , pStandardCipherAlgorithms{[qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<CipherAlgorithm> {
        const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pStandardCipherAlgorithms);
        shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};
        Set<CipherAlgorithm>                                results;

        results += CipherAlgorithms::kAES_128_CBC;
        results += CipherAlgorithms::kAES_128_ECB;
        results += CipherAlgorithms::kAES_128_OFB;
        results += CipherAlgorithms::kAES_128_CFB1;
        results += CipherAlgorithms::kAES_128_CFB8;
        results += CipherAlgorithms::kAES_128_CFB128;
        results += CipherAlgorithms::kAES_192_CBC;
        results += CipherAlgorithms::kAES_192_ECB;
        results += CipherAlgorithms::kAES_192_OFB;
        results += CipherAlgorithms::kAES_192_CFB1;
        results += CipherAlgorithms::kAES_192_CFB8;
        results += CipherAlgorithms::kAES_192_CFB128;
        results += CipherAlgorithms::kAES_256_CBC;
        results += CipherAlgorithms::kAES_256_ECB;
        results += CipherAlgorithms::kAES_256_OFB;
        results += CipherAlgorithms::kAES_256_CFB1;
        results += CipherAlgorithms::kAES_256_CFB8;
        results += CipherAlgorithms::kAES_256_CFB128;

    /*
     * @todo mark these below as deprecated...??? in openssl3?
     */
#if OPENSSL_VERSION_MAJOR < 3
        results += CipherAlgorithms::kBlowfish_CBC;
        results += CipherAlgorithms::kBlowfish_ECB;
        results += CipherAlgorithms::kBlowfish_CFB;
        results += CipherAlgorithms::kBlowfish_OFB;
        results += CipherAlgorithms::kBlowfish;
        results += CipherAlgorithms::kRC2_CBC;
        results += CipherAlgorithms::kRC2_ECB;
        results += CipherAlgorithms::kRC2_CFB;
        results += CipherAlgorithms::kRC2_OFB;
        results += CipherAlgorithms::kRC4;
#endif

        return results;
    }}
    , pAvailableDigestAlgorithms{[qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<DigestAlgorithm> {
        const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pAvailableDigestAlgorithms);
        shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};

        Set<String> digestNames;
#if OPENSSL_VERSION_MAJOR >= 3
        ::EVP_MD_do_all_provided (
            nullptr,
            [] (::EVP_MD* md, void* arg) { AccumulateIntoSetOfDigestNames_ (md, reinterpret_cast<Set<String>*> (arg)); },
            &digestNames);
#else
        ::EVP_MD_do_all_sorted (
#if qCompilerAndStdLib_maybe_unused_in_lambda_ignored_Buggy
            [] (const ::EVP_MD* md, const char*, const char*, void* arg) { AccumulateIntoSetOfDigestNames_ (md, reinterpret_cast<Set<String>*> (arg)); },
#else
            [] (const ::EVP_MD* md, [[maybe_unused]] const char* from, [[maybe_unused]] const char* to, void* arg) { AccumulateIntoSetOfDigestNames_ (md, reinterpret_cast<Set<String>*> (arg)); },
#endif
            &digestNames);
#endif
        DbgTrace (L"Found pAvailableDigestAlgorithms-FIRST-PASS (cnt=%d): %s", digestNames.size (), Characters::ToString (digestNames).c_str ());

        Set<DigestAlgorithm> results{digestNames.Select<DigestAlgorithm> ([] (const String& n) -> optional<DigestAlgorithm> { return OpenSSL::DigestAlgorithm::GetByNameQuietly (n); })};
        DbgTrace (L"Found pAvailableDigestAlgorithms (cnt=%d): %s", results.size (), Characters::ToString (results).c_str ());

        return results;
    }}
    , pStandardDigestAlgorithms{[qStroika_Foundation_Common_Property_ExtraCaptureStuff] ([[maybe_unused]] const auto* property) -> Set<DigestAlgorithm> {
        const LibraryContext*                               thisObj = qStroika_Foundation_Common_Property_OuterObjPtr (property, &LibraryContext::pStandardDigestAlgorithms);
        shared_lock<const AssertExternallySynchronizedLock> critSec{*thisObj};
        Set<DigestAlgorithm>                                results;
        results += DigestAlgorithms::kMD5;
        results += DigestAlgorithms::kSHA1;
        results += DigestAlgorithms::kSHA224;
        results += DigestAlgorithms::kSHA256;
        results += DigestAlgorithms::kSHA384;
        results += DigestAlgorithms::kSHA512;
        return results;
    }}
{
    LoadProvider (kDefaultProvider);
}

LibraryContext ::~LibraryContext ()
{
    lock_guard<const AssertExternallySynchronizedLock> critSec{*this};
#if OPENSSL_VERSION_MAJOR >= 3
    for (auto i : fLoadedProviders_) {
        Verify (::OSSL_PROVIDER_unload (i.fValue.first) == 1);
    }
#endif
}

void LibraryContext::LoadProvider ([[maybe_unused]] const String& providerName)
{
    Debug::TraceContextBumper                          ctx{Stroika_Foundation_Debug_OptionalizeTraceArgs (L"OpenSSL::LibraryContext::LoadProvider", L"%s", providerName.c_str ())};
    lock_guard<const AssertExternallySynchronizedLock> critSec{*this};
#if OPENSSL_VERSION_MAJOR >= 3
    if (auto l = fLoadedProviders_.Lookup (providerName)) {
        l->second++;
        fLoadedProviders_.Add (providerName, *l);
    }
    else {
        OSSL_PROVIDER*                                p = ::OSSL_PROVIDER_load (nullptr, providerName.AsNarrowSDKString ().c_str ());
        static const Execution::RuntimeErrorException kErr_{L"No such SSL provider"sv};
        Execution::ThrowIfNull (p, kErr_);
        fLoadedProviders_.Add (providerName, {p, 1});
        DbgTrace (L"real load");
    }
#else
    Require (providerName == kDefaultProvider or providerName == kLegacyProvider);
#endif
}

void LibraryContext ::UnLoadProvider ([[maybe_unused]] const String& providerName)
{
    Debug::TraceContextBumper                          ctx{Stroika_Foundation_Debug_OptionalizeTraceArgs (L"OpenSSL::LibraryContext::UnLoadProvider", L"%s", providerName.c_str ())};
    lock_guard<const AssertExternallySynchronizedLock> critSec{*this};
#if OPENSSL_VERSION_MAJOR >= 3
    Require (fLoadedProviders_.ContainsKey (providerName));
    auto l = fLoadedProviders_.Lookup (providerName);
    Assert (l);
    l->second--;
    if (l->second == 0) {
        fLoadedProviders_.Remove (providerName);
        Verify (::OSSL_PROVIDER_unload (l->first) == 1);
        DbgTrace (L"real unload");
    }
    else {
        fLoadedProviders_.Add (providerName, *l);
    }
#endif
}
#endif
<|endoftext|>