/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "rcbdd_synthesis.hpp"
#include "synthesis_utils_p.hpp"

#include <fstream>

#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>

#include <core/utils/terminal.hpp>
#include <core/utils/timer.hpp>
#include <reversible/functions/add_circuit.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/functions/pattern_to_circuit.hpp>
#include <reversible/io/print_circuit.hpp>
#include <classical/optimization/optimization.hpp>

#define timer timer_class
#include <boost/progress.hpp>
#undef timer

#include <cuddInt.h>

namespace cirkit
{

enum Direction {
  ChangeLeft = 0,
  ChangeRight
};

int Cudd_bddPickOneCubeForRCBDD( DdManager * ddm, DdNode * node, char * repr)
{
  DdNode *N, *T, *E;
  DdNode *one, *bzero;
  char   dir;
  int    i;

  if ( repr == nullptr || node == nullptr ) return 0;

  /* The constant 0 function has no on-set cubes. */
  one = DD_ONE(ddm);
  bzero = Cudd_Not(one);
  if (node == bzero) return 0;

  for ( i = 0; i < ddm->size; i++ ) repr[i] = 2;

  for (;;)
  {
    if ( node == one ) break;

    N = Cudd_Regular(node);

    T = cuddT(N); E = cuddE(N);
    if ( Cudd_IsComplement( node ) )
    {
	    T = Cudd_Not(T); E = Cudd_Not(E);
    }
    if ( T == bzero )
    {
	    repr[N->index] = 0;
	    node = E;
    }
    else if ( E == bzero )
    {
	    repr[N->index] = 1;
	    node = T;
    }
    else
    {
      if ( N->index % 3 == 1 )
      {
        repr[N->index] = repr[N->index - 1];
        node = repr[N->index] ? T : E;
      }
      else
      {
        dir = (char) ((Cudd_Random( ddm ) & 0x2000) >> 13);
        repr[N->index] = dir;
        node = dir ? T : E;
      }
    }
  }
  return 1;
}

int pick_one_cube_for_rcbdd( BDD node, char * repr )
{
  return Cudd_bddPickOneCubeForRCBDD( node.manager(), node.getNode(), repr );
}

struct rcbdd_synthesis_manager
{
  rcbdd_synthesis_manager( const rcbdd& _cf, circuit& _circ )
    : cf( _cf ),
      circ( _circ ),
      insert_position( 0u )
  {
    f = _cf.chi();

    circ.set_lines( _cf.num_vars() );

    std::vector<std::string> inputs( _cf.num_vars(), _cf.constant_value() ? "1" : "0" );
    boost::copy( cf.input_labels(), inputs.end() - _cf.num_inputs() );
    circ.set_inputs( inputs );

    std::vector<std::string> outputs( _cf.num_vars(), "-" );
    boost::copy( cf.output_labels(), outputs.begin() );
    circ.set_outputs( outputs );

    std::vector<constant> constants( _cf.num_vars(), constant() );
    std::fill( constants.begin(), constants.end() - _cf.num_inputs(), _cf.constant_value() );
    circ.set_constants( constants );

    std::vector<bool> garbage( _cf.num_vars(), true );
    std::fill( garbage.begin(), garbage.begin() + _cf.num_outputs(), false );
    circ.set_garbage( garbage );

    node_count += f.nodeCount();
  }

  void set_var(unsigned v)
  {
    _var = v;

    left_f = cf.manager().bddZero();
    right_f = cf.manager().bddZero();
  }

  void compute_cofactors()
  {
    n  = cf.cofactor(f, _var, false, false);
    pp = cf.cofactor(f, _var, true,  false);
    np = cf.cofactor(f, _var, false, true);
    p  = cf.cofactor(f, _var, true,  true);

    nx  = cf.remove_ys(n);
    ppx = cf.remove_ys(pp);
    npx = cf.remove_ys(np);
    px  = cf.remove_ys(p);

    ny  = cf.remove_xs(n);
    ppy = cf.remove_xs(pp);
    npy = cf.remove_xs(np);
    py  = cf.remove_xs(p);
  }

  void apply_gates(const BDD& lf, const BDD& rf)
  {
    left_f ^= lf;
    right_f ^= rf;

    BDD gate_left = cf.create_from_gate(_var, lf);
    BDD gate_right = cf.create_from_gate(_var, cf.move_ys_to_xs(rf));
    f = cf.compose(cf.compose(gate_left, f), gate_right);
    node_count += f.nodeCount();
  }

  void only_left_gate_shortcut()
  {
    BDD chi_prime = f;

    // Copy y_var to x_var
    chi_prime = chi_prime.ExistAbstract(cf.x(_var)) & cf.x(_var).Xnor(cf.y(_var));

    // Check whether chi' is reversible
    if (mpz_class(chi_prime.CountMinterm(2u * cf.num_vars())) == pow2(cf.num_vars())
        && cf.remove_ys(chi_prime) == cf.manager().bddOne()
        && cf.remove_xs(chi_prime) == cf.manager().bddOne())
    {
      BDD lf = cf.remove_ys(!f & chi_prime).ExistAbstract(cf.x(_var));
      apply_gates(lf, cf.manager().bddZero());
    }
  }

  void resolve_one_cycles()
  {
    // Via F_n
    compute_cofactors();

    //BDD lf = (!nx & ppx) ^ (!px & npx);
    BDD lf = ppx & npx;

    apply_gates(lf, cf.manager().bddZero());

    compute_cofactors();

    BDD rf = ppy & npy;

    apply_gates(cf.manager().bddZero(), rf);

    // // Via F_p
    // compute_cofactors();

    // lf = !px & npx;
    // rf = !py & ppy;

    // apply_gates(lf, rf);
  }

  void resolve_two_cycles()
  {
    // Via F_p
    compute_cofactors();

    BDD _f = ppy & p & npx;
    BDD lf = cf.remove_ys(_f);
    BDD rf = cf.remove_xs(_f);

    apply_gates(lf, rf);

    // Via F_n
    compute_cofactors();

    _f = npy & n & ppx;
    lf = cf.remove_ys(_f);
    rf = cf.remove_xs(_f);

    apply_gates(lf, rf);
  }

  void cycle_step()
  {
    compute_cofactors();

    std::vector<BDD> variables;
    boost::push_back(variables, cf.xs());
    boost::push_back(variables, cf.ys());

    // Pick an arbitrary cube from pp
    BDD cube;

    if ( pp != cf.manager().bddZero() )
    {
      cube = pp.PickOneMinterm(variables);
    }
    else
    {
      cube = ( !cf.remove_xs( f ) & !cf.remove_ys( f ) & cf.x( _var) & !cf.y( _var ) ).PickOneMinterm( variables );
      f |= cube;
    }
    char change = ChangeLeft;

    BDD lf = cf.manager().bddZero();
    BDD rf = cf.manager().bddZero();
    BDD cube_part;
    char *scube = new char[3u * cf.num_vars()];

    ++access;

    do
    {
      if (change == ChangeLeft)
      {
        // Update left gate
        cube_part = cf.remove_ys(cube).ExistAbstract(cf.x(_var));
        lf |= cube_part;
        //change = CHANGE_RIGHT;
        cube = !cf.x(_var) & cube_part & f;
      }
      else
      {
        cube_part = cf.remove_xs(cube).ExistAbstract(cf.y(_var));
        rf |= cube_part;
        //change = CHANGE_LEFT;
        cube = cf.y(_var) & cube_part & f;
      }

      /* Cube is not part of the function? */
      if (cube == cf.manager().bddZero())
      {
        BDD unused_outputs = !cf.remove_xs(f);
        BDD unused_inputs = !cf.remove_ys(f);

        BDD icube, ocube;

        if (change == ChangeLeft)
        {
          icube = !cf.x(_var) & cube_part;

          ocube = cf.manager().bddOne();
          if ((unused_outputs & cf.y(_var)) != cf.manager().bddZero())
          {
            (unused_outputs & cf.y(_var)).PickOneCube(scube);
          }
          else
          {
            unused_outputs.PickOneCube(scube);
          }
          for (unsigned i = 0u; i < cf.num_vars(); ++i)
          {
            ocube &= (scube[3u * i + 1u] == 1) ? cf.y(i) : !cf.y(i);
          }
        }
        else
        {
          ocube = cf.y(_var) & cube_part;

          icube = cf.manager().bddOne();
          if ((unused_inputs & !cf.x(_var)) != cf.manager().bddZero())
          {
            (unused_inputs & !cf.x(_var)).PickOneCube(scube);
          }
          else
          {
            unused_inputs.PickOneCube(scube);
          }
          for (unsigned i = 0u; i < cf.num_vars(); ++i)
          {
            icube &= (scube[3u * i] == 1) ? cf.x(i) : !cf.x(i);
          }
        }

        cube = icube & ocube;
        f |= cube;

        compute_cofactors();
      }

      change = ((char)1u - change);
      ++access;
    } while ( ( cube & ( pp | np ) ) == cf.manager().bddZero() );

    /* Cleanup */
    delete[] scube;

    apply_gates(lf, rf);
  }

  void resolve_k_cycles()
  {
    while (cf.cofactor(f, _var, true, false) != cf.manager().bddZero() || cf.cofactor(f, _var, false, true) != cf.manager().bddZero())
    {
      compute_cofactors();

      /* special case #P' is 0 and #N' is not zero, then swap */
      if ( pp == cf.manager().bddZero() && np != cf.manager().bddZero() )
      {
        apply_gates( cf.manager().bddOne(), cf.manager().bddOne() );
      }

      if ( false /*verbose*/ )
      {
        std::cout << "#N: " << cf.cofactor( f, _var, false, false ).CountMinterm( 2u * cf.num_vars() ) << " "
                  << "#P': " << cf.cofactor( f, _var, true, false ).CountMinterm( 2u * cf.num_vars() ) << " "
                  << "#N': " << cf.cofactor( f, _var, false, true ).CountMinterm( 2u * cf.num_vars() ) << " "
                  << "#P: " << cf.cofactor( f, _var, true, true ).CountMinterm( 2u * cf.num_vars() ) << std::endl;
      }

      cycle_step();
    }
  }

  typedef std::tuple<
    bool,                                                   // p1[_var] value
    std::function<BDD(rcbdd_synthesis_manager*)>,           // compute fc
    unsigned,                                               // scube offset
    std::function<BDD(rcbdd_synthesis_manager*, BDD, BDD)>, // update f
    std::function<void(circuit&, const circuit&)>           // update circ
    > resolve_cycles_configuration_t;

  /* DISABLE FOR NOW
  resolve_cycles_configuration_t resolve_x = std::make_tuple(
    true,
    []( rcbdd_synthesis_manager* manager ) {
      BDD fc = manager->ppx & manager->cf.move_xs_to_tmp( manager->npx );
      for ( unsigned j = 0u; j < manager->_var; ++j )
      {
        fc &= ( manager->cf.x( j ).Xnor( manager->cf.z( j ) ) );
      }
      return fc;
    },
    0u,
    []( rcbdd_synthesis_manager* manager, BDD gcirc, BDD f ) { return manager->cf.compose( gcirc, f ); },
    []( circuit& circ, const circuit& c ) { append_circuit( circ, c ); } );

  resolve_cycles_configuration_t resolve_y = std::make_tuple(
    false,
    []( rcbdd_synthesis_manager* manager ) {
      BDD fc = manager->ppy & manager->cf.move_ys_to_tmp( manager->npy );
      for ( unsigned j = 0u; j < manager->_var; ++j )
      {
        fc &= ( manager->cf.y( j ).Xnor( manager->cf.z( j ) ) );
      }
      return fc;
    },
    1u,
    []( rcbdd_synthesis_manager* manager, BDD gcirc, BDD f ) { return manager->cf.compose( f, gcirc ); },
    []( circuit& circ, const circuit& c ) { prepend_circuit( circ, c ); } );
  */

  void resolve_cycles_with_transpositions( const resolve_cycles_configuration_t& configuration )
  {
    if ( false /*verbose*/ )
    {
      std::cout << boost::format( "[i] resolve cycles for var %d" ) % _var << std::endl;
    }

    while ( cf.cofactor( f, _var, true, false ) != cf.manager().bddZero() ||
            cf.cofactor( f, _var, false, true ) != cf.manager().bddZero() )
    {
      compute_cofactors();

      /* Extract two cubes and save them into bitset */
      boost::dynamic_bitset<> p1( cf.num_vars() ), p2( cf.num_vars() );

      p1[_var] =  std::get<0>( configuration );
      p2[_var] = !std::get<0>( configuration );

      char *scube = new char[3u * cf.num_vars()];

      BDD fc = std::get<1>( configuration )( this );

      if ( !smart_pickcube )
      {
        fc.PickOneCube( scube );
      }
      else
      {
        pick_one_cube_for_rcbdd( fc, scube );
      }

      for ( unsigned i = 0u; i < cf.num_vars(); ++i )
      {
        if ( i == _var ) continue;
        p1[i] = scube[3u * i + std::get<2>( configuration )];
        p2[i] = scube[3u * i + 2u];
      }

      delete[] scube;

      /* Create circuit for p1 and p2 */
      if ( verbose )
      {
        //std::cout << "[i] realize transposition (" << p1 << " " << p2 << ")" << std::endl;
      }
      circuit c( cf.num_vars() );
      pattern_to_circuit( c, p1, p2 );
      if ( verbose )
      {
        //std::cout << "[i] result: " << std::endl << c;
      }

      /* Apply circuit to f */
      BDD gcirc = cf.create_from_circuit( c );
      f = std::get<3>( configuration )( this, gcirc, f );
      std::get<4>( configuration )( circ, c );
    }
  }

  void add_toffoli_gate( const cube_t& cube, unsigned offset )
  {
    gate::control_container controls;
    for ( unsigned i = 0u; i < cf.num_vars(); ++i )
    {
      if ( cube.second[3u * i + offset] )
      {
        controls += make_var( i, cube.first[3u * i + offset] );
      }
    }

    insert_toffoli( circ, insert_position, controls, _var );
    insert_position++;
  }

  void create_toffoli_gates_with_exorcism(const BDD& gate, unsigned var, unsigned offset, bool add_gates_to_circuit = true)
  {
    if (gate == cf.manager().bddZero()) return;

    if ( genesop )
    {
      std::ofstream esopout;
      esopout.open( boost::str( boost::format( "/tmp/%s_%d_%d.pla" ) % name % var % offset ) );
      esopout << ".i " << cf.num_vars() << std::endl;
      esopout << ".o " << 1 << std::endl;

      int *cube;
      CUDD_VALUE_TYPE value;
      DdGen *gen;

      Cudd_ForeachCube(gate.manager(), gate.getNode(), gen, cube, value)
      {
        char v;

        for (unsigned i = 0u; i < cf.num_vars(); ++i)
        {
          v = cube[3u * i + offset];
          if (v != 2)
          {
            esopout << ((v == 0) ? "0" : "1");
          }
          else
          {
            esopout << "-";
          }
        }

        esopout << " 1" << std::endl;
      }

      esopout << ".e" << std::endl;
      esopout.close();
    }

    if ( create_gates )
    {
      if ( add_gates_to_circuit )
      {
        esopmin.settings()->set( "on_cube", cube_function_t( [this, &offset]( const cube_t& c ) { add_toffoli_gate( c, offset ); } ) );
      }
      else
      {
        esopmin.settings()->set( "on_cube", cube_function_t( [this, &offset]( const cube_t& c ) {} ) );
      }
      esopmin.settings()->set( "verify", false );
      esopmin( gate.manager(), gate.getNode() );

      if ( add_gates_to_circuit && offset == 1u )
      {
        insert_position -= esopmin.statistics()->get<unsigned>( "cube_count" );
      }

      total_toffoli_gates += esopmin.statistics()->get<unsigned>( "cube_count" );
      total_control_lines += esopmin.statistics()->get<unsigned>( "literal_count" );
    }

    /*
    system("exorcism /tmp/test.esop");

    // Get number of gates
    total_toffoli_gates += boost::lexical_cast<unsigned long long>(execute_and_return_output("cat /tmp/test.esop | grep -v -e \"^[#\\.]\" | wc -l"));
    total_control_lines += boost::lexical_cast<unsigned long long>(execute_and_return_output("cat /tmp/test.esop | grep -v -e \"^[#\\.]\" | awk '{print $1}' | tr '\\n' ' ' | sed -e \"s/[^01]//g\" | wc -c"));

    // Get gates
    using boost::format;
    using boost::str;

    std::ifstream is;
    is.open("/tmp/test.esop");

    std::string line;
    while (std::getline(is, line)) {
      boost::trim(line);
      if (line.empty()) continue;

      if (line[0] == '0' || line[0] == '1' || line[0] == '-') {
        std::string negate, vars;
        unsigned c = 0u;
        for (unsigned i = 0u; i < mgr.num_vars(); ++i) {
          if (line[i] == '0') {
            negate += str(format("t1 x%d\n") % i);
          }
          if (line[i] == '0' || line[i] == '1') {
            vars += str(format(" x%d") % i);
            ++c;
          }
        }
        std::string _gate = str(format("%st%d%s x%d\n%s") % negate % (c + 1u) % vars % _var % negate);
        if (offset == 0u) {
          real_l += _gate;
        } else {
          real_r = _gate + real_r;
        }
      }
    }
    is.close();
    */
  }

  void default_synthesis()
  {
    null_stream ns;
    std::ostream null_out( &ns );
    boost::progress_display show_progress( cf.num_vars(), progress ? std::cout : null_out );

    for (unsigned var = 0; var < cf.num_vars(); ++var)
    {
      ++show_progress;
      set_var(var);

      if ( synthesis_method == ResolveCycles )
      {
        only_left_gate_shortcut();
        resolve_one_cycles();
        resolve_two_cycles();
        resolve_k_cycles();

        if ( false /*verbose*/ )
        {
          std::cout << "Target: " << _var << std::endl << " - left control function:" << std::endl;
          left_f.PrintMinterm();
          std::cout << " - right control function:" << std::endl;
          right_f.PrintMinterm();
        }

        create_toffoli_gates_with_exorcism(left_f, var, 0u);
        create_toffoli_gates_with_exorcism(right_f, var, 1u);
      }
      else if ( synthesis_method == TranspositionsX )
      {
        //resolve_cycles_with_transpositions( resolve_x );
      }
      else if ( synthesis_method == TranspositionsY )
      {
        //resolve_cycles_with_transpositions( resolve_y );
      }
    }
  }

  void heuristic_swap() //Get chi
  {
    std::vector<unsigned> list_lines;
    for (unsigned var = 0; var < cf.num_vars(); ++var)
    {
      list_lines.push_back(var);
    }

    BDD lf_c, lr_c;

    while (!list_lines.empty())
    {
      unsigned min_cost = UINT_MAX;
      unsigned best_line = 0u;

      for (unsigned i = 0u; i < list_lines.size(); ++i)
      {
        BDD oldchi = f; //make a copy of chi
        unsigned old_control_lines = total_control_lines;
        unsigned old_toffoli_gates = total_toffoli_gates;
        if ( false /*verbose*/ )
        {
          std::cout << "[I] - total_toffoli_gates" << total_toffoli_gates << std::endl;
        }
        set_var(list_lines[i]);

        if ( false /*verbose*/ )
        {
          std::cout << "[I] set_var(var): " << _var << std::endl;
        }
        only_left_gate_shortcut();
        resolve_one_cycles();
        resolve_two_cycles();
        resolve_k_cycles();

        if ( false /*verbose*/ )
        {
          std::cout << "Target: " << _var << std::endl << " - left control function:" << std::endl;
          left_f.PrintMinterm();
          std::cout << "[I] - right control function:" << std::endl;
          right_f.PrintMinterm();
        }

        create_toffoli_gates_with_exorcism( left_f, list_lines[i], 0u, false );
        create_toffoli_gates_with_exorcism( right_f, list_lines[i], 1u, false );

        // Determine cost and save in new_cost
        unsigned new_cost = total_toffoli_gates - old_toffoli_gates;

        if ( false /*verbose*/ )
        {
          std::cout << "[I] h1: Lines:    " << cf.num_vars() << std::endl;
          std::cout << "[I] h1: Gates:    " << new_cost << std::endl;
          std::cout << "[I] Controls:     " << total_control_lines << std::endl;
        }

        if (new_cost < min_cost)
        {
          best_line = list_lines[i];
          min_cost = new_cost;
          if ( false /*verbose*/ )
          {
            std::cout << "[I] Min cost: " << min_cost << std::endl;
          }
        }

        f = oldchi;
        total_toffoli_gates = old_toffoli_gates;
        total_control_lines = old_control_lines;
      }

      set_var(best_line);
      // Synthesis with best_line
      only_left_gate_shortcut();
      resolve_one_cycles();
      resolve_two_cycles();
      resolve_k_cycles();

      create_toffoli_gates_with_exorcism(left_f, best_line, 0u);
      create_toffoli_gates_with_exorcism(right_f, best_line, 1u);

      list_lines.erase(std::remove(list_lines.begin(),list_lines.end(),best_line));

      if ( false /*verbose*/ )
      {
        std::cout << "[I] Best Line: " << best_line << std::endl;
      }
    }
  }

  void heuristic_hamming()
  {
    std::vector<unsigned> list_lines;
    for (unsigned var = 0; var < cf.num_vars(); ++var)
    {
      list_lines.push_back(var);
    }

    BDD lf_c, lr_c;

    while (!list_lines.empty())
    {
      double min_cost = DBL_MAX;
      unsigned best_line = 0u;

      for (unsigned i = 0u; i < list_lines.size(); ++i)
      {
        // Determine costs and save in new_cost
        double new_cost = cf.cofactor(f, list_lines[i], false, true).CountMinterm(2 * cf.num_vars());

        if (new_cost < min_cost)
        {
          best_line = list_lines[i];
          min_cost = new_cost;
          if ( false /*verbose*/ )
          {
            std::cout << "[I] Min cost: " << min_cost << std::endl;
          }
        }
      }

      set_var(best_line);
      // Synthesis with best_line
      only_left_gate_shortcut();
      resolve_one_cycles();
      resolve_two_cycles();
      resolve_k_cycles();

      create_toffoli_gates_with_exorcism(left_f, best_line, 0u);
      create_toffoli_gates_with_exorcism(right_f, best_line, 1u);

      list_lines.erase(std::remove(list_lines.begin(),list_lines.end(),best_line));

      if ( false /*verbose*/ )
      {
        std::cout << "[I] Best Line: " << best_line << std::endl;
      }
    }
  }

  const rcbdd& cf;
  circuit& circ;

  bool verbose;
  bool progress;
  std::string name;
  bool genesop;
  dd_based_esop_optimization_func esopmin;
  bool create_gates;
  bool smart_pickcube;
  SynthesisMethod synthesis_method;

  BDD f;
  BDD left_f, right_f;
  unsigned _var;
  unsigned insert_position;
  BDD n, pp, np, p;
  BDD nx, ppx, npx, px;
  BDD ny,  ppy, npy, py;
  unsigned total_control_lines = 0u, total_toffoli_gates = 0u;
  unsigned long long access = 0ull;
  std::vector<int> node_count;
};

bool rcbdd_synthesis( circuit& circ, const rcbdd& cf, properties::ptr settings, properties::ptr statistics )
{
  /* Settings */
  auto verbose          = get( settings, "verbose",          false                             );
  auto progress         = get( settings, "progress",         false                             );
  auto name             = get( settings, "name",             std::string( "test" )             );
  auto genesop          = get( settings, "genesop",          false                             );
  auto esopmin          = get( settings, "esopmin",          dd_based_esop_optimization_func() );
  auto create_gates     = get( settings, "create_gates",     true                              );
  /* 0: default, 1: swap, 2: hamming */
  auto mode             = get( settings, "mode",             0u                                );
  auto synthesis_method = get( settings, "synthesis_method", ResolveCycles                     );
  auto smart_pickcube   = get( settings, "smart_pickcube",   true                              );

  /* Timing */
  properties_timer t( statistics );

  rcbdd_synthesis_manager mgr( cf, circ );
  mgr.verbose          = verbose;
  mgr.progress         = progress;
  mgr.name             = name;
  mgr.genesop          = genesop;
  mgr.esopmin          = esopmin;
  mgr.create_gates     = create_gates;
  mgr.synthesis_method = synthesis_method;
  mgr.smart_pickcube   = smart_pickcube;
  switch ( mode )
  {
  case 1u:
    mgr.heuristic_swap();
    break;
  case 2u:
    mgr.heuristic_hamming();
    break;
  default:
    mgr.default_synthesis();
  };

  if ( statistics )
  {
    statistics->set( "access", mgr.access );
    statistics->set( "node_count", mgr.node_count );
  }

  return true;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/**
 *  Copyright 2014-2016 CyberVision, Inc.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *       http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include <memory>
#include <string>
#include <cstdint>
#include <random>
#include <cmath>

#include <boost/asio.hpp>
#include <boost/bind.hpp>

#include <kaa/Kaa.hpp>
#include <kaa/IKaaClient.hpp>
#include <kaa/configuration/manager/IConfigurationReceiver.hpp>
#include <kaa/configuration/storage/FileConfigurationStorage.hpp>
#include <kaa/log/strategies/RecordCountLogUploadStrategy.hpp>

using namespace kaa;

class TemperatureSensor : public IConfigurationReceiver {
public:
    TemperatureSensor():
        kaaClient_(Kaa::newClient()),
        samplePeriod_(0),
        interval_(samplePeriod_),
        timer_(service_, interval_)
    {
        // Set a custom strategy for uploading logs.
        // The logs will be uploaded to server
        // each time the number of buckets reaches
        // the logUploadThreshold value
        kaaClient_->setLogUploadStrategy(
            std::make_shared<RecordCountLogUploadStrategy>(int(logUploadThreshold), kaaClient_->getKaaClientContext()));

        // Set up a configuration subsystem.
        IConfigurationStoragePtr storage(
            std::make_shared<FileConfigurationStorage>(std::string(savedConfig_)));
        kaaClient_->setConfigurationStorage(storage);
        kaaClient_->addConfigurationListener(*this);
    }

    ~TemperatureSensor()
    {
        // Stop the Kaa endpoint.
        kaaClient_->stop();
        std::lock_guard<std::mutex> guard(iostream_mutex_);
        std::cout << "Data collection demo stopped" << std::endl;
    }

    void run()
    {
        // Run the Kaa endpoint.
        kaaClient_->start();
        if (kaaClient_->getConfiguration().samplePeriod <= 0) {
            std::cout << "Started configuration is wrong, so use sample period = 1" << std::endl;
            samplePeriod_ = 1;
        } else {
            samplePeriod_ = kaaClient_->getConfiguration().samplePeriod;
        }
        timer_.async_wait(boost::bind(&TemperatureSensor::sendTemperature, this));
        timer_.expires_from_now(boost::posix_time::seconds(samplePeriod_));
        service_.run();
    }

private:
    static constexpr auto savedConfig_ = "saved_config.cfg";
    static constexpr int logUploadThreshold = 5;
    std::shared_ptr<IKaaClient> kaaClient_;
    int32_t samplePeriod_;
    boost::asio::io_service service_;
    boost::posix_time::seconds interval_;
    boost::asio::deadline_timer timer_;
    std::mutex iostream_mutex_;

    int32_t getTemperature()
    {
        static std::random_device rand_device;
        static std::mt19937 generator(rand_device());
        static const float T = 90.0;
        static const float PI = 3.141592653;
        std::uniform_real_distribution<float> distribution(0, 1);
        return (0.6f * (distribution(generator) - 0.5f) + std::sin((2*PI/T) * std::time(nullptr))) * 25 + 15;
    }

    void sendTemperature()
    {
        KaaUserLogRecord logRecord;
        logRecord.temperature = getTemperature();
        logRecord.timeStamp = std::time(nullptr);
        // Send value of temperature
        kaaClient_->addLogRecord(logRecord);
        // Show log
        {
            std::lock_guard<std::mutex> guard(iostream_mutex_);
            std::cout << "Sampled temperature: " << logRecord.temperature
                << ", timestamp: " << logRecord.timeStamp << std::endl;
        }
        // Change timer expiry period
        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(samplePeriod_));
        // Posts the timer event
        timer_.async_wait(boost::bind(&TemperatureSensor::sendTemperature, this));
    }

    void onConfigurationUpdated(const KaaRootConfiguration &configuration)
    {
        std::lock_guard<std::mutex> guard(iostream_mutex_);
	if (configuration.samplePeriod > 0) {
            samplePeriod_ = configuration.samplePeriod;
            std::cout << "Received configuration data. New sample period: "
                << configuration.samplePeriod << " seconds" << "\n";
        } else {
            std::cout << "Sample period value in updated configuration is wrong, so ignore it" << std::endl;
        }
    }
};

int main()
{
    try {
        TemperatureSensor sensor;
        sensor.run();
    } catch (std::exception& e) {
        std::cout << "Exception: " << e.what();
    }
    return 0;
}


/*
	ALL THE DISTANCE UNITS HERE ARE IN METERS!
	Speed would be meters/second
	For x axis:
		positive = forwards
		negative = backwards
	For y axis:
		postive = left
		negative = right
*/

#include <vector>
#include "Drone.h"
#include <math.h>
#include <boost/asio/io_service.hpp>
#include <Fullnavdata.h>
#include <gnuplot_iostream.h>
#include <deque>
#include <fstream>
#include <iostream>
#include <sstream>
/*
 * PRIVATE HEADER
 */

#define DRONE_IP                    "192.168.42.1"
#define DRONE_MAX_ALTITUDE          1.0
#define DRONE_MAX_HORIZONTAL_SPEED  0.3
#define DRONE_MAX_VERTICAL_SPEED    0.3
#define LAND_AFTER_LAST_WAYPOINT    true
#define CALIBRATION_FILE            "res/calib_bd2.xml"
#define HULLPROTECTIONON            true
#define LOOK_FOR_CHESSBOARD         false

double INTERSECTION_THRESHOLD = 4.0;
double WALL_THRESHOLD = 1.0;
//arbitrary value... probably something better could be used
double INTERSECTION_INDICATOR[2] = { 100000000000, -100000000000 };

using namespace std;

class DistAndDirec
{
public:
	double distance;
	char direction;
};

/*
	This lets us know where the drone is at
*/
//drone position
vector <double*> dpos;
//drone iterator
vector <double*> :: iterator dpit;
//drone reverse iterator
vector <double*> :: reverse_iterator dprit;
//which way the drone was moving
vector <char> ddirec;
vector <char> :: iterator ddit;
vector <char> :: reverse_iterator ddrit;
bool IsValueInDpos(double *value);

void ReadyToReadScan();
double ** ReadScanFile(string fn);
double * CheckMaxCoord(double x, double y, int check_x_or_y, double current_min[]);
DistAndDirec FindMaxDistance(double mbd, double mfd, double mld, double mrd);
DistAndDirec CheckForIntersections(double mbd, double mfd, double mld, double mrd, char cd);
double GetDistance(double mbd, double mfd, double mld, double mrd, char cd);
char ReverseDirection(char cd);
//void ChangeDistance(char cd, double cv, double *mbd, double *mfd, double *mld, double *mrd);
void ChangeDistance(char cd, double cv, double * dist);

/* TODO: Need to uncomment drone code if trying to use drone;
          currently just trying to make a droneless code   */
//Drone bebop;

/* Signal catches; if a process dies, we need drone to emergency stop */
/*
void kill_handler (int junk)
{
	signal(SIGINT, kill_handler);
	cout << endl << "AHHHHHHHHHHHH, WHAT ARE YOU DOING" << endl;
	bebop.emergency();
	exit(1);
}
*/

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

	cout << argc << endl;
	if( argc != 2 )
	{
		//need to put the file name of where the scans will go;
		// this assumes all scans will go to the same file
		cout << "PUT IN A FILE NAME, FOOL!" << endl;
 		exit(0);
	}

	/* SETUP FOR DONE */
/*	signal(SIGINT, kill_handler);
	bebop.connect();

	while(!bebop.isRunning()){ sleep(1); }

	bebop.setMaxAltitude(DRONE_MAX_ALTITUDE);
   bebop.setMaxHorizontalSpeed(DRONE_MAX_HORIZONTAL_SPEED);
   bebop.setMaxVerticalSpeed(DRONE_MAX_VERTICAL_SPEED);

	if(bebop.blockingFlatTrim())
	{
		std::cout << "FLAT TRIM IS OK" << std::endl;
	}
	else
	{
		std::cerr << "FLAT TRIM NOT OK" << std::endl;
		return 1;
	}
*/	/*END SETUP*/

	/*
	   This is very rudimentary; the drone cannot currently fly with
		everything on it.
	*/

	string file_name = argv[1];

	bool getting_info_loop = true;
	//node info array
	double ** nia = NULL;

	//max backwards distance
	double mbd;
	//max forward distance
	double mfd;
	//max left distance
	double mld;
	//max right distance
	double mrd;

	//n meaning not determined
	char current_direction = 'n';
	char initial_direction = 'n';
	DistAndDirec drone_dd_vals;
	DistAndDirec intersection;
	double current_distance;

	bool currently_reversing = false;
	bool did_reverse = false;

	while(getting_info_loop)
	{
		ReadyToReadScan();

		//nia[0] = node coordinates
		//nia[1] = max backward distance vector
		//nia[2] = max forward distance vector
		//nia[3] = max left distance vector
		//nia[4] = max right distance vector
		nia = ReadScanFile(file_name);

		//helps readability, hopefully..

		double *nia_position = nia[0];
		mbd = nia[1][0];
		mfd = nia[2][0];
		mld = nia[3][1];
		mrd = nia[4][1];

		if( initial_direction == 'n' )
		{
			drone_dd_vals = FindMaxDistance(mbd, mfd, mld, mrd);
			initial_direction = drone_dd_vals.direction;
			current_direction = initial_direction;
			cout << "Go: " << initial_direction << endl;
			continue;
		}

		intersection = CheckForIntersections(mbd, mfd, mld, mrd, current_direction);

		// n indicates no value was set, thus it isn't an intersection
		if( intersection.direction != 'n')
		{
			cout << "INTERSECTION!!!!!" << endl;

			bool prev_intersection = false;

			char prev_direction = current_direction;

			current_direction = intersection.direction;

			//looking to see if we are at a new intersection!
			if ( !IsValueInDpos(nia_position) )
			{
				cout << "????" << endl;
				ddirec.push_back('x');
				//If we reach an intersection, don't want to go down the way we came
				// when we come back to it
				ddirec.push_back(ReverseDirection(prev_direction));
			}
			else //we are at a previous intersection
			{
				prev_intersection = true;
			}

			//This means we need to go back through the other paths we did not take yet
			if( prev_intersection == true )
			{
				cout << "AHKKKKKKKK" << endl;
				//making temp ints for intersection maxes
				double imbd = mbd;
				double imfd = mfd;
				double imld = mld;
				double imrd = mrd;

				double idist[4] = {imbd, imfd, imld, imrd};

				//the idea behind this is that we go through all the previous route we
				// went to at this intersection and make their max 0 (temporarily)
				// when we do this, we know which new direction to go
				for (ddrit = ddirec.rbegin(); ddrit != ddirec.rend() && (*ddrit) != 'x'; ddrit++)
				{
					cout << (*ddrit) << endl;
					ChangeDistance(*ddrit, 0, idist);
				}

				imbd = idist[0];
				imfd = idist[1];
				imld = idist[2];
				imrd = idist[3];

				/*
					Checking l and f here, since it will basically check all directions at the
					 intersection. left first since we are prioritizing: left, forward, right
				*/
				current_direction = 'l';
				intersection = CheckForIntersections(imbd, imfd, imld, imrd, current_direction);
				if(intersection.direction == 'n')
				{
					current_direction = 'f';
					intersection = CheckForIntersections(imbd, imfd, imld, imrd, current_direction);
				}

				current_direction = intersection.direction;

				if (current_direction == 'n')
				{
					cout << "THE DRONE HAS NOWHERE ELSE TO GO! NAVIGATION COMPLETE!" << endl;
					exit(0);
				}

			}

			dpos.push_back(INTERSECTION_INDICATOR);

			//don't want to go back down this way when we travel back to the
			// intersection
			ddirec.push_back(current_direction);
		}
		//need to insert the position after checking the intersection, since
		// the intersection check looks for it inside. Not a huge deal,
		// but will be easier since we don't need it til now anyways...
		// That was a lot of comments for no real good reason.
		dpos.push_back(nia_position);

		current_distance = GetDistance(mbd, mfd, mld, mrd, current_direction);

		if( current_distance <= WALL_THRESHOLD )
		{
			cout << "WALLLLLLL!" << endl;
			current_direction = ReverseDirection(current_direction);
			currently_reversing = true;
			dpos.pop_back();
			//want to remove last position, since we need to reverse back
			//dpos.pop_back();
		}

		if( currently_reversing == true )
		{
			cout << "Begin reversing" << endl;
			for (dprit = dpos.rbegin(); *dprit != INTERSECTION_INDICATOR; ++dprit)
			{
				ReadyToReadScan();
				cout << "Go: " << current_direction << endl;
				//dpos.pop_back();
			}

			cout << "End reversing" << endl;
			currently_reversing = false;
			did_reverse = true;
		}

		/*
		for(dpit = dpos.begin(); dpit != dpos.end(); dpit++)
		{
			cout << *dpit[0] << endl;
		}

		for( ddit = ddirec.begin(); ddit != ddirec.end(); ddit++)
		{
			cout << *ddit << endl;
		}
		*/

		//just don't want to print out what direction to go twice
		if(did_reverse != true)
		{
			cout << "Go: " << current_direction << endl;
		}
		else //we did just reverse
		{
			did_reverse = false;
		}

	}

	exit(0);
}

/*
	This just makes sure the user is ready
*/
void ReadyToReadScan()
{
	char scan_ready;
	cout << "Ready for next scan (y for yes; e for exit)" << endl;
	bool scan_ready_loop = true;
	while(scan_ready_loop)
	{
		cin >> scan_ready;
		switch(scan_ready)
		{
			case 'y':
				scan_ready_loop = false;
				break;

			case 'e':
				exit(0);
				break;

			default:
				cout << "FOOL, DO NOT TEST ME!" << endl;
		}
	}
}

double **  ReadScanFile(string fn)
{
	ifstream fin;
	double x_info;
	double y_info;
	double z_info;

	double *node_info = new double[2];
	double *max_forward_x = NULL;
	double *max_backward_x = NULL;
	double *max_left_y = NULL;
	double *max_right_y = NULL;

	string info_line;

	string junk;
	double x_node_info;
	double y_node_info;

	fin.open(fn);

	//the first line is the "NODE" line, which should be discarded here
	getline(fin, info_line);

	istringstream iss(info_line);

	iss >> junk;
	iss >> x_node_info;
	iss >> y_node_info;
	node_info[0] = x_node_info;
	node_info[1] = y_node_info;

	//getting each x and y pair of data
	while( getline(fin, info_line) )
	{
		//cout << info_line << endl;

		istringstream iss(info_line);

		iss >> x_info;
		iss >> y_info;
		iss >> z_info;

		if( x_info < 0)
		{
			max_backward_x = CheckMaxCoord(x_info, y_info, 1, max_backward_x);
		}
		if( x_info >= 0 )
		{
			max_forward_x = CheckMaxCoord(x_info, y_info, 1, max_forward_x);
		}
		//TODO: THIS MIGHT BE BACKWARDS, need to check!!!!!!!!!!!!!!!!!!!!
		if( y_info >= 0 )
		{
			max_left_y = CheckMaxCoord(x_info, y_info, 0, max_left_y);
		}
		if( y_info < 0 )
		{
			max_right_y = CheckMaxCoord(x_info, y_info, 0, max_right_y);
		}
	}

	/*
	cout << "MBX: " << max_backward_x[0] << " " << max_backward_x[1] << endl;
	cout << "MFX: " << max_forward_x[0] << " " << max_forward_x[1] << endl;
	cout << "MLY: " << max_left_y[0] << " " << max_left_y[1] << endl;
	cout << "MRY: " << max_right_y[0] << " " << max_right_y[1] << endl;
	*/
	fin.close();

	double ** max_arrays = new double*[5];

	max_arrays[0] = node_info;
	max_arrays[1] = max_backward_x;
	max_arrays[2] = max_forward_x;
	max_arrays[3] = max_left_y;
	max_arrays[4] = max_right_y;

	return max_arrays;

}

/*
	check_x_or_y means which index it should be checking in the array:
		0 for x, 1 for y
*/
double * CheckMaxCoord(double x, double y, int check_x_or_y, double current_min[])
{
	double * new_min_array = new double[2];

	//if it is the first loop, need to set the min to current point
	if( current_min == NULL )
	{
		new_min_array[0] = x;
		new_min_array[1] = y;
		return new_min_array;
	}

	double check_min = current_min[check_x_or_y];

	new_min_array[0] = current_min[0];
	new_min_array[1] = current_min[1];

	//checking y
	if( check_x_or_y == 0 )
	{
		if( abs(check_min) > abs(x) )
		{
			new_min_array[0] = x;
			new_min_array[1] = y;
		}
	}
	//checking x
	if( check_x_or_y == 1 )
	{
		if( abs(check_min) > abs(y) )
		{
			new_min_array[0] = x;
			new_min_array[1] = y;
		}
	}
	delete[] current_min;

	return new_min_array;
}

DistAndDirec FindMaxDistance(double mbd, double mfd, double mld, double mrd)
{
	DistAndDirec drone_dd_vals;

	/*
		I am putting mfd first because this would be the direction to move
		if there is a tie between the distances.
	*/
	double distance_array[4] = {mfd, mbd, mld, mrd};

	double cur_max = 0;
	int best_index = -1;

	char direction = 'n';

	for(int i = 0; i < 4; i++)
	{
		if (abs(distance_array[i]) > cur_max)
		{
			cur_max = abs(distance_array[i]);
			best_index = i;
		}
	}

	switch(best_index)
	{
		case 0:
			direction = 'f';
			break;
		case 1:
			direction = 'b';
			break;
		case 2:
			direction = 'l';
			break;
		case 3:
			direction = 'r';
			break;
		default:
			cout << "This should be an intersection." << endl;
			break;

	}

	drone_dd_vals.distance = cur_max;
	drone_dd_vals.direction = direction;

	return drone_dd_vals;
}

/*
	cd = current direction
*/
DistAndDirec CheckForIntersections(double mbd, double mfd, double mld, double mrd, char cd)
{
	DistAndDirec drone_dd_vals;
	DistAndDirec intersection_vals;

	intersection_vals.distance = 0;
	intersection_vals.direction = 'n';

	//We check for f and b here, because we don't want to go in the opposite direction
	// and loop between the two largest values.
	if( cd == 'f' || cd == 'b' )
	{
		//0, 0 is because we only want to look for mld and mrd
		drone_dd_vals = FindMaxDistance(0, 0, mld, mrd);
		if (drone_dd_vals.distance >= INTERSECTION_THRESHOLD)
		{
			intersection_vals.distance = drone_dd_vals.distance;
			intersection_vals.direction = drone_dd_vals.direction;
		}
	}

	if( cd == 'l' || cd == 'r' )
	{
		//0, 0 is because we only want to look for mld and mrd
		drone_dd_vals = FindMaxDistance(mbd, mfd, 0, 0);
		if (drone_dd_vals.distance >= INTERSECTION_THRESHOLD)
		{
			intersection_vals.distance = drone_dd_vals.distance;
			intersection_vals.direction = drone_dd_vals.direction;
		}
	}

	return intersection_vals;
}

double GetDistance(double mbd, double mfd, double mld, double mrd, char cd)
{
	double current_distance;

	switch(cd)
	{
		case 'b':
			current_distance = abs(mbd);
			break;
		case 'f':
			current_distance = abs(mfd);
			break;
		case 'l':
			current_distance = abs(mld);
			break;
		case 'r':
			current_distance = abs(mrd);
			break;
		default:
			cout << "how" << endl;
			break;
	}

	return current_distance;
}

/*
	cd = current_direction
*/
char ReverseDirection(char cd)
{
	//new direction
	char nd;

	switch(cd)
	{
		case 'f':
			nd = 'b';
			break;
		case 'b':
			nd = 'f';
			break;
		case 'l':
			nd = 'r';
			break;
		case 'r':
			nd = 'l';
			break;
		default:
			cout << "STOP MESSING THINGS UP!" << endl;
			break;
	}

	return nd;
}

/*
	cd = current_direction
	cv = changed value
*/
//void ChangeDistance(char cd, double cv, double *mbd, double *mfd, double *mld, double *mrd)
void ChangeDistance(char cd, double cv, double * dist)
{
	//new value
	double nv;

	switch(cd)
	{
		case 'f':
			dist[1] = cv;
			break;
		case 'b':
			dist[0] = cv;
			break;
		case 'l':
			dist[2] = cv;
			break;
		case 'r':
			dist[3] = cv;
			break;
		default:
			cout << "STOP MESSING THINGS UP!" << endl;
			break;
	}

	//cout << cv << (*mfd) << (*mbd) << (*mld) << (*mrd) << endl;
}

bool IsValueInDpos(double *value)
{
	for( dpit = dpos.begin(); dpit != dpos.end(); dpit++)
	{
		cout << (*dpit)[0] << " " << (*dpit)[1] << " " << value[0] << " " << value[1] << endl;
		if( (*dpit)[0] == value[0] && (*dpit)[1] == value[1] )
		{
			return true;
		}
	}
	return false;
}


/*
 * Copyright (c) 2008-2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * 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.
 *
 * 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.
 *
 * $Id: sac_model_sphere.cpp 21050 2009-08-07 21:24:30Z jfaustwg $
 *
 */

/** \author Radu Bogdan Rusu */

#include <point_cloud_mapping/sample_consensus/sac_model_sphere.h>
#include <point_cloud_mapping/geometry/nearest.h>
#include <Eigen/LU>

#include <cminpack.h>

namespace sample_consensus
{
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Get 4 random points (3 non-collinear) as data samples and return them as point indices.
    * \param iterations the internal number of iterations used by SAC methods
    * \param samples the resultant model samples
    * \note assumes unique points!
    * \note Two different points could be enough in theory, to infere some sort of a center and a radius,
    *       but in practice, we might end up with a lot of points which are just 'close' to one another.
    *       Therefore we have two options:
    *       a) use normal information (good but I wouldn't rely on it in extremely noisy point clouds, no matter what)
    *       b) get two more points and uniquely identify a sphere in space (3 unique points define a circle)
    */
  void
    SACModelSphere::getSamples (int &iterations, std::vector<int> &samples)
  {
    samples.resize (4);
    double trand = indices_.size () / (RAND_MAX + 1.0);

    // Get a random number between 1 and max_indices
    int idx = (int)(rand () * trand);
    // Get the index
    samples[0] = indices_.at (idx);

    // Get a second point which is different than the first
    do
    {
      idx = (int)(rand () * trand);
      samples[1] = indices_.at (idx);
      iterations++;
    } while (samples[1] == samples[0]);
    iterations--;

    double Dx1, Dy1, Dz1, Dx2, Dy2, Dz2, Dy1Dy2;
    // Compute the segment values (in 3d) between XY
    Dx1 = cloud_->points[samples[1]].x - cloud_->points[samples[0]].x;
    Dy1 = cloud_->points[samples[1]].y - cloud_->points[samples[0]].y;
    Dz1 = cloud_->points[samples[1]].z - cloud_->points[samples[0]].z;

    int iter = 0;
    do
    {
      // Get the third point, different from the first two
      do
      {
        idx = (int)(rand () * trand);
        samples[2] = indices_.at (idx);
        iterations++;
      } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) );
      iterations--;

      // Compute the segment values (in 3d) between XZ
      Dx2 = cloud_->points[samples[2]].x - cloud_->points[samples[0]].x;
      Dy2 = cloud_->points[samples[2]].y - cloud_->points[samples[0]].y;
      Dz2 = cloud_->points[samples[2]].z - cloud_->points[samples[0]].z;

      Dy1Dy2 = Dy1 / Dy2;
      iter++;

      if (iter > MAX_ITERATIONS_COLLINEAR )
      {
        ROS_WARN ("[SACModelSphere::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR);
        break;
      }
      iterations++;
    }
    // Use Zoli's method for collinearity check
    while (((Dx1 / Dx2) == Dy1Dy2) && (Dy1Dy2 == (Dz1 / Dz2)));
    iterations--;

    // Need to improve this: we need 4 points, 3 non-collinear always, and the 4th should not be in the same plane as the other 3
    // otherwise we can encounter degenerate cases
    do
    {
      samples[3] = (int)(rand () * trand);
      iterations++;
    } while ( (samples[3] == samples[2]) || (samples[3] == samples[1]) || (samples[3] == samples[0]) );
    iterations--;

    return;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Select all the points which respect the given model coefficients as inliers.
    * \param model_coefficients the coefficients of a sphere model that we need to compute distances to
    * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
    * \param inliers the resultant model inliers
    * \note: To get the refined inliers of a model, use:
    * ANNpoint refined_coeff = refitModel (...); selectWithinDistance (refined_coeff, threshold);
    */
  void
    SACModelSphere::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
  {
    int nr_p = 0;
    inliers.resize (indices_.size ());

    // Iterate through the 3d points and calculate the distances from them to the sphere
    for (unsigned int i = 0; i < indices_.size (); i++)
    {
      // Calculate the distance from the point to the sphere as the difference between
      //dist(point,sphere_origin) and sphere_radius
      if (fabs (sqrt (
                      ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) *
                      ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) +

                      ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) *
                      ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) +

                      ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) ) *
                      ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) )
                     ) - model_coefficients.at (3)) < threshold)
      {
        // Returns the indices of the points whose distances are smaller than the threshold
        inliers[nr_p] = indices_[i];
        nr_p++;
      }
    }
    inliers.resize (nr_p);
    return;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Compute all distances from the cloud data to a given sphere model.
    * \param model_coefficients the coefficients of a sphere model that we need to compute distances to
    * \param distances the resultant estimated distances
    */
  void
    SACModelSphere::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
  {
      distances.resize (indices_.size ());

    // Iterate through the 3d points and calculate the distances from them to the sphere
    for (unsigned int i = 0; i < indices_.size (); i++)
      // Calculate the distance from the point to the sphere as the difference between
      //dist(point,sphere_origin) and sphere_radius
      distances[i] = fabs (sqrt (
                                 ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) *
                                 ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) +

                                 ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) *
                                 ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) +

                                 ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) ) *
                                 ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) )
                                ) - model_coefficients.at (3));
    return;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Create a new point cloud with inliers projected onto the sphere model.
    * \param inliers the data inliers that we want to project on the sphere model
    * \param model_coefficients the coefficients of a sphere model
    * \param projected_points the resultant projected points
    * \todo implement this.
    */
  void
    SACModelSphere::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
                                   sensor_msgs::PointCloud &projected_points)
  {
    std::cerr << "[SACModelSphere::projecPoints] Not implemented yet." << std::endl;
    projected_points = *cloud_;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Project inliers (in place) onto the given sphere model.
    * \param inliers the data inliers that we want to project on the sphere model
    * \param model_coefficients the coefficients of a sphere model
    * \todo implement this.
    */
  void
    SACModelSphere::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
  {
    std::cerr << "[SACModelSphere::projecPointsInPlace] Not implemented yet." << std::endl;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Check whether the given index samples can form a valid sphere model, compute the model coefficients from
    * these samples and store them internally in model_coefficients_. The sphere coefficients are: x, y, z, R.
    * \param samples the point indices found as possible good candidates for creating a valid model
    */
  bool
    SACModelSphere::computeModelCoefficients (const std::vector<int> &samples)
  {
    model_coefficients_.resize (4);

    Eigen::Matrix4d temp;
    for (int i = 0; i < 4; i++)
    {
      temp (i, 0) = cloud_->points.at (samples.at (i)).x;
      temp (i, 1) = cloud_->points.at (samples.at (i)).y;
      temp (i, 2) = cloud_->points.at (samples.at (i)).z;
      temp (i, 3) = 1;
    }
    double m11 = temp.determinant ();
    if (m11 == 0)
      return (false);             // the points don't define a sphere!

    for (int i = 0; i < 4; i++)
      temp (i, 0) = (cloud_->points.at (samples.at (i)).x) * (cloud_->points.at (samples.at (i)).x) +
                    (cloud_->points.at (samples.at (i)).y) * (cloud_->points.at (samples.at (i)).y) +
                    (cloud_->points.at (samples.at (i)).z) * (cloud_->points.at (samples.at (i)).z);
    double m12 = temp.determinant ();

    for (int i = 0; i < 4; i++)
    {
      temp (i, 1) = temp (i, 0);
      temp (i, 0) = cloud_->points.at (samples.at (i)).x;
    }
    double m13 = temp.determinant ();

    for (int i = 0; i < 4; i++)
    {
      temp (i, 2) = temp (i, 1);
      temp (i, 1) = cloud_->points.at (samples.at (i)).y;
    }
    double m14 = temp.determinant ();

    for (int i = 0; i < 4; i++)
    {
      temp (i, 0) = temp (i, 2);
      temp (i, 1) = cloud_->points.at (samples.at (i)).x;
      temp (i, 2) = cloud_->points.at (samples.at (i)).y;
      temp (i, 3) = cloud_->points.at (samples.at (i)).z;
    }
    double m15 = temp.determinant ();

    // Center (x , y, z)
    model_coefficients_[0] = 0.5 * m12 / m11;
    model_coefficients_[1] = 0.5 * m13 / m11;
    model_coefficients_[2] = 0.5 * m14 / m11;
    // Radius
    model_coefficients_[3] = sqrt (
                                   model_coefficients_[0] * model_coefficients_[0] +
                                   model_coefficients_[1] * model_coefficients_[1] +
                                   model_coefficients_[2] * model_coefficients_[2] - m15 / m11);

    return (true);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user.
    * @note: these are the coefficients of the sphere model after refinement (eg. after SVD)
    * \param inliers the data inliers found as supporting the model
    * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
    */
  void
    SACModelSphere::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
  {
    if (inliers.size () == 0)
    {
      ROS_ERROR ("[SACModelSphere::RefitModel] Cannot re-fit 0 inliers!");
      refit_coefficients = model_coefficients_;
      return;
    }
    if (model_coefficients_.size () == 0)
    {
      ROS_WARN ("[SACModelSphere::RefitModel] Initial model coefficients have not been estimated yet - proceeding without an initial solution!");
      best_inliers_ = indices_;
    }

    tmp_inliers_ = &inliers;

    int m = inliers.size ();

    double *fvec = new double[m];

    int n = 4;      // 4 unknowns
    int iwa[n];

    int lwa = m * n + 5 * n + m;
    double *wa = new double[lwa];

    // Set the initial solution
    double x[4] = {0.0, 0.0, 0.0, 0.0};
    if ((int)model_coefficients_.size () == n)
      for (int d = 0; d < n; d++)
        x[d] = model_coefficients_.at (d);

    // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
    double tol = sqrt (dpmpar (1));

    // Optimize using forward-difference approximation LM
    int info = lmdif1 (&sample_consensus::SACModelSphere::functionToOptimize, this, m, n, x, fvec, tol, iwa, wa, lwa);

    // Compute the L2 norm of the residuals
    ROS_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g \nFinal solution: %g %g %g %g",
               info, enorm (m, fvec), model_coefficients_.at (0), model_coefficients_.at (1), model_coefficients_.at (2), model_coefficients_.at (3),
               x[0], x[1], x[2], x[3]);

    refit_coefficients.resize (n);
    for (int d = 0; d < n; d++)
      refit_coefficients[d] = x[d];

    free (wa); free (fvec);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////(
  /** \brief Cost function to be minimized
    * \param p a pointer to our data structure array
    * \param m the number of functions
    * \param n the number of variables
    * \param x a pointer to the variables array
    * \param fvec a pointer to the resultant functions evaluations
    * \param iflag set to -1 inside the function to terminate execution
    */
  int
    SACModelSphere::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
  {
    SACModelSphere *model = (SACModelSphere*)p;

    for (int i = 0; i < m; i ++)
    {
      // Compute the difference between the center of the sphere and the datapoint X_i
      double xt = model->cloud_->points[model->tmp_inliers_->at (i)].x - x[0];
      double yt = model->cloud_->points[model->tmp_inliers_->at (i)].y - x[1];
      double zt = model->cloud_->points[model->tmp_inliers_->at (i)].z - x[2];

      // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R
      fvec[i] = sqrt (xt * xt + yt * yt + zt * zt) - x[3];
    }
    return (0);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Verify whether a subset of indices verifies the internal sphere model coefficients.
    * \param indices the data indices that need to be tested against the sphere model
    * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
    */
  bool
    SACModelSphere::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
  {
    for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
      // Calculate the distance from the point to the sphere as the difference between
      //dist(point,sphere_origin) and sphere_radius
      if (fabs (sqrt (
                      ( cloud_->points.at (*it).x - model_coefficients_.at (0) ) *
                      ( cloud_->points.at (*it).x - model_coefficients_.at (0) ) +

                      ( cloud_->points.at (*it).y - model_coefficients_.at (1) ) *
                      ( cloud_->points.at (*it).y - model_coefficients_.at (1) ) +

                      ( cloud_->points.at (*it).z - model_coefficients_.at (2) ) *
                      ( cloud_->points.at (*it).z - model_coefficients_.at (2) )
                     ) - model_coefficients_.at (3)) > threshold)
        return (false);

    return (true);
  }
}


#ifndef _maps_InteractiveBox_hpp_
#define _maps_InteractiveBox_hpp_

#include <vector>
#include <Eigen/Geometry>

namespace maps {

class InteractiveBox {
protected:
  enum TransformMode {
    TransformModeTranslation,
    TransformModeScale,
    TransformModeRotation,
  };

public:
  InteractiveBox();
  ~InteractiveBox();

  void drawBox();

  bool mousePress(const Eigen::Vector2f& iClickPoint, const int iButton,
                  const Eigen::Vector3f& iOrigin,
                  const Eigen::Vector3f& iDirection);
  bool mouseMotion(const Eigen::Vector2f& iCurPoint, const int iButtonMask,
                   const Eigen::Vector3f& iOrigin,
                   const Eigen::Vector3f& iDirection);
  bool mouseRelease(const Eigen::Vector2f& iCurPoint, const int iButton,
                    const Eigen::Vector3f& iOrigin,
                    const Eigen::Vector3f& iDirection);

  void setColor(const float iR, const float iG, const float iB);
  void createBox(const Eigen::Vector3f& iPosition, const float iSize);

  Eigen::Affine3f getBoxToWorldTransform() const;
  void setBoxParameters(const Eigen::Vector3f& iPosition,
                        const Eigen::Vector3f& iSize,
                        const Eigen::Quaternionf& iOrientation);
  bool getBoxParameters(Eigen::Vector3f& oPosition, Eigen::Vector3f& oSize,
                        Eigen::Quaternionf& oOrientation) const;
  bool getBoxValid() const;
  void setBoxValid(const bool iVal);

protected:
  int whichFaceHit(const Eigen::Vector3f& iOrigin,
                   const Eigen::Vector3f& iDirection,
                   Eigen::Vector3f& oIntersection);
  void composeBoxToWorld();

protected:
  Eigen::Vector3f mBoxPosition;
  Eigen::Vector3f mBoxSize;
  Eigen::Quaternionf mBoxOrientation;
  Eigen::Affine3f mBoxToWorld;
  std::vector<Eigen::Vector3f> mBoxPoints;
  std::vector<Eigen::Vector4f> mBoxPlanes;
  std::vector<Eigen::Vector4i> mBoxQuads;
  std::vector<Eigen::Vector2i> mBoxEdges;

  bool mBoxValid;
  bool mDragging;
  int mWhichButton;
  Eigen::Vector2f mDragPoint1;
  Eigen::Vector2f mDragPoint2;
  TransformMode mTransformMode;
  int mCurrentFace;
  Eigen::Vector3f mBoxPositionBase;
  Eigen::Vector3f mBoxSizeBase;
  Eigen::Quaternionf mBoxOrientationBase;
  float mBoxDepth;

  Eigen::Vector3f mBoxColor;
};

}

#endif


#include <gtest/gtest.h>

#include <vector>

#include <Eigen/Dense>

#include "Option.h"
#include "System.h"
#include "PointCharge.h"
#include "FieldLocations.h"
#include "Frame.h"

TEST(System, SimpleField) {
  cpet::Option option;

  option.addFieldLocations(cpet::FieldLocations::fromSimple({"1:1:1"}));

  std::vector<cpet::PointCharge> pc;
  pc.emplace_back(Eigen::Vector3d{0, 0, 0}, 1, cpet::AtomID{"A:1:NH"});
  cpet::Frame frame{pc};
  {
    cpet::System sys{frame, option};
    EXPECT_FLOAT_EQ(sys.center().norm(), 0.0);

    Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();

    EXPECT_EQ(sys.basisMatrix(), identity);

    Eigen::Vector3d field = sys.electricFieldAt(
        *option.calculateFieldLocations()[0].locations()[0].position());

    Eigen::Vector3d expected_result{2.77121, 2.77121, 2.77121};

    EXPECT_NEAR((field - expected_result).norm(), 0, 0.00001);
  }

  option.centerID("1:1:1");

  {
    cpet::System sys{frame, option};
    sys.transformToUserSpace();
    Eigen::Vector3d expected_center{1, 1, 1};
    EXPECT_NEAR((sys.center() - expected_center).norm(), 0, 0.00001);

    Eigen::Matrix3d identity = Eigen::Matrix3d::Identity();
    EXPECT_EQ(sys.basisMatrix(), identity);

    Eigen::Vector3d loc = sys.transformToUserSpace(
        *option.calculateFieldLocations()[0].locations()[0].position());

    Eigen::Vector3d field = sys.electricFieldAt(loc);
    Eigen::Vector3d expected_result{2.77121, 2.77121, 2.77121};
    EXPECT_NEAR((field - expected_result).norm(), 0, 0.00001);
  }
}


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "xregRayCastOccContourOCL.h"

#include <boost/compute/utility/source.hpp>
#include <boost/compute/types/struct.hpp>

#include "xregAssert.h"
#include "xregExceptionUtils.h"

//////////////////////////////////////////////////////////////////////

struct ContourSurCollArgs
{
  float sur_coll_thresh;
  float occluding_ang_thresh_rad;
};

BOOST_COMPUTE_ADAPT_STRUCT(ContourSurCollArgs, ContourSurCollArgs,
                           (sur_coll_thresh, occluding_ang_thresh_rad))

//////////////////////////////////////////////////////////////////////

namespace  // un-named
{

const char* kRAY_CASTING_OCCLUDING_CONTOUR_OPENCL_SRC = BOOST_COMPUTE_STRINGIZE_SOURCE(

__kernel void xregOccludingContourKernel(const RayCastArgs args,
                                         const ContourSurCollArgs sur_coll_args,
                                         __global const float4* det_pts,
                                         image3d_t vol_tex,
                                         __global const float16* cam_to_itk_phys_xforms,
                                         __global float* dst_contours_buf,
                                         __global const ulong* cam_model_for_proj,
                                         __global const float4* cam_focal_pts)
{
  const ulong idx = get_global_id(0);

  const ulong num_rays = args.num_projs * args.num_det_pts;

  if (idx < num_rays)
  {
    const ulong proj_idx   = idx / args.num_det_pts;
    const ulong det_pt_idx = idx - (proj_idx * args.num_det_pts);
    const ulong cam_idx    = cam_model_for_proj[proj_idx];

    const float4 focal_pt_wrt_cam = cam_focal_pts[cam_idx];

    const sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_LINEAR;

    const float4 tex_coords_off = (float4) (0.5f, 0.5f, 0.5f, 0);

    const float16 xform_cam_to_itk_idx = xregFrm4x4Composition(args.itk_phys_pt_to_itk_idx_xform, cam_to_itk_phys_xforms[proj_idx]);

    const float4 pinhole_wrt_itk_idx = xregFrm4x4XformFloat4Pt(xform_cam_to_itk_idx, focal_pt_wrt_cam);

    const float4 cur_det_pt_wrt_cam = det_pts[(cam_idx * args.num_det_pts) + det_pt_idx];

    const float4 pinhole_to_det_wrt_itk_idx = xregFrm4x4XformFloat4Pt(xform_cam_to_itk_idx, cur_det_pt_wrt_cam) - pinhole_wrt_itk_idx;

    // check intersection with the image/volume boundary
    // these pointer were passed as float4's since boost::compute does not have a float3
    float2 t = xregRayRectIntersect(xregFloat4HmgToFloat3(args.img_aabb_min),
                                    xregFloat4HmgToFloat3(args.img_aabb_max),
                                    xregFloat4HmgToFloat3(pinhole_wrt_itk_idx),
                                    xregFloat4HmgToFloat3(pinhole_to_det_wrt_itk_idx));
    // t.x indicates the first intersection with the volume along the source to detector ray,
    // t.y indicates the exit of the volume along the source to detector ray
    // t.x, t.y are in [0,inf)

    const float4 start_pt_wrt_itk_idx = pinhole_wrt_itk_idx + (t.x * pinhole_to_det_wrt_itk_idx);

    const float pinhole_to_det_len_wrt_itk_idx = xregFloat4HmgNorm(pinhole_to_det_wrt_itk_idx);
    const float intersect_len_wrt_itk_idx = (t.y - t.x) * pinhole_to_det_len_wrt_itk_idx;

    const float4 focal_pt_to_det_wrt_cam = cur_det_pt_wrt_cam - focal_pt_wrt_cam;
    // NOTE: since we are transforming a vector with a scaling transform (points to indices),
    //       the output vector may not have the same norm, thus we take the norm
    const float step_len_wrt_itk_idx = xregFloat4HmgNorm(
                                          xregFrm4x4XformFloat4Vec(xform_cam_to_itk_idx,
                                                                   (focal_pt_to_det_wrt_cam / xregFloat4HmgNorm(focal_pt_to_det_wrt_cam)) * args.step_size));

    const ulong num_steps = (ulong)(intersect_len_wrt_itk_idx / step_len_wrt_itk_idx);

    float4 cur_cont_vol_idx = (float4) (start_pt_wrt_itk_idx.x, start_pt_wrt_itk_idx.y, start_pt_wrt_itk_idx.z, 0);

    cur_cont_vol_idx += tex_coords_off;

    // "/ pinhole_to_det_len_wrt_itk_idx" makes pinhole_to_det_wrt_itk_idx a unit vector
    const float scale_to_step = step_len_wrt_itk_idx / pinhole_to_det_len_wrt_itk_idx;

    const float4 step_vec_wrt_itk_idx = (float4) (pinhole_to_det_wrt_itk_idx.x * scale_to_step, pinhole_to_det_wrt_itk_idx.y * scale_to_step, pinhole_to_det_wrt_itk_idx.z * scale_to_step, 0);

    int is_edge = 0;

    for (ulong step_idx = 0; step_idx <= num_steps; ++step_idx, cur_cont_vol_idx += step_vec_wrt_itk_idx)
    {
      if (read_imagef(vol_tex, sampler, cur_cont_vol_idx).x >= sur_coll_args.sur_coll_thresh)
      {
        // approximate the derivative at this point with finite differencing adjacent indices
        // NOTE: these are wrt image (index) axes.

        float3 grad_vec;

        // Gradient in X Direction:
        grad_vec.x = read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x + 1, cur_cont_vol_idx.y, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x -
                     read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x - 1, cur_cont_vol_idx.y, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x;

        // Gradient in Y Direction:
        grad_vec.y = read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y + 1, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x -
                     read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y - 1, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x;

        // Gradient in Z Direction:
        grad_vec.z = read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y, cur_cont_vol_idx.z + 1, cur_cont_vol_idx.w)).x -
                     read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y, cur_cont_vol_idx.z - 1, cur_cont_vol_idx.w)).x;

        grad_vec /= xregFloat3Norm(grad_vec);

        // 1.5707963267948966 == pi/2
        is_edge = fabs(acos(xregFloat3Inner(grad_vec,
                     xregFloat3Normalize(xregFloat4HmgToFloat3(pinhole_to_det_wrt_itk_idx))))
                                            - 1.5707963267948966f) < sur_coll_args.occluding_ang_thresh_rad;

        break;
      }
    }

    dst_contours_buf[idx] += is_edge ? 1 : 0;
  }  // if (idx < num_rays)
}

);

//////////////////////////////////////////////////////////////////////

}  // un-named

xreg::RayCasterOccludingContoursOCL::RayCasterOccludingContoursOCL(const boost::compute::device& dev)
  : RayCasterOCL(dev)
{ }

xreg::RayCasterOccludingContoursOCL::RayCasterOccludingContoursOCL(const boost::compute::context& ctx,
                                                                   const boost::compute::command_queue& queue)
  : RayCasterOCL(ctx, queue)
{ }

void xreg::RayCasterOccludingContoursOCL::compute(const size_type vol_idx)
{
  xregASSERT(this->resources_allocated_);

  compute_helper_pre_kernels(vol_idx);

  // setup kernel arguments and launch
  ContourSurCollArgs coll_args = { this->render_thresh(), this->occlusion_angle_thresh_rad() };

  dev_kernel_.set_arg(0, sizeof(ray_cast_kernel_args_), &ray_cast_kernel_args_);

  dev_kernel_.set_arg(1, sizeof(coll_args), &coll_args);

  dev_kernel_.set_arg(2, det_pts_dev_);

  dev_kernel_.set_arg(3, vol_texs_dev_[vol_idx]);

  dev_kernel_.set_arg(4, cam_to_itk_phys_xforms_dev_);

  dev_kernel_.set_arg(5, *proj_pixels_dev_to_use_);

  dev_kernel_.set_arg(6, cam_model_for_proj_dev_);

  dev_kernel_.set_arg(7, focal_pts_dev_);

  std::size_t global_work_size = ray_cast_kernel_args_.num_det_pts * this->num_projs_;

  cmd_queue_.enqueue_nd_range_kernel(dev_kernel_,
                                     1, // dim
                                     0, // null offset -> start at 0
                                     &global_work_size,
                                     0  // passing null lets open CL pick a local size
                                    ).wait();

  compute_helper_post_kernels(vol_idx);
}

void xreg::RayCasterOccludingContoursOCL::allocate_resources()
{
  namespace bc = boost::compute;

  if (this->num_backtracking_steps() > 0)
  {
    xregThrow("For GPU Occluding Contours - Backtracking is NOT supported!");
  }

  if (!this->stop_after_collision())
  {
    xregThrow("For GPU Occluding Contours - Continuing after collision is NOT supported!");
  }

  RayCasterOCL::allocate_resources();

  // Build the surface rendering ray casting program
  std::stringstream ss;
  ss << RayCastBaseOCLStr()
     << boost::compute::type_definition<ContourSurCollArgs>()
     << kRAY_CASTING_OCCLUDING_CONTOUR_OPENCL_SRC;

  bc::program prog = bc::program::create_with_source(ss.str(), ctx_);

  try
  {
    prog.build();
  }
  catch (bc::opencl_error &)
  {
    std::cerr << "OpenCL Kernel Compile Error (RayCasterOccludingContoursOCL):\n"
              << prog.build_log() << std::endl;
    throw;
  }

  dev_kernel_ = prog.create_kernel("xregOccludingContourKernel");
}


// Copyright (c) 2020 The Navcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <blsct/transaction.h>
#include <chainparams.h>
#include <coins.h>
#include <hash.h>
#include <key.h>
#include <keystore.h>
#include <random.h>
#include <uint256.h>
#include <wallet/test/wallet_test_fixture.h>
#include <main.h>

#include <vector>
#include <map>

#include <boost/test/unit_test.hpp>

const uint32_t BIP32_HARDENED_KEY_LIMIT = 0x80000000;

BOOST_FIXTURE_TEST_SUITE(blsct_wallet_tests, WalletTestingSetup)

BOOST_AUTO_TEST_CASE(blsct_wallet)
{
    LOCK(cs_main);

    CStateViewCache view(pcoinsTip);
    CBasicKeyStore keystore;

    CKey key;
    key.MakeNewKey(0);

    CHashWriter h(0, 0);
    std::vector<unsigned char> vKey(key.begin(), key.end());

    h << vKey;

    uint256 hash = h.GetHash();
    unsigned char h_[32];
    memcpy(h_, &hash, 32);

    blsctKey masterBLSKey = blsctKey(bls::PrivateKey::FromSeed(h_, 32));
    blsctKey childBLSKey = blsctKey(masterBLSKey.PrivateChild(BIP32_HARDENED_KEY_LIMIT|130));
    blsctKey transactionBLSKey = blsctKey(childBLSKey.PrivateChild(BIP32_HARDENED_KEY_LIMIT));
    blsctKey blindingBLSKey = blsctKey(childBLSKey.PrivateChild(BIP32_HARDENED_KEY_LIMIT|1));
    bls::PrivateKey viewKey = blsctKey(transactionBLSKey).PrivateChild(BIP32_HARDENED_KEY_LIMIT);
    bls::PrivateKey spendKey = blsctKey(transactionBLSKey).PrivateChild(BIP32_HARDENED_KEY_LIMIT|1);

    BOOST_CHECK(pwalletMain->SetBLSCTDoublePublicKey(blsctDoublePublicKey(viewKey.GetG1Element(), spendKey.GetG1Element())));
    BOOST_CHECK(pwalletMain->SetBLSCTViewKey(blsctKey(viewKey)));
    BOOST_CHECK(pwalletMain->SetBLSCTSpendKey(blsctKey(spendKey)));
    BOOST_CHECK(pwalletMain->SetBLSCTBlindingMasterKey(blindingBLSKey));

    BOOST_CHECK(pwalletMain->NewBLSCTBlindingKeyPool());
    BOOST_CHECK(pwalletMain->NewBLSCTSubAddressKeyPool(0));
    BOOST_CHECK(!pwalletMain->IsLocked());

    BOOST_CHECK(pwalletMain->TopUpBLSCTBlindingKeyPool());
    BOOST_CHECK(pwalletMain->TopUpBLSCTSubAddressKeyPool(0));

    CKeyID keyID;
    BOOST_CHECK(pwalletMain->GetBLSCTSubAddressKeyFromPool(0, keyID));

    blsctPublicKey bpk;
    BOOST_CHECK(pwalletMain->GetBLSCTBlindingKeyFromPool(bpk));

    blsctKey b;
    BOOST_CHECK(pwalletMain->GetBLSCTBlindingKey(bpk, b));

    blsctDoublePublicKey destKey;
    BOOST_CHECK(pwalletMain->GetBLSCTSubAddressPublicKeys(keyID, destKey));

    Scalar gamma;
    std::string strFailReason;
    std::vector<bls::G2Element> vBLSSignatures;
    CTxOut txout;
    bls::G1Element nonce;
    BOOST_CHECK(CreateBLSCTOutput(b.GetKey(), nonce, txout, destKey, 10*COIN, "", gamma, strFailReason, false, vBLSSignatures));

    BOOST_CHECK(pwalletMain->IsMine(txout));
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     glass_walls.cpp
* \author   Collin Johnson
*
* Definition of GlassWall and associated predict_glass_walls function.
*/

#include <hssh/metrical/mapping/glass_walls.h>
#include <hssh/metrical/mapping/glass_map_utils.h>
#include <hssh/metrical/glass_map.h>
#include <core/laser_scan.h>
#include <laser/line_extraction.h>
#include <laser/line_extractor_params.h>
#include <math/regression.h>
#include <math/clustering.h>
#include <utils/timestamp.h>
#include <boost/optional.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <iomanip>

namespace vulcan
{
namespace hssh
{

template <class Compare>  // (cell, cell, glassMap) -> bool
CellVector bfs_glass_map(cell_t start, const GlassMap& map, CellSet& visited, Compare comp);
std::vector<GlassWall> expand_glass_wall(cell_t start, const GlassMap& map, CellSet& visited);
CellToIntMap split_into_angle_clusters(const CellVector& cells, const GlassMap& map);
std::vector<GlassWall> split_wall(const GlassWall& wall, const GlassMap& map);
std::vector<GlassWall> merge_walls(const std::vector<GlassWall>& walls, const GlassMap& map);
double distance_between_walls(const GlassWall& lhs, const GlassWall& rhs);

////////////////////////////  GlassWall implementation  ////////////////////////////////////

GlassWall::GlassWall(const CellVector& cells, const GlassMap& map)
: cells_(cells.size())
{
    // Convert the cells to the corresponding global positions
    std::transform(cells.begin(), cells.end(), cells_.begin(), [&map](cell_t cell) {
        return utils::grid_point_to_global_point(cell, map);
    });

    computeWall(map);
}

GlassWall::GlassWall(const std::vector<GlassWall>& walls, const GlassMap& map)
{
    for(auto& w : walls)
    {
        boost::push_back(cells_, boost::as_array(w.cells_));
    }

    computeWall(map);
}

void GlassWall::computeWall(const GlassMap& map)
{
    // The wall line can be fit to these grid points associated with the wall
    wall_ = math::total_least_squares(cells_.begin(), cells_.end());

    // Sum the angle bins for all cells in the wall to find the normal
    std::vector<int> sumBins(map.numAngleBins(), 0);
    for(auto global : cells_)
    {
        auto c = utils::global_point_to_grid_cell_round(global, map);
        auto binBegin = map.beginBin(c.x, c.y);
        for(int n = 0; n < map.numAngleBins(); ++n)
        {
            sumBins[n] = *(binBegin + n);
        }
    }

    normal_ = weighted_angle_sum(sumBins.begin(), sumBins.end());

    double totalError = 0.0;
    for(auto& c : cells_)
    {
        totalError += distance_to_line_segment(c, wall_);
    }
    error_ = totalError / cells_.size();

    // TODO: Compute the predicted wall.

    std::cout << "Wall with " << cells_.size() << " cells at " << wall_ << " Normal:" << normal_ << " Error:"
        << error_ << '\n';
}

////////////////////////////  predict_glass_walls implementation  ///////////////////////////////////?

std::vector<GlassWall> predict_glass_walls(const GlassMap& map)
{
    int64_t computationStartTimeUs = utils::system_time_us();

    auto flatActiveRegion = active_region_in_flat_map(map);

    const auto& flatGrid = map.flattenedMap();
    CellSet visited;
    std::vector<GlassWall> walls;

    // Any cells deemed to be
    for(int y = flatActiveRegion.bottomLeft.y; y < flatActiveRegion.topRight.y; ++y)
    {
        for(int x = flatActiveRegion.bottomLeft.x; x < flatActiveRegion.topRight.x; ++x)
        {
            if((flatGrid.getCellTypeNoCheck(x, y) & kOccupiedOccGridCell)
                && (visited.find(cell_t(x, y)) == visited.end()))
            {
                auto newWalls = expand_glass_wall(cell_t(x, y), map, visited);
                boost::push_back(walls, boost::as_array(newWalls));
            }
        }
    }

    int64_t computationTimeUs = utils::system_time_us() - computationStartTimeUs;
    std::cout << "Wall prediction time: " << computationTimeUs << "us\n";
    return walls;
}

template <class Compare>
CellVector bfs_glass_map(cell_t start, const GlassMap& map, CellSet& visited, Compare comp)
{
    auto activeRegion = map.activeRegionInCells();
    auto flatActiveRegion = active_region_in_flat_map(map);
    auto glassToFlatOffset = map.glassToFlatMapOffset();

    const auto& flatGrid = map.flattenedMap();
    CellVector glassCells;

    // Use an 8-way search
    const int xDeltas[8] = { -1, 0, 1, -1, 1, -1, 0, 1 };
    const int yDeltas[8] = { -1, -1, -1, 0, 0, 1, 1, 1 };

    std::deque<cell_t> searchQueue;
    searchQueue.push_back(start);
    visited.insert(start);

    cell_t debugCell(553, 429);

    while(!searchQueue.empty())
    {
        auto cell = searchQueue.front();
        searchQueue.pop_front();

        auto angleCell = cell - glassToFlatOffset;
        glassCells.push_back(angleCell);

        bool debug = debugCell == angleCell;
        if(debug)
        {
            std::cout << "Debugging BFS for " << debugCell << " filled bins:";

            int binIndex = 0;
            for(auto b : boost::make_iterator_range(map.beginBin(angleCell.x, angleCell.y),
                map.endBin(angleCell.x, angleCell.y)))
            {
                if(b > 0)
                {
                    std::cout << binIndex << ' ';
                }
                ++binIndex;
            }
            std::cout << "Offset:" << glassToFlatOffset << " Glass origin:" << map.getBottomLeft() << " Flat origin:"
                << flatGrid.getBottomLeft();
        }

        for(int n = 0; n < 8; ++n)
        {
            if(debug)
            {
                std::cout << '\n';
            }
            Point<int> adjacentCell(cell.x + xDeltas[n], cell.y + yDeltas[n]);
            auto adjacentAngleCell = adjacentCell - glassToFlatOffset;

            if(debug)
            {
                std::cout << "Adj:" << adjacentAngleCell << " Visited:" << (visited.find(adjacentCell) != visited.end());
            }

            if(debug)
            {
                std::cout << " bins:";
                int binIndex = 0;
                for(auto b : boost::make_iterator_range(map.beginBin(adjacentAngleCell.x, adjacentAngleCell.y),
                    map.endBin(adjacentAngleCell.x, adjacentAngleCell.y)))
                {
                    if(b > 0)
                    {
                        std::cout << binIndex << ' ';
                    }
                    ++binIndex;
                }
            }

            // Ignore the neighbor if it has been visited already
            if(visited.find(adjacentCell) != visited.end())
            {
                continue;
            }

            assert(is_cell_in_region(adjacentCell, flatActiveRegion) == is_cell_in_region(adjacentAngleCell, activeRegion));

            if(debug)
            {
                std::cout << " in-region:" << is_cell_in_region(adjacentCell, flatActiveRegion);
            }

            // Ensure the cell is in the active region of the map
            if(!is_cell_in_region(adjacentCell, flatActiveRegion))
            {
                continue;
            }

            if(debug)
            {
                std::cout << " occ:" << std::hex << (int)flatGrid.getCellTypeNoCheck(adjacentCell) << ' '
                    << (int)(~flatGrid.getCellTypeNoCheck(adjacentCell) & kOccupiedOccGridCell) << std::dec
                    << " cost:" << (int)flatGrid.getCostNoCheck(adjacentCell.x, adjacentCell.y);
            }

            // Skip anything that isn't occupied because it isn't a confirmed piece of glass
            if(~flatGrid.getCellTypeNoCheck(adjacentCell) & kOccupiedOccGridCell)
            {
                continue;
            }

            // If the cells are also adjacent across angles, then add the cell to the queue and mark as part of
            // the occupied portion of the environment.
            if(debug)
            {
                std::cout << " angle:" << comp(angleCell, adjacentAngleCell);
            }
            if(comp(angleCell, adjacentAngleCell))
            {
                searchQueue.push_back(adjacentCell);
                visited.insert(adjacentCell);
            }
        }
    }

    return glassCells;
}

std::vector<GlassWall> expand_glass_wall(cell_t start, const GlassMap& map, CellSet& visited)
{
    // Find all angle-adjacent cells to start cell
    auto cells = bfs_glass_map(start, map, visited, [&map](cell_t lhs, cell_t rhs) {
        return map.haveOverlappingAngles(lhs.x, lhs.y, rhs.x, rhs.y);
    });

    assert(!cells.empty());

//     std::vector<GlassWall> walls;
//     walls.push_back(GlassWall(cells, map));
//     return walls;

    // Split those cells into clusters based on angle bins
    auto clusters = split_into_angle_clusters(cells, map);

    // Expand each cluster to only adjacent cells to create the appropriate wall
    std::vector<GlassWall> walls;
    CellSet visitedClusters;
    auto offset = map.glassToFlatMapOffset();

    for(auto c : cells)
    {
        auto offsetCell = c + offset;

        if(visitedClusters.find(offsetCell) != visitedClusters.end())
        {
            continue;
        }

        auto wallCells = bfs_glass_map(offsetCell, map, visitedClusters, [&clusters](cell_t lhs, cell_t rhs) {
            return clusters[lhs] == clusters[rhs];
        });

        if(wallCells.size() > 2)
        {
            GlassWall wall(wallCells, map);
            if(wall.error() > 0.1)
            {
                std::cout << "Splitting wall error was too high!\n";
                boost::push_back(walls, split_wall(wall, map));
            }
            else
            {
                walls.push_back(std::move(wall));
            }
        }
    }

    std::sort(walls.begin(), walls.end(), [](const GlassWall& lhs, const GlassWall& rhs) {
        return length(lhs.wall()) > length(rhs.wall());
    });

    return merge_walls(walls, map);
}

CellToIntMap split_into_angle_clusters(const CellVector& cells, const GlassMap& map)
{
    // Create the normals
    std::vector<double> normals(cells.size());
    std::transform(cells.begin(), cells.end(), normals.begin(), [&map](cell_t c) {
        return wrap_to_2pi(glass_cell_normal(c.x, c.y, map));
    });

    // Cluster them to see what happens
    auto clusters = math::dbscan_1d_linear(normals.begin(), normals.end(), 10.0 * M_PI / 180.0, 25);

    CellToIntMap cellCluster;
    for(std::size_t n = 0; n < cells.size(); ++n)
    {
        cellCluster[cells[n]] = clusters.assignedCluster[n];
    }

    return cellCluster;
}

std::vector<GlassWall> split_wall(const GlassWall& wall, const GlassMap& map)
{
    CellToIntMap cellToLine;

    // Create a fake laser scan from the laser data to try and do a line fitting.
    Point<float> mean;
    for(auto c : wall)
    {
        mean.x += c.x;
        mean.y += c.y;
    }
    mean.x /= wall.size();
    mean.y /= wall.size();

    // Do a radial sort of the cells
    std::vector<Point<float>> sorted(wall.begin(), wall.end());
    std::sort(sorted.begin(), sorted.end(), [mean](Point<float> lhs, Point<float> rhs) {
        return angle_to_point(mean, lhs) < angle_to_point(mean, rhs);
    });

    polar_laser_scan_t scan;
    scan.numRanges = sorted.size();
    for(auto c : sorted)
    {
        scan.ranges.push_back(distance_between_points(mean, c));
    }
    cartesian_laser_scan_t cartScan;
    cartScan.numPoints = sorted.size();
    cartScan.scanPoints = sorted;

    laser::quick_split_params_t params;
    params.clusterDistance     = 0.5;
    params.maxDistanceFromLine = 0.05;
    params.minPoints           = 5;

    std::vector<GlassWall> splitWalls;
    auto extracted = laser::quick_split(scan, cartScan, params);
    CellVector splitCells;
    for(int index = 0, endIndex = static_cast<int>(extracted.lines.size()); index < endIndex; ++index)
    {
        splitCells.clear();
        for(std::size_t n = 0; n < sorted.size(); ++n)
        {
            if(extracted.indices[n] == index)
            {
                splitCells.push_back(utils::global_point_to_grid_cell_round(sorted[n], map));
            }
        }
        splitWalls.push_back(GlassWall(splitCells, map));
    }

    return splitWalls;
}

std::vector<GlassWall> merge_walls(const std::vector<GlassWall>& walls, const GlassMap& map)
{
    std::vector<GlassWall> mergedWalls(walls);
    std::vector<GlassWall> toMerge;
    for(std::size_t n = 0; n < mergedWalls.size(); ++n)
    {
        const GlassWall& outer = mergedWalls[n];
        toMerge.push_back(outer);
        for(std::size_t i = n + 1; i < mergedWalls.size(); ++i)
        {
            const GlassWall& inner = mergedWalls[i];
            toMerge.push_back(inner);
            GlassWall merged(toMerge, map);
            if((outer.error() > merged.error() || inner.error() > merged.error())
                && (distance_between_walls(outer, inner) < 0.1))
            {
                std::cout << "Orig: " << outer.wall() << " , " << inner.wall() << " Errors:" << outer.error()
                    << ',' << inner.error() << " Merged:" << merged.wall() << " Error:" << merged.error() << '\n';
                mergedWalls[n] = merged;
                mergedWalls.erase(mergedWalls.begin() + i);
                --i;
                toMerge.clear();
                toMerge.push_back(merged);
            }
            else
            {
                toMerge.pop_back();
            }
        }
        toMerge.clear();
    }

    return mergedWalls;
}

double distance_between_walls(const GlassWall& lhs, const GlassWall& rhs)
{
    double lhsA = distance_to_line_segment(lhs.wall().a, rhs.wall());
    double lhsB = distance_to_line_segment(lhs.wall().b, rhs.wall());
    double rhsA = distance_to_line_segment(rhs.wall().a, lhs.wall());
    double rhsB = distance_to_line_segment(rhs.wall().b, lhs.wall());

    return std::min(lhsA,
                    std::min(lhsB,
                             std::min(rhsA, rhsB)));
}

} // namespace hssh
} // namespace vulcan