<commit_before>/*
 * ============================================================================
 *
 *       Filename:  test-io.cc
 *    Description:  Tests of the IO module.
 *        License:  LGPL-3+
 *         Author:  Kevin Murray, spam@kdmurray.id.au
 *
 * ============================================================================
 */

#include "catch.hpp"
#include "helpers.hh"

#include "qc-io.hh"

#include <iostream>

TEST_CASE("Read structure behaves correctly", "[Read]") {
    qcpp::Read read("Name", "ACGT", "IIII");

    SECTION("Filling read members works") {
        REQUIRE(read.name.size() == 4);
        REQUIRE(read.sequence.size() == 4);
        REQUIRE(read.quality.size() == 4);
    }

    SECTION("Clearing a read empties members") {
        read.clear();

        REQUIRE(read.name.size() == 0);
        REQUIRE(read.sequence.size() == 0);
        REQUIRE(read.quality.size() == 0);
    }

    SECTION("size() gives correct results w/ quality") {
        REQUIRE(read.sequence.size() == read.size());
        REQUIRE(read.quality.size() == read.size());
    }

    SECTION("size() gives correct results w/o quality") {
        read.quality.clear();
        REQUIRE(read.sequence.size() == read.size());
        REQUIRE(read.quality.size() == 0);
    }

    SECTION("str() works w/ quality") {
        REQUIRE(read.str() == "@Name\nACGT\n+\nIIII\n");
    }
    SECTION("str() works w/o quality") {
        read.quality.clear();
        REQUIRE(read.str() == ">Name\nACGT\n");
    }
}

TEST_CASE("ReadParser opening works", "[ReadParser]") {
    qcpp::ReadParser parser;
    TestConfig *config = TestConfig::get_config();
    std::string infile = config->get_data_file("valid_il.fastq");

    SECTION("opening valid FASTQ as std::string works") {
        REQUIRE_NOTHROW(parser.open(infile));
    }

    SECTION("opening valid FASTQ as char * works") {
        REQUIRE_NOTHROW(parser.open(infile.c_str()));
    }
}

TEST_CASE("Valid file parsing, including get_num_reads", "[ReadParser]") {
    qcpp::Read read;
    qcpp::ReadParser parser;
    TestConfig *config = TestConfig::get_config();
    std::string infile;
    size_t n_reads = 0;

    SECTION("valid_il.fastq") {
        infile = config->get_data_file("valid_il.fastq");

        REQUIRE_NOTHROW(parser.open(infile));

        // Count all reads, parse_read returns false on EOF
        while (parser.parse_read(read)) {
            n_reads++;
        }

        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
    }

    SECTION("valid.fasta") {
        infile = config->get_data_file("valid.fasta");

        REQUIRE_NOTHROW(parser.open(infile));

        // Count all reads, parse_read returns false on EOF
        while (parser.parse_read(read)) {
            n_reads++;
        }

        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
    }
}

TEST_CASE("Parsing of invalid or weird files", "[ReadParser]") {
    qcpp::Read              read;
    qcpp::ReadParser        parser;
    TestConfig             *config = TestConfig::get_config();
    std::string             infile;
    size_t                  n_reads = 0;

    SECTION("Empty fastq") {
        infile = config->get_data_file("empty.fastq");
        REQUIRE_THROWS_AS(parser.open(infile), qcpp::IOError);

        while (parser.parse_read(read)) {
            n_reads++;
        }

        REQUIRE(n_reads == 0);
        REQUIRE(parser.get_num_reads() == n_reads);
    }

    SECTION("Truncated fastq") {
        infile = config->get_data_file("truncated.fastq");
        REQUIRE_NOTHROW(parser.open(infile));

        REQUIRE_NOTHROW(parser.parse_read(read)); // First read is OK
        REQUIRE_THROWS_AS(parser.parse_read(read), qcpp::IOError); // 2nd bad
    }
}

TEST_CASE("Read Interleaving", "[ReadInterleaver]") {
    qcpp::ReadPair          read_parser_pair;
    qcpp::ReadPair          read_interleaver_pair;
    qcpp::ReadParser        parser;
    qcpp::ReadInterleaver   interleaver;
    TestConfig             *config = TestConfig::get_config();
    std::string             il_file = config->get_data_file("valid_il.fastq");
    std::string             r1_file = config->get_data_file("valid_R1.fastq");
    std::string             r2_file = config->get_data_file("valid_R2.fastq");

    REQUIRE_NOTHROW(parser.open(il_file));
    REQUIRE_NOTHROW(interleaver.open(r1_file, r2_file));

    SECTION("Interleaving yields a correctly paired read stream") {
        size_t n_pairs = 0;
        bool parse_ret = true, il_ret = true;

        // Count all reads, parse_read returns false on EOF
        while (parse_ret && il_ret) {
            parse_ret = parser.parse_read_pair(read_parser_pair);
            il_ret = interleaver.parse_read_pair(read_interleaver_pair);
            REQUIRE(read_parser_pair == read_interleaver_pair);
            if (parse_ret && il_ret) {
                n_pairs++;
            }
        }
        REQUIRE(parse_ret == il_ret);

        size_t n_reads = n_pairs * 2;
        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
        REQUIRE(interleaver.get_num_reads() == n_reads);
        REQUIRE(interleaver.get_num_pairs() == n_pairs);
    }
}

TEST_CASE("Fastq writing", "[ReadParser]") {
    qcpp::ReadWriter writer;
    TestConfig *config = TestConfig::get_config();
    std::string outfile = config->get_writable_file("fastq");

    CAPTURE(outfile);
    REQUIRE_NOTHROW(writer.open(outfile));

    SECTION("Writing a single read works") {
        qcpp::Read read("Name", "ACGT", "IIII");

        REQUIRE(writer.get_num_reads() == 0);

        REQUIRE_NOTHROW(writer.write_read(read));

        REQUIRE(writer.get_num_reads() == 1);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(outfile, "data/truths/se.fastq"));
    }
    SECTION("Writing a single read works") {
        qcpp::ReadPair rp("seq1", "ACGT", "IIII",
                          "seq2", "ACGT", "IIII");

        REQUIRE(writer.get_num_reads() == 0);

        REQUIRE_NOTHROW(writer.write_read_pair(rp));

        REQUIRE(writer.get_num_reads() == 2);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(outfile, "data/truths/pe.fastq"));
    }
}

TEST_CASE("Round-trip parse-write", "[ReadParser]") {
    qcpp::ReadParser    parser;
    qcpp::ReadWriter    writer;
    TestConfig         *config = TestConfig::get_config();
    std::string         infile = config->get_data_file("valid_il.fastq");
    std::string         outfile = config->get_writable_file("fastq", true);
    size_t              n_reads = 0;

    CAPTURE(infile);
    CAPTURE(outfile);
    REQUIRE_NOTHROW(parser.open(infile));
    REQUIRE_NOTHROW(writer.open(outfile));

    SECTION("Single-ended round trip") {
        qcpp::Read read;

        while (parser.parse_read(read)) {
            REQUIRE_NOTHROW(writer.write_read(read));
            n_reads++;
        }

        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
        REQUIRE(writer.get_num_reads() == n_reads);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(infile, outfile));
    }

    SECTION("Paired-ended round trip") {
        qcpp::ReadPair pair;

        while (parser.parse_read_pair(pair)) {
            REQUIRE_NOTHROW(writer.write_read_pair(pair));
            n_reads++;
        }

        REQUIRE(n_reads == 5); // Pairs, not reads
        REQUIRE(parser.get_num_reads() == 10);
        REQUIRE(writer.get_num_reads() == 10);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(infile, outfile));
    }
}
<commit_msg>Add tests of new Read.erase behaviour<commit_after>/*
 * ============================================================================
 *
 *       Filename:  test-io.cc
 *    Description:  Tests of the IO module.
 *        License:  LGPL-3+
 *         Author:  Kevin Murray, spam@kdmurray.id.au
 *
 * ============================================================================
 */

#include "catch.hpp"
#include "helpers.hh"

#include "qc-io.hh"

#include <iostream>

TEST_CASE("Read structure behaves correctly", "[Read]") {
    qcpp::Read read("Name", "ACGT", "IIII");

    SECTION("Filling read members works") {
        REQUIRE(read.name.size() == 4);
        REQUIRE(read.sequence.size() == 4);
        REQUIRE(read.quality.size() == 4);
    }

    SECTION("Clearing a read empties members") {
        read.clear();

        REQUIRE(read.name.size() == 0);
        REQUIRE(read.sequence.size() == 0);
        REQUIRE(read.quality.size() == 0);
    }

    SECTION("size() gives correct results w/ quality") {
        REQUIRE(read.sequence.size() == read.size());
        REQUIRE(read.quality.size() == read.size());
    }

    SECTION("size() gives correct results w/o quality") {
        read.quality.clear();
        REQUIRE(read.sequence.size() == read.size());
        REQUIRE(read.quality.size() == 0);
    }

    SECTION("str() works w/ quality") {
        REQUIRE(read.str() == "@Name\nACGT\n+\nIIII\n");
    }

    SECTION("str() works w/o quality") {
        read.quality.clear();
        REQUIRE(read.str() == ">Name\nACGT\n");
    }

    SECTION("Erase with just start") {
        read.erase(1);
        REQUIRE(read.sequence == "A");
        REQUIRE(read.quality == "I");
        REQUIRE(read.size() == 1);
    }

    SECTION("Erase in middle with start and end") {
        read.erase(1, 2);
        REQUIRE(read.sequence == "AT");
        REQUIRE(read.quality == "II");
        REQUIRE(read.size() == 2);
    }

    SECTION("Erase from start with start and end") {
        read.erase(0, 1);
        REQUIRE(read.sequence == "CGT");
        REQUIRE(read.quality == "III");
        REQUIRE(read.size() == 3);
    }
    SECTION("Erase count=0") {
        read.erase(0, 0);
        REQUIRE(read.sequence == "ACGT");
        REQUIRE(read.quality == "IIII");
        REQUIRE(read.size() == 4);
    }
}

TEST_CASE("ReadParser opening works", "[ReadParser]") {
    qcpp::ReadParser parser;
    TestConfig *config = TestConfig::get_config();
    std::string infile = config->get_data_file("valid_il.fastq");

    SECTION("opening valid FASTQ as std::string works") {
        REQUIRE_NOTHROW(parser.open(infile));
    }

    SECTION("opening valid FASTQ as char * works") {
        REQUIRE_NOTHROW(parser.open(infile.c_str()));
    }
}

TEST_CASE("Valid file parsing, including get_num_reads", "[ReadParser]") {
    qcpp::Read read;
    qcpp::ReadParser parser;
    TestConfig *config = TestConfig::get_config();
    std::string infile;
    size_t n_reads = 0;

    SECTION("valid_il.fastq") {
        infile = config->get_data_file("valid_il.fastq");

        REQUIRE_NOTHROW(parser.open(infile));

        // Count all reads, parse_read returns false on EOF
        while (parser.parse_read(read)) {
            n_reads++;
        }

        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
    }

    SECTION("valid.fasta") {
        infile = config->get_data_file("valid.fasta");

        REQUIRE_NOTHROW(parser.open(infile));

        // Count all reads, parse_read returns false on EOF
        while (parser.parse_read(read)) {
            n_reads++;
        }

        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
    }
}

TEST_CASE("Parsing of invalid or weird files", "[ReadParser]") {
    qcpp::Read              read;
    qcpp::ReadParser        parser;
    TestConfig             *config = TestConfig::get_config();
    std::string             infile;
    size_t                  n_reads = 0;

    SECTION("Empty fastq") {
        infile = config->get_data_file("empty.fastq");
        REQUIRE_THROWS_AS(parser.open(infile), qcpp::IOError);

        while (parser.parse_read(read)) {
            n_reads++;
        }

        REQUIRE(n_reads == 0);
        REQUIRE(parser.get_num_reads() == n_reads);
    }

    SECTION("Truncated fastq") {
        infile = config->get_data_file("truncated.fastq");
        REQUIRE_NOTHROW(parser.open(infile));

        REQUIRE_NOTHROW(parser.parse_read(read)); // First read is OK
        REQUIRE_THROWS_AS(parser.parse_read(read), qcpp::IOError); // 2nd bad
    }
}

TEST_CASE("Read Interleaving", "[ReadInterleaver]") {
    qcpp::ReadPair          read_parser_pair;
    qcpp::ReadPair          read_interleaver_pair;
    qcpp::ReadParser        parser;
    qcpp::ReadInterleaver   interleaver;
    TestConfig             *config = TestConfig::get_config();
    std::string             il_file = config->get_data_file("valid_il.fastq");
    std::string             r1_file = config->get_data_file("valid_R1.fastq");
    std::string             r2_file = config->get_data_file("valid_R2.fastq");

    REQUIRE_NOTHROW(parser.open(il_file));
    REQUIRE_NOTHROW(interleaver.open(r1_file, r2_file));

    SECTION("Interleaving yields a correctly paired read stream") {
        size_t n_pairs = 0;
        bool parse_ret = true, il_ret = true;

        // Count all reads, parse_read returns false on EOF
        while (parse_ret && il_ret) {
            parse_ret = parser.parse_read_pair(read_parser_pair);
            il_ret = interleaver.parse_read_pair(read_interleaver_pair);
            REQUIRE(read_parser_pair == read_interleaver_pair);
            if (parse_ret && il_ret) {
                n_pairs++;
            }
        }
        REQUIRE(parse_ret == il_ret);

        size_t n_reads = n_pairs * 2;
        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
        REQUIRE(interleaver.get_num_reads() == n_reads);
        REQUIRE(interleaver.get_num_pairs() == n_pairs);
    }
}

TEST_CASE("Fastq writing", "[ReadParser]") {
    qcpp::ReadWriter writer;
    TestConfig *config = TestConfig::get_config();
    std::string outfile = config->get_writable_file("fastq");

    CAPTURE(outfile);
    REQUIRE_NOTHROW(writer.open(outfile));

    SECTION("Writing a single read works") {
        qcpp::Read read("Name", "ACGT", "IIII");

        REQUIRE(writer.get_num_reads() == 0);

        REQUIRE_NOTHROW(writer.write_read(read));

        REQUIRE(writer.get_num_reads() == 1);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(outfile, "data/truths/se.fastq"));
    }
    SECTION("Writing a single read works") {
        qcpp::ReadPair rp("seq1", "ACGT", "IIII",
                          "seq2", "ACGT", "IIII");

        REQUIRE(writer.get_num_reads() == 0);

        REQUIRE_NOTHROW(writer.write_read_pair(rp));

        REQUIRE(writer.get_num_reads() == 2);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(outfile, "data/truths/pe.fastq"));
    }
}

TEST_CASE("Round-trip parse-write", "[ReadParser]") {
    qcpp::ReadParser    parser;
    qcpp::ReadWriter    writer;
    TestConfig         *config = TestConfig::get_config();
    std::string         infile = config->get_data_file("valid_il.fastq");
    std::string         outfile = config->get_writable_file("fastq", true);
    size_t              n_reads = 0;

    CAPTURE(infile);
    CAPTURE(outfile);
    REQUIRE_NOTHROW(parser.open(infile));
    REQUIRE_NOTHROW(writer.open(outfile));

    SECTION("Single-ended round trip") {
        qcpp::Read read;

        while (parser.parse_read(read)) {
            REQUIRE_NOTHROW(writer.write_read(read));
            n_reads++;
        }

        REQUIRE(n_reads == 10);
        REQUIRE(parser.get_num_reads() == n_reads);
        REQUIRE(writer.get_num_reads() == n_reads);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(infile, outfile));
    }

    SECTION("Paired-ended round trip") {
        qcpp::ReadPair pair;

        while (parser.parse_read_pair(pair)) {
            REQUIRE_NOTHROW(writer.write_read_pair(pair));
            n_reads++;
        }

        REQUIRE(n_reads == 5); // Pairs, not reads
        REQUIRE(parser.get_num_reads() == 10);
        REQUIRE(writer.get_num_reads() == 10);

        REQUIRE_NOTHROW(writer.close());
        REQUIRE(filecmp(infile, outfile));
    }
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2015 Cryptonomex, Inc., and contributors.
 *
 * The MIT License
 *
 * 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 <graphene/app/application.hpp>
#include <graphene/app/plugin.hpp>

#include <graphene/chain/balance_object.hpp>

#include <graphene/utilities/tempdir.hpp>

#include <graphene/account_history/account_history_plugin.hpp>
#include <graphene/market_history/market_history_plugin.hpp>
#include <graphene/witness/witness.hpp>
#include <graphene/grouped_orders/grouped_orders_plugin.hpp>

#include <fc/thread/thread.hpp>
#include <fc/smart_ref_impl.hpp>

#include <boost/filesystem/path.hpp>

#define BOOST_TEST_MODULE Test Application
#include <boost/test/included/unit_test.hpp>

#include "../common/genesis_file_util.hpp"

using namespace graphene;

/////////////
/// @brief create a 2 node network
/////////////
BOOST_AUTO_TEST_CASE( two_node_network )
{
   using namespace graphene::chain;
   using namespace graphene::app;
   try {
      BOOST_TEST_MESSAGE( "Creating and initializing app1" );

      fc::temp_directory app_dir( graphene::utilities::temp_directory_path() );

      graphene::app::application app1;
      app1.register_plugin< graphene::account_history::account_history_plugin>();
      app1.register_plugin< graphene::market_history::market_history_plugin >();
      app1.register_plugin< graphene::witness_plugin::witness_plugin >();
      app1.register_plugin< graphene::grouped_orders::grouped_orders_plugin>();
      app1.startup_plugins();
      boost::program_options::variables_map cfg;
      cfg.emplace("p2p-endpoint", boost::program_options::variable_value(string("127.0.0.1:3939"), false));
      cfg.emplace("genesis-json", boost::program_options::variable_value(create_genesis_file(app_dir), false));
      cfg.emplace("seed-nodes", boost::program_options::variable_value(string("[]"), false));
      app1.initialize(app_dir.path(), cfg);
      BOOST_TEST_MESSAGE( "Starting app1 and waiting 500 ms" );
      app1.startup();
      fc::usleep(fc::milliseconds(500));

      BOOST_TEST_MESSAGE( "Creating and initializing app2" );

      fc::temp_directory app2_dir( graphene::utilities::temp_directory_path() );
      graphene::app::application app2;
      app2.register_plugin<account_history::account_history_plugin>();
      app2.register_plugin< graphene::market_history::market_history_plugin >();
      app2.register_plugin< graphene::witness_plugin::witness_plugin >();
      app2.register_plugin< graphene::grouped_orders::grouped_orders_plugin>();
      app2.startup_plugins();
      auto cfg2 = cfg;
      cfg2.erase("p2p-endpoint");
      cfg2.emplace("p2p-endpoint", boost::program_options::variable_value(string("127.0.0.1:4040"), false));
      cfg2.emplace("genesis-json", boost::program_options::variable_value(create_genesis_file(app_dir), false));
      cfg2.emplace("seed-node", boost::program_options::variable_value(vector<string>{"127.0.0.1:3939"}, false));
      cfg2.emplace("seed-nodes", boost::program_options::variable_value(string("[]"), false));
      app2.initialize(app2_dir.path(), cfg2);

      BOOST_TEST_MESSAGE( "Starting app2 and waiting 500 ms" );
      app2.startup();
      fc::usleep(fc::milliseconds(500));

      BOOST_REQUIRE_EQUAL(app1.p2p_node()->get_connection_count(), 1);
      BOOST_CHECK_EQUAL(std::string(app1.p2p_node()->get_connected_peers().front().host.get_address()), "127.0.0.1");
      BOOST_TEST_MESSAGE( "app1 and app2 successfully connected" );

      std::shared_ptr<chain::database> db1 = app1.chain_database();
      std::shared_ptr<chain::database> db2 = app2.chain_database();

      BOOST_CHECK_EQUAL( db1->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 0 );
      BOOST_CHECK_EQUAL( db2->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 0 );

      BOOST_TEST_MESSAGE( "Creating transfer tx" );
      graphene::chain::signed_transaction trx;
      {
         account_id_type nathan_id = db2->get_index_type<account_index>().indices().get<by_name>().find( "nathan" )->id;
         fc::ecc::private_key nathan_key = fc::ecc::private_key::regenerate(fc::sha256::hash(string("nathan")));

         balance_claim_operation claim_op;
         balance_id_type bid = balance_id_type();
         claim_op.deposit_to_account = nathan_id;
         claim_op.balance_to_claim = bid;
         claim_op.balance_owner_key = nathan_key.get_public_key();
         claim_op.total_claimed = bid(*db1).balance;
         trx.operations.push_back( claim_op );
         db1->current_fee_schedule().set_fee( trx.operations.back() );

         transfer_operation xfer_op;
         xfer_op.from = nathan_id;
         xfer_op.to = GRAPHENE_NULL_ACCOUNT;
         xfer_op.amount = asset( 1000000 );
         trx.operations.push_back( xfer_op );
         db1->current_fee_schedule().set_fee( trx.operations.back() );

         trx.set_expiration( db1->get_slot_time( 10 ) );
         trx.sign( nathan_key, db1->get_chain_id() );
         trx.validate();
      }

      BOOST_TEST_MESSAGE( "Pushing tx locally on db1" );
      processed_transaction ptrx = db1->push_transaction(trx);

      BOOST_CHECK_EQUAL( db1->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 1000000 );
      BOOST_CHECK_EQUAL( db2->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 0 );

      BOOST_TEST_MESSAGE( "Broadcasting tx" );
      app1.p2p_node()->broadcast(graphene::net::trx_message(trx));

      fc::usleep(fc::milliseconds(500));

      BOOST_CHECK_EQUAL( db1->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 1000000 );
      BOOST_CHECK_EQUAL( db2->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 1000000 );

      BOOST_TEST_MESSAGE( "Generating block on db2" );
      fc::ecc::private_key committee_key = fc::ecc::private_key::regenerate(fc::sha256::hash(string("nathan")));

      auto block_1 = db2->generate_block(
         db2->get_slot_time(1),
         db2->get_scheduled_witness(1),
         committee_key,
         database::skip_nothing);

      BOOST_TEST_MESSAGE( "Broadcasting block" );
      app2.p2p_node()->broadcast(graphene::net::block_message( block_1 ));

      fc::usleep(fc::milliseconds(500));
      BOOST_TEST_MESSAGE( "Verifying nodes are still connected" );
      BOOST_CHECK_EQUAL(app1.p2p_node()->get_connection_count(), 1);
      BOOST_CHECK_EQUAL(app1.chain_database()->head_block_num(), 1);

      BOOST_TEST_MESSAGE( "Checking GRAPHENE_NULL_ACCOUNT has balance" );
   } catch( fc::exception& e ) {
      edump((e.to_detail_string()));
      throw;
   }
}

// a contrived example to test the breaking out of application_impl to a header file

#include "../../libraries/app/application_impl.hxx"

BOOST_AUTO_TEST_CASE(application_impl_breakout) {
   class test_impl : public graphene::app::detail::application_impl {
      // override the constructor, just to test that we can
   public:
      test_impl() : application_impl(nullptr) {}
      bool has_item(const net::item_id& id) override {
         return true;
      }
   };

   test_impl impl;
   graphene::net::item_id id;
   BOOST_CHECK(impl.has_item(id));
}
<commit_msg>load_configuration_options basic tests<commit_after>/*
 * Copyright (c) 2015 Cryptonomex, Inc., and contributors.
 *
 * The MIT License
 *
 * 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 <graphene/app/application.hpp>
#include <graphene/app/plugin.hpp>
#include <graphene/app/config_util.hpp>

#include <graphene/chain/balance_object.hpp>

#include <graphene/utilities/tempdir.hpp>

#include <graphene/account_history/account_history_plugin.hpp>
#include <graphene/market_history/market_history_plugin.hpp>
#include <graphene/witness/witness.hpp>
#include <graphene/grouped_orders/grouped_orders_plugin.hpp>

#include <fc/thread/thread.hpp>
#include <fc/smart_ref_impl.hpp>
#include <fc/log/appender.hpp>
#include <fc/log/logger.hpp>

#include <boost/filesystem/path.hpp>

#define BOOST_TEST_MODULE Test Application
#include <boost/test/included/unit_test.hpp>

#include "../common/genesis_file_util.hpp"

using namespace graphene;
namespace bpo = boost::program_options;

namespace fc {
   extern std::unordered_map<std::string, logger> &get_logger_map();
   extern std::unordered_map<std::string, appender::ptr> &get_appender_map();
}

BOOST_AUTO_TEST_CASE(load_configuration_options_test_config_files_created)
{
   fc::temp_directory app_dir(graphene::utilities::temp_directory_path());
   auto dir = app_dir.path();
   auto config_ini_file = dir / "config.ini";
   auto logging_ini_file = dir / "logging.ini";

   /// create default config options
   auto node = new app::application();
   bpo::options_description cli, cfg;
   node->set_program_options(cli, cfg);
   bpo::options_description cfg_options("Graphene Witness Node");
   cfg_options.add(cfg);

   /// check preconditions
   BOOST_CHECK(!fc::exists(config_ini_file));
   BOOST_CHECK(!fc::exists(logging_ini_file));

   bpo::variables_map options;
   app::load_configuration_options(dir, cfg_options, options);

   /// check post-conditions
   BOOST_CHECK(fc::exists(config_ini_file));
   BOOST_CHECK(fc::exists(logging_ini_file));
   BOOST_CHECK_GT(fc::file_size(config_ini_file), 0);
   BOOST_CHECK_GT(fc::file_size(logging_ini_file), 0);
}

BOOST_AUTO_TEST_CASE(load_configuration_options_test_config_ini_options)
{
   fc::temp_directory app_dir(graphene::utilities::temp_directory_path());
   auto dir = app_dir.path();
   auto config_ini_file = dir / "config.ini";
   auto logging_ini_file = dir / "logging.ini";

   /// create config.ini
   bpo::options_description cfg_options("config.ini options");
   cfg_options.add_options()
   ("option1", bpo::value<std::string>(), "")
   ("option2", bpo::value<int>(), "")
   ;
   std::ofstream out(config_ini_file.preferred_string());
   out << "option1=is present\n"
          "option2=1\n\n";
   out.close();

   bpo::variables_map options;
   app::load_configuration_options(dir, cfg_options, options);

   /// check the options values are parsed into the output map
   BOOST_CHECK(!options.empty());
   BOOST_CHECK_EQUAL(options.count("option1"), 1);
   BOOST_CHECK_EQUAL(options.count("option2"), 1);
   BOOST_CHECK_EQUAL(options["option1"].as<std::string>(), "is present");
   BOOST_CHECK_EQUAL(options["option2"].as<int>(), 1);
}

BOOST_AUTO_TEST_CASE(load_configuration_options_test_logging_ini_options)
{
   fc::temp_directory app_dir(graphene::utilities::temp_directory_path());
   auto dir = app_dir.path();
   auto config_ini_file = dir / "config.ini";
   auto logging_ini_file = dir / "logging.ini";

   /// create logging.ini
   /// configure exactly one logger and appender
   std::ofstream out(logging_ini_file.preferred_string());
   out << "[log.file_appender.default]\n"
          "filename=test.log\n\n"
          "[logger.default]\n"
          "level=info\n"
          "appenders=default\n\n"
          ;
   out.close();

   /// clear logger and appender state
   fc::get_logger_map().clear();
   fc::get_appender_map().clear();
   BOOST_CHECK(fc::get_logger_map().empty());
   BOOST_CHECK(fc::get_appender_map().empty());

   bpo::options_description cfg_options("empty");
   bpo::variables_map options;
   app::load_configuration_options(dir, cfg_options, options);

   /// check the options values are parsed into the output map
   /// this is a little bit tricky since load_configuration_options() doesn't provide output variable for logging_config
   auto logger_map = fc::get_logger_map();
   auto appender_map = fc::get_appender_map();
   BOOST_CHECK_EQUAL(logger_map.size(), 1);
   BOOST_CHECK(logger_map.count("default"));
   BOOST_CHECK_EQUAL(appender_map.size(), 1);
   BOOST_CHECK(appender_map.count("default"));
}

/////////////
/// @brief create a 2 node network
/////////////
BOOST_AUTO_TEST_CASE( two_node_network )
{
   using namespace graphene::chain;
   using namespace graphene::app;
   try {
      BOOST_TEST_MESSAGE( "Creating and initializing app1" );

      fc::temp_directory app_dir( graphene::utilities::temp_directory_path() );

      graphene::app::application app1;
      app1.register_plugin< graphene::account_history::account_history_plugin>();
      app1.register_plugin< graphene::market_history::market_history_plugin >();
      app1.register_plugin< graphene::witness_plugin::witness_plugin >();
      app1.register_plugin< graphene::grouped_orders::grouped_orders_plugin>();
      app1.startup_plugins();
      boost::program_options::variables_map cfg;
      cfg.emplace("p2p-endpoint", boost::program_options::variable_value(string("127.0.0.1:3939"), false));
      cfg.emplace("genesis-json", boost::program_options::variable_value(create_genesis_file(app_dir), false));
      cfg.emplace("seed-nodes", boost::program_options::variable_value(string("[]"), false));
      app1.initialize(app_dir.path(), cfg);
      BOOST_TEST_MESSAGE( "Starting app1 and waiting 500 ms" );
      app1.startup();
      fc::usleep(fc::milliseconds(500));

      BOOST_TEST_MESSAGE( "Creating and initializing app2" );

      fc::temp_directory app2_dir( graphene::utilities::temp_directory_path() );
      graphene::app::application app2;
      app2.register_plugin<account_history::account_history_plugin>();
      app2.register_plugin< graphene::market_history::market_history_plugin >();
      app2.register_plugin< graphene::witness_plugin::witness_plugin >();
      app2.register_plugin< graphene::grouped_orders::grouped_orders_plugin>();
      app2.startup_plugins();
      auto cfg2 = cfg;
      cfg2.erase("p2p-endpoint");
      cfg2.emplace("p2p-endpoint", boost::program_options::variable_value(string("127.0.0.1:4040"), false));
      cfg2.emplace("genesis-json", boost::program_options::variable_value(create_genesis_file(app_dir), false));
      cfg2.emplace("seed-node", boost::program_options::variable_value(vector<string>{"127.0.0.1:3939"}, false));
      cfg2.emplace("seed-nodes", boost::program_options::variable_value(string("[]"), false));
      app2.initialize(app2_dir.path(), cfg2);

      BOOST_TEST_MESSAGE( "Starting app2 and waiting 500 ms" );
      app2.startup();
      fc::usleep(fc::milliseconds(500));

      BOOST_REQUIRE_EQUAL(app1.p2p_node()->get_connection_count(), 1);
      BOOST_CHECK_EQUAL(std::string(app1.p2p_node()->get_connected_peers().front().host.get_address()), "127.0.0.1");
      BOOST_TEST_MESSAGE( "app1 and app2 successfully connected" );

      std::shared_ptr<chain::database> db1 = app1.chain_database();
      std::shared_ptr<chain::database> db2 = app2.chain_database();

      BOOST_CHECK_EQUAL( db1->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 0 );
      BOOST_CHECK_EQUAL( db2->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 0 );

      BOOST_TEST_MESSAGE( "Creating transfer tx" );
      graphene::chain::signed_transaction trx;
      {
         account_id_type nathan_id = db2->get_index_type<account_index>().indices().get<by_name>().find( "nathan" )->id;
         fc::ecc::private_key nathan_key = fc::ecc::private_key::regenerate(fc::sha256::hash(string("nathan")));

         balance_claim_operation claim_op;
         balance_id_type bid = balance_id_type();
         claim_op.deposit_to_account = nathan_id;
         claim_op.balance_to_claim = bid;
         claim_op.balance_owner_key = nathan_key.get_public_key();
         claim_op.total_claimed = bid(*db1).balance;
         trx.operations.push_back( claim_op );
         db1->current_fee_schedule().set_fee( trx.operations.back() );

         transfer_operation xfer_op;
         xfer_op.from = nathan_id;
         xfer_op.to = GRAPHENE_NULL_ACCOUNT;
         xfer_op.amount = asset( 1000000 );
         trx.operations.push_back( xfer_op );
         db1->current_fee_schedule().set_fee( trx.operations.back() );

         trx.set_expiration( db1->get_slot_time( 10 ) );
         trx.sign( nathan_key, db1->get_chain_id() );
         trx.validate();
      }

      BOOST_TEST_MESSAGE( "Pushing tx locally on db1" );
      processed_transaction ptrx = db1->push_transaction(trx);

      BOOST_CHECK_EQUAL( db1->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 1000000 );
      BOOST_CHECK_EQUAL( db2->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 0 );

      BOOST_TEST_MESSAGE( "Broadcasting tx" );
      app1.p2p_node()->broadcast(graphene::net::trx_message(trx));

      fc::usleep(fc::milliseconds(500));

      BOOST_CHECK_EQUAL( db1->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 1000000 );
      BOOST_CHECK_EQUAL( db2->get_balance( GRAPHENE_NULL_ACCOUNT, asset_id_type() ).amount.value, 1000000 );

      BOOST_TEST_MESSAGE( "Generating block on db2" );
      fc::ecc::private_key committee_key = fc::ecc::private_key::regenerate(fc::sha256::hash(string("nathan")));

      auto block_1 = db2->generate_block(
         db2->get_slot_time(1),
         db2->get_scheduled_witness(1),
         committee_key,
         database::skip_nothing);

      BOOST_TEST_MESSAGE( "Broadcasting block" );
      app2.p2p_node()->broadcast(graphene::net::block_message( block_1 ));

      fc::usleep(fc::milliseconds(500));
      BOOST_TEST_MESSAGE( "Verifying nodes are still connected" );
      BOOST_CHECK_EQUAL(app1.p2p_node()->get_connection_count(), 1);
      BOOST_CHECK_EQUAL(app1.chain_database()->head_block_num(), 1);

      BOOST_TEST_MESSAGE( "Checking GRAPHENE_NULL_ACCOUNT has balance" );
   } catch( fc::exception& e ) {
      edump((e.to_detail_string()));
      throw;
   }
}

// a contrived example to test the breaking out of application_impl to a header file

#include "../../libraries/app/application_impl.hxx"

BOOST_AUTO_TEST_CASE(application_impl_breakout) {
   class test_impl : public graphene::app::detail::application_impl {
      // override the constructor, just to test that we can
   public:
      test_impl() : application_impl(nullptr) {}
      bool has_item(const net::item_id& id) override {
         return true;
      }
   };

   test_impl impl;
   graphene::net::item_id id;
   BOOST_CHECK(impl.has_item(id));
}
<|endoftext|>


<commit_before>/**
 * The MIT License (MIT)
 *
 * Copyright (c) 2014 wisol technologie GmbH
 *
 * 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.
 *
 * \author    Michael Egli
 * \copyright wisol technologie GmbH
 * \date      18-Dec-2014
 *
 * \file fsm_test.cpp
 * Test definitions for the state machine implementation.
 */

#define CATCH_CONFIG_MAIN
#include "catch.hpp"
#include "../fsm.h"

TEST_CASE("Test Initialization")
{
	FSM::Fsm fsm;
	REQUIRE(fsm.execute('a') == FSM::Fsm_NotInitialized);
}

TEST_CASE("Test initial and final pseudo states")
{
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+1);
	fsm.init();

	SECTION("Test initial pseudo state") {
		REQUIRE(fsm.state() == (int)FSM::Fsm_Initial);
		REQUIRE(fsm.is_initial() == true);
		REQUIRE(fsm.is_final() == false);
	}

	SECTION("Test initial pseudo state") {
		fsm.execute('a');
		REQUIRE(fsm.state() == (int)FSM::Fsm_Final);
		REQUIRE(fsm.is_initial() == false);
		REQUIRE(fsm.is_final() == true);
	}
}

TEST_CASE("Test missing trigger")
{
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, FSM::Fsm_Final, 'b', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+1);
	fsm.init();
	REQUIRE(fsm.execute('a') == FSM::Fsm_NoMatchingTrigger);
}

TEST_CASE("Test guards")
{
	SECTION("Test false guard") {
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return false;}, nullptr},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+1);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// ensure that the transition to final is not taken (because of the guard).
		REQUIRE(fsm.state() == (int)FSM::Fsm_Initial);
	}

	SECTION("Test true guard") {
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return true;}, nullptr},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+1);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// ensure that the transition to final is taken (because of the guard).
		REQUIRE(fsm.state() == (int)FSM::Fsm_Final);
	}

	SECTION("Test same action with different guards") {
		int count = 0;
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return false;}, [&count]{count++;}},
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return true; }, [&count]{count = 10;}},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+2);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// ensure that action2 was taken (because of the guard).
		REQUIRE(count == 10);
	}
}

TEST_CASE("Test Transitions")
{
	SECTION("Test multiple matching transitions") {
		int count = 0;
		const int stateA = 1;
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, stateA        , 'a', nullptr, [&count]{count++;}},
			{stateA          , stateA        , 'a', nullptr, [&count]{count++;}},
			{stateA          , FSM::Fsm_Final, 'a', nullptr, [&count]{count++;}},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+3);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// Ensure that only one action has executed.
		REQUIRE(count == 1);
	}
}

TEST_CASE("Test state machine reset")
{
	int action_count = 0;
	const int stateA = 1;
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, stateA        , 'a', nullptr, nullptr},
		{stateA          , FSM::Fsm_Final, 'b', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+2);
	fsm.init();
	REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
	REQUIRE(fsm.state() == stateA);
	fsm.reset();
	REQUIRE(fsm.state() == (int)FSM::Fsm_Initial);
	REQUIRE(fsm.execute('a') == FSM::Fsm_NotInitialized);
	fsm.init();
	REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
	REQUIRE(fsm.execute('b') == FSM::Fsm_Success);
	REQUIRE(fsm.is_final() == true);
}

<commit_msg>Add a test for the new debug function.<commit_after>/**
 * The MIT License (MIT)
 *
 * Copyright (c) 2014 wisol technologie GmbH
 *
 * 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.
 *
 * \author    Michael Egli
 * \copyright wisol technologie GmbH
 * \date      18-Dec-2014
 *
 * \file fsm_test.cpp
 * Test definitions for the state machine implementation.
 */

#define CATCH_CONFIG_MAIN
#include "catch.hpp"
#include "../fsm.h"

TEST_CASE("Test Initialization")
{
	FSM::Fsm fsm;
	REQUIRE(fsm.execute('a') == FSM::Fsm_NotInitialized);
}

TEST_CASE("Test initial and final pseudo states")
{
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+1);
	fsm.init();

	SECTION("Test initial pseudo state") {
		REQUIRE(fsm.state() == (int)FSM::Fsm_Initial);
		REQUIRE(fsm.is_initial() == true);
		REQUIRE(fsm.is_final() == false);
	}

	SECTION("Test initial pseudo state") {
		fsm.execute('a');
		REQUIRE(fsm.state() == (int)FSM::Fsm_Final);
		REQUIRE(fsm.is_initial() == false);
		REQUIRE(fsm.is_final() == true);
	}
}

TEST_CASE("Test missing trigger")
{
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, FSM::Fsm_Final, 'b', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+1);
	fsm.init();
	REQUIRE(fsm.execute('a') == FSM::Fsm_NoMatchingTrigger);
}

TEST_CASE("Test guards")
{
	SECTION("Test false guard") {
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return false;}, nullptr},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+1);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// ensure that the transition to final is not taken (because of the guard).
		REQUIRE(fsm.state() == (int)FSM::Fsm_Initial);
	}

	SECTION("Test true guard") {
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return true;}, nullptr},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+1);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// ensure that the transition to final is taken (because of the guard).
		REQUIRE(fsm.state() == (int)FSM::Fsm_Final);
	}

	SECTION("Test same action with different guards") {
		int count = 0;
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return false;}, [&count]{count++;}},
			{FSM::Fsm_Initial, FSM::Fsm_Final, 'a', []{return true; }, [&count]{count = 10;}},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+2);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// ensure that action2 was taken (because of the guard).
		REQUIRE(count == 10);
	}
}

TEST_CASE("Test Transitions")
{
	SECTION("Test multiple matching transitions") {
		int count = 0;
		const int stateA = 1;
		FSM::Fsm fsm;
		FSM::Trans transitions[] = {
			{FSM::Fsm_Initial, stateA        , 'a', nullptr, [&count]{count++;}},
			{stateA          , stateA        , 'a', nullptr, [&count]{count++;}},
			{stateA          , FSM::Fsm_Final, 'a', nullptr, [&count]{count++;}},
		};
		fsm.add_transitions(&transitions[0], &transitions[0]+3);
		fsm.init();
		REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
		// Ensure that only one action has executed.
		REQUIRE(count == 1);
	}
}

TEST_CASE("Test state machine reset")
{
	int action_count = 0;
	const int stateA = 1;
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, stateA        , 'a', nullptr, nullptr},
		{stateA          , FSM::Fsm_Final, 'b', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+2);
	fsm.init();
	REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
	REQUIRE(fsm.state() == stateA);
	fsm.reset();
	REQUIRE(fsm.state() == (int)FSM::Fsm_Initial);
	REQUIRE(fsm.execute('a') == FSM::Fsm_NotInitialized);
	fsm.init();
	REQUIRE(fsm.execute('a') == FSM::Fsm_Success);
	REQUIRE(fsm.execute('b') == FSM::Fsm_Success);
	REQUIRE(fsm.is_final() == true);
}

TEST_CASE("Test debug function")
{
	int action_count = 0;
	const int stateA = 1;
	FSM::Fsm fsm;
	FSM::Trans transitions[] = {
		{FSM::Fsm_Initial, stateA        , 'a', nullptr, nullptr},
		{stateA          , FSM::Fsm_Final, 'b', nullptr, nullptr},
	};
	fsm.add_transitions(&transitions[0], &transitions[0]+2);
	fsm.init();

	SECTION("Test enable debugging function.") {
		int dbg_from = 0;
		int dbg_to = 0;
		char dbg_tr = 0;
		fsm.add_debug_fn([&dbg_from, &dbg_to, &dbg_tr](int from, int to, char tr){ dbg_from = from; dbg_to = to; dbg_tr = tr; });
		fsm.execute('a');
		REQUIRE(dbg_from == FSM::Fsm_Initial);
		REQUIRE(dbg_to == stateA);
		REQUIRE(dbg_tr == 'a');
	}

	SECTION("Test disable debugging function.") {
		int dbg_from = 0;
		int dbg_to = 0;
		char dbg_tr = 0;
		fsm.reset();
		fsm.add_debug_fn(nullptr);
		REQUIRE(dbg_from == 0);
		REQUIRE(dbg_to == 0);
		REQUIRE(dbg_tr == 0);
	}
}

<|endoftext|>


<commit_before>#pragma once
//=====================================================================//
/*!	@file
	@brief	RX64M/RX71M/RX651/RX65N/RX66T/RX72T/RX72N/RX72M グループ・システム制御 @n
			※RX24T は構成が大きく異なるので、RX24T/system_io.hpp に分離しています。@n
			※ USB を使う場合：96MHz, 144MHz, 192MHz, 240MHz のいづれか @n
			※ 通常ベースクロックは、12MHz、24MHz を使います。@n
			RX72x 系では、内部 PLL 回路が追加され、Ethernet などに必要な 25MHz を得る為 @n
			16MHz を使います。@n
			(16MHz x 12.5 -> 200MHz, 16MHz x 15 -> 240MHz)
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2017, 2021 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/RX/blob/master/LICENSE
*/
//=====================================================================//
#include "RX600/system.hpp"

#if defined(SIG_RX64M) || defined(SIG_RX71M) || defined(SIG_RX66T) || defined(SIG_RX65N) || defined(SIG_RX72T) || defined(SIG_RX72N) || defined(SIG_RX72M)
#elif
#  error "system_io.hpp: Not available on RX24T"
#endif

namespace device {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  systen_base クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	struct system_base {

		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
		/*!
			@brief  発信器タイプ @n
					HOCO を使う場合、同時に、BASE_CLOCK_ に周波数を設定します。
		*/
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
		enum class OSC_TYPE {
			XTAL,		///< クリスタル接続
			EXT,		///< クロック入力
			HOCO,		///< 高速オンチップオシレーター
			LOCO,		///< 低速オンチップオシレーター (240KHz)
		};
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  systen_io クラス @n
				INTR_CLOCK は、内部 PLL で扱える最大速度です、@n
				外部クリスタルを微妙な周波数にする場合、その整数倍にしなければなりません。 @n
				RX71M はスーパーバイザモードでの変更が必要なので、「start.s」内で行う。 @n
				RX71M の場合、アセンブラにオプション「--defsym MEMWAIT=1」を渡す。
		@param[in]	BASE_CLOCK_	ベース・クロック周波数
		@param[in]	INTR_CLOCK	内臓クロック周波数 @n
								※USB を使う場合、「INTR_CLOCK」は48の倍数である事
		@param[in]	OSC_TYPE	発信器タイプを設定（通常、XTAL）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <auto OSC_TYPE_ = system_base::OSC_TYPE::XTAL>
	struct system_io : public system_base {

		static uint8_t clock_div_(uint32_t clk) noexcept
		{
			uint8_t div = 0;
			while(clk < clock_profile::PLL_BASE) {
				++div;
				clk <<= 1;
			}
			if(div > 0b0110) div = 0b0110;
			return div;
		}

		//-------------------------------------------------------------//
		/*!
			@brief  マスター・クロックのブースト @n
					インストラクション・クロックを最大速度にブーストする。
		*/
		//-------------------------------------------------------------//
		static void boost_master_clock() noexcept
		{
			device::SYSTEM::PRCR = 0xA50B;	// クロック、低消費電力、関係書き込み許可

			device::SYSTEM::MOSCWTCR = 9;	// 1ms wait

			// メインクロック強制発振とドライブ能力設定
			if(OSC_TYPE_ == OSC_TYPE::XTAL) {
				uint8_t modrv2 = 0b11;
				if(clock_profile::BASE > 20'000'000) modrv2 = 0b00;
				else if(clock_profile::BASE > 16'000'000) modrv2 = 0b01;
				else if(clock_profile::BASE > 8'000'000) modrv2 = 0b10;
				device::SYSTEM::MOFCR = device::SYSTEM::MOFCR.MODRV2.b(modrv2);
				device::SYSTEM::MOSCCR.MOSTP = 0;		// メインクロック発振器動作
				while(device::SYSTEM::OSCOVFSR.MOOVF() == 0) { asm("nop"); }
			} else if(OSC_TYPE_ == OSC_TYPE::EXT) {
				device::SYSTEM::MOSCCR.MOSTP = 1;		// メインクロック発振器停止
				device::SYSTEM::MOFCR = device::SYSTEM::MOFCR.MOSEL.b();
			} else if(OSC_TYPE_ == OSC_TYPE::HOCO) {  // 高速オンチップオシレータ
				device::SYSTEM::MOSCCR.MOSTP = 1;
				device::SYSTEM::HOCOCR = device::SYSTEM::HOCOCR.HCSTP.b(1);  // 停止
				uint8_t frq;
				if(clock_profile::BASE == 16'000'000) frq = 0b00;
				else if(clock_profile::BASE == 18'000'000) frq = 0b01;
				else if(clock_profile::BASE == 20'000'000) frq = 0b10;
				else frq = 0b00;
				device::SYSTEM::HOCOCR2.HCFRQ = frq;
				device::SYSTEM::HOCOCR = device::SYSTEM::HOCOCR.HCSTP.b(0);  // 動作
				while(device::SYSTEM::OSCOVFSR.MOOVF() == 0) { asm("nop"); }
			} else {
				device::SYSTEM::PRCR = 0xA500;
				return;
			}

#if defined(SIG_RX65N)
			if(clock_profile::ICLK >= 120'000'000) {  // 120MHz 以上の場合設定
				device::SYSTEM::ROMWT = 0b10;
			} else if(clock_profile::ICLK >= 100'000'000) {
				device::SYSTEM::ROMWT = 0b01;
			} else if(clock_profile::ICLK >= 50'000'000) {
				device::SYSTEM::ROMWT = 0b00;
			}
#endif
			// RX71M はスーパーバイザモードでの変更が必要なので、「start.s」内で行う。
			// RX71M の場合、アセンブラにオプション「--defsym MEMWAIT=1」を渡す。
#if defined(SIG_RX66T) || defined(SIG_RX72M) || defined(SIG_RX72T) || defined(SIG_RX72N)
			if(clock_profile::ICLK > 120'000'000) {  // 120MHz 以上の場合設定
				device::SYSTEM::MEMWAIT = 1;
				volatile auto tmp = device::SYSTEM::MEMWAIT();  // 読み出しを行う
			}
#endif
			// (x10.0) 0b010011, (x10.5) 0b010100, (x11.0) 0b010101, (x11.5) 0b010110
			// ... MAX x30.0
			uint32_t n = clock_profile::PLL_BASE * 2 / clock_profile::BASE;
			if(n < 20) n = 20;
			else if(n > 60) n = 60;
			n -= 20;
			device::SYSTEM::PLLCR.STC = n + 0b010011;  // base x10
			device::SYSTEM::PLLCR2.PLLEN = 0;			// PLL 動作
			while(device::SYSTEM::OSCOVFSR.PLOVF() == 0) { asm("nop"); }

			device::SYSTEM::SCKCR = device::SYSTEM::SCKCR.FCK.b(clock_div_(clock_profile::FCLK))
								  | device::SYSTEM::SCKCR.ICK.b(clock_div_(clock_profile::ICLK))
								  | device::SYSTEM::SCKCR.BCK.b(clock_div_(clock_profile::BCLK))
								  | device::SYSTEM::SCKCR.PCKA.b(clock_div_(clock_profile::PCLKA))
								  | device::SYSTEM::SCKCR.PCKB.b(clock_div_(clock_profile::PCLKB))
								  | device::SYSTEM::SCKCR.PCKC.b(clock_div_(clock_profile::PCLKC))
								  | device::SYSTEM::SCKCR.PCKD.b(clock_div_(clock_profile::PCLKD));
			{  // USB Master Clock の設定
				auto usb_div = clock_profile::PLL_BASE / 48'000'000;
				if(usb_div >= 2 && usb_div <= 5) {
					// 1/2, 1/3, 1/4, 1/5
					device::SYSTEM::SCKCR2.UCK = usb_div - 1;
				}
			}

			if(OSC_TYPE_ == OSC_TYPE::HOCO) {
				device::SYSTEM::PLLCR.PLLSRCSEL = 1;
			}

			device::SYSTEM::SCKCR3.CKSEL = 0b100;   ///< PLL 選択

			if(OSC_TYPE_ == OSC_TYPE::XTAL || OSC_TYPE_ == OSC_TYPE::EXT) {
				device::SYSTEM::LOCOCR.LCSTP = 1;  ///< 低速オンチップオシレータ停止
				device::SYSTEM::HOCOCR.HCSTP = 1;  ///< 高速オンチップオシレータ停止
				device::SYSTEM::HOCOPCR.HOCOPCNT = 1;  ///< 高速オンチップオシレーター電源 OFF
			}

			// クロック関係書き込み不許可
			device::SYSTEM::PRCR = 0xA500;

#if defined(SIG_RX65N) || defined(SIG_RX66T) || defined(SIG_RX72M) || defined(SIG_RX72T) || defined(SIG_RX72N)
			// ROM キャッシュを有効（標準）
			device::SYSTEM::ROMCE = 1;
#endif
#if defined(__TFU)
			__init_tfu();
#endif
		}

#if defined(SIG_RX72M) || defined(SIG_RX72N)
		//-------------------------------------------------------------//
		/*!
			@brief  PPLL 制御を使って PHY 向け 25MHz を出力する。
			@return 成功なら「true」
		*/
		//-------------------------------------------------------------//
		static bool setup_phy25() noexcept
		{
			if(clock_profile::BASE != 16'000'000) {  // ベースクロックが 16MHz 以外は、生成不可とする。
				return false;
			}

			bool ret = true;
			device::SYSTEM::PRCR = 0xA50B;	// クロック、低消費電力、関係書き込み許可

			device::SYSTEM::PACKCR.OUTCKSEL = 1;

			// PPLIDIV: 1/2, PPLSTC: x12.5 (100MHz)
			device::SYSTEM::PPLLCR  = device::SYSTEM::PPLLCR.PPLIDIV.b(0b01)
								    | device::SYSTEM::PPLLCR.PPLSTC.b(0b011000);
			device::SYSTEM::PPLLCR2 = device::SYSTEM::PPLLCR2.PPLLEN.b(1);  // 発信許可

			// PPLL 安定待ち
			while(device::SYSTEM::OSCOVFSR.PPLOVF() == 0) { asm("nop"); }

			// PPLLCR3: 1/4 (25MHz)
			device::SYSTEM::PPLLCR3 = device::SYSTEM::PPLLCR3.PPLCK.b(0b0011);

			// クロック関係書き込み不許可
			device::SYSTEM::PRCR = 0xA500;

			// ポート・マッピングを行う必要があるが、あえて、ここでは許可しない。
			// ※このヘッダーに、port_map.hpp の依存を無くす為。
			// deveice::port_map::turn_CLKOUT25M();

			return true;
		}
#endif
	};

	//-------------------------------------------------------------//
	/*!
		@brief  ソフト・リセットの起動
	*/
	//-------------------------------------------------------------//
	inline void assert_soft_reset()
	{
		device::SYSTEM::PRCR = 0xA502;
		device::SYSTEM::SWRR = 0xA501;
		device::SYSTEM::PRCR = 0xA500;
	}

	//-------------------------------------------------------------//
	/*!
		@brief  MTU マスタークロック取得
		@return MTU マスタークロック
	*/
	//-------------------------------------------------------------//
	inline uint32_t get_mtu_master_clock() noexcept
	{
#if defined(SIG_RX66T) || defined(SIG_RX72T)
		return clock_profile::PCLKC;
#else
		return clock_profile::PCLKA;
#endif
	}
}
<commit_msg>Update: internal OSC<commit_after>#pragma once
//=====================================================================//
/*!	@file
	@brief	RX64M/RX71M/RX651/RX65N/RX66T/RX72T/RX72N/RX72M グループ・システム制御 @n
			※RX24T は構成が大きく異なるので、RX24T/system_io.hpp に分離しています。@n
			※ USB を使う場合：96MHz, 144MHz, 192MHz, 240MHz のいづれか @n
			RX72x 系では、内部 PLL 回路が追加され、Ethernet などに必要な 25MHz を得る為 @n
			16MHz を使います。@n
			(16MHz x 12.5 -> 200MHz, 16MHz x 15 -> 240MHz)
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2017, 2021 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/RX/blob/master/LICENSE
*/
//=====================================================================//
#include "RX600/system.hpp"

#if defined(SIG_RX64M) || defined(SIG_RX71M) || defined(SIG_RX66T) || defined(SIG_RX65N) || defined(SIG_RX72T) || defined(SIG_RX72N) || defined(SIG_RX72M)
#elif
#  error "system_io.hpp: Not available on RX24T"
#endif

namespace device {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  systen_base クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	struct system_base {

		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
		/*!
			@brief  発信器タイプ @n
					HOCO を使う場合、同時に、BASE_CLOCK_ に周波数（16,18,20 MHz）を設定します。
		*/
		//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
		enum class OSC_TYPE {
			XTAL,		///< クリスタル接続
			EXT,		///< クロック入力
			HOCO,		///< 高速オンチップオシレーター
			LOCO,		///< 低速オンチップオシレーター (240KHz)
		};
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief  systen_io クラス @n
				INTR_CLOCK は、内部 PLL で扱える最大速度です、@n
				外部クリスタルを微妙な周波数にする場合、その整数倍にしなければなりません。 @n
				RX71M はスーパーバイザモードでの変更が必要なので、「start.s」内で行う。 @n
				RX71M の場合、アセンブラにオプション「--defsym MEMWAIT=1」を渡す。
		@param[in]	OSC_TYPE	発信器タイプを設定（通常、XTAL）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <auto OSC_TYPE_ = system_base::OSC_TYPE::XTAL>
	struct system_io : public system_base {

		static uint8_t clock_div_(uint32_t clk) noexcept
		{
			uint8_t div = 0;
			while(clk < clock_profile::PLL_BASE) {
				++div;
				clk <<= 1;
			}
			if(div > 0b0110) div = 0b0110;
			return div;
		}

		//-------------------------------------------------------------//
		/*!
			@brief  マスター・クロックのブースト @n
					インストラクション・クロックを最大速度にブーストする。
		*/
		//-------------------------------------------------------------//
		static void boost_master_clock() noexcept
		{
			device::SYSTEM::PRCR = 0xA50B;	// クロック、低消費電力、関係書き込み許可

			device::SYSTEM::MOSCWTCR = 9;	// 1ms wait

			// メインクロック強制発振とドライブ能力設定
			if(OSC_TYPE_ == OSC_TYPE::XTAL) {
				uint8_t modrv2 = 0b11;
				if(clock_profile::BASE > 20'000'000) modrv2 = 0b00;
				else if(clock_profile::BASE > 16'000'000) modrv2 = 0b01;
				else if(clock_profile::BASE > 8'000'000) modrv2 = 0b10;
				device::SYSTEM::MOFCR = device::SYSTEM::MOFCR.MODRV2.b(modrv2);
				device::SYSTEM::MOSCCR.MOSTP = 0;		// メインクロック発振器動作
				while(device::SYSTEM::OSCOVFSR.MOOVF() == 0) { asm("nop"); }
			} else if(OSC_TYPE_ == OSC_TYPE::EXT) {
				device::SYSTEM::MOSCCR.MOSTP = 1;		// メインクロック発振器停止
				device::SYSTEM::MOFCR = device::SYSTEM::MOFCR.MOSEL.b();
			} else if(OSC_TYPE_ == OSC_TYPE::HOCO) {  // 高速オンチップオシレータ
				uint8_t frq;
				if(clock_profile::BASE == 16'000'000) frq = 0b00;
				else if(clock_profile::BASE == 18'000'000) frq = 0b01;
				else if(clock_profile::BASE == 20'000'000) frq = 0b10;
				else frq = 0b00;
				device::SYSTEM::HOCOCR2.HCFRQ = frq;
				device::SYSTEM::HOCOCR.HCSTP = 0;  // 動作
				while(device::SYSTEM::OSCOVFSR.HCOVF() == 0) { asm("nop"); }
				device::SYSTEM::PLLCR.PLLSRCSEL = 1;
			} else {
				device::SYSTEM::PRCR = 0xA500;
				return;
			}

#if defined(SIG_RX65N)
			if(clock_profile::ICLK >= 120'000'000) {  // 120MHz 以上の場合設定
				device::SYSTEM::ROMWT = 0b10;
			} else if(clock_profile::ICLK >= 100'000'000) {
				device::SYSTEM::ROMWT = 0b01;
			} else if(clock_profile::ICLK >= 50'000'000) {
				device::SYSTEM::ROMWT = 0b00;
			}
#endif
			// RX71M はスーパーバイザモードでの変更が必要なので、「start.s」内で行う。
			// RX71M の場合、アセンブラにオプション「--defsym MEMWAIT=1」を渡す。
#if defined(SIG_RX66T) || defined(SIG_RX72M) || defined(SIG_RX72T) || defined(SIG_RX72N)
			if(clock_profile::ICLK > 120'000'000) {  // 120MHz 以上の場合設定
				device::SYSTEM::MEMWAIT = 1;
				volatile auto tmp = device::SYSTEM::MEMWAIT();  // 読み出しを行う
			}
#endif
			// (x10.0) 0b010011, (x10.5) 0b010100, (x11.0) 0b010101, (x11.5) 0b010110
			// ... MAX x30.0
			uint32_t n = clock_profile::PLL_BASE * 2 / clock_profile::BASE;
			if(n < 20) n = 20;
			else if(n > 60) n = 60;
			n -= 20;
			device::SYSTEM::PLLCR.STC = n + 0b010011;  // base x10
			device::SYSTEM::PLLCR2.PLLEN = 0;			// PLL 動作
			while(device::SYSTEM::OSCOVFSR.PLOVF() == 0) { asm("nop"); }

			device::SYSTEM::SCKCR = device::SYSTEM::SCKCR.FCK.b(clock_div_(clock_profile::FCLK))
								  | device::SYSTEM::SCKCR.ICK.b(clock_div_(clock_profile::ICLK))
								  | device::SYSTEM::SCKCR.BCK.b(clock_div_(clock_profile::BCLK))
								  | device::SYSTEM::SCKCR.PCKA.b(clock_div_(clock_profile::PCLKA))
								  | device::SYSTEM::SCKCR.PCKB.b(clock_div_(clock_profile::PCLKB))
								  | device::SYSTEM::SCKCR.PCKC.b(clock_div_(clock_profile::PCLKC))
								  | device::SYSTEM::SCKCR.PCKD.b(clock_div_(clock_profile::PCLKD));
			{  // USB Master Clock の設定
				auto usb_div = clock_profile::PLL_BASE / 48'000'000;
				if(usb_div >= 2 && usb_div <= 5) {
					// 1/2, 1/3, 1/4, 1/5
					device::SYSTEM::SCKCR2.UCK = usb_div - 1;
				}
			}

			device::SYSTEM::SCKCR3.CKSEL = 0b100;   ///< PLL 選択

			if(OSC_TYPE_ == OSC_TYPE::XTAL || OSC_TYPE_ == OSC_TYPE::EXT) {
				device::SYSTEM::LOCOCR.LCSTP = 1;  ///< 低速オンチップオシレータ停止
				device::SYSTEM::HOCOCR.HCSTP = 1;  ///< 高速オンチップオシレータ停止
				device::SYSTEM::HOCOPCR.HOCOPCNT = 1;  ///< 高速オンチップオシレーター電源 OFF
			} else if(OSC_TYPE_ == OSC_TYPE::HOCO) {
				device::SYSTEM::LOCOCR.LCSTP = 1;  ///< 低速オンチップオシレータ停止
			}

			// クロック関係書き込み不許可
			device::SYSTEM::PRCR = 0xA500;

#if defined(SIG_RX65N) || defined(SIG_RX66T) || defined(SIG_RX72M) || defined(SIG_RX72T) || defined(SIG_RX72N)
			// ROM キャッシュを有効（標準）
			device::SYSTEM::ROMCE = 1;
#endif
#if defined(__TFU)
			__init_tfu();
#endif
		}

#if defined(SIG_RX72M) || defined(SIG_RX72N)
		//-------------------------------------------------------------//
		/*!
			@brief  PPLL 制御を使って PHY 向け 25MHz を出力する。
			@return 成功なら「true」
		*/
		//-------------------------------------------------------------//
		static bool setup_phy25() noexcept
		{
			if(clock_profile::BASE != 16'000'000) {  // ベースクロックが 16MHz 以外は、生成不可とする。
				return false;
			}

			bool ret = true;
			device::SYSTEM::PRCR = 0xA50B;	// クロック、低消費電力、関係書き込み許可

			device::SYSTEM::PACKCR.OUTCKSEL = 1;

			// PPLIDIV: 1/2, PPLSTC: x12.5 (100MHz)
			device::SYSTEM::PPLLCR  = device::SYSTEM::PPLLCR.PPLIDIV.b(0b01)
								    | device::SYSTEM::PPLLCR.PPLSTC.b(0b011000);
			device::SYSTEM::PPLLCR2 = device::SYSTEM::PPLLCR2.PPLLEN.b(1);  // 発信許可

			// PPLL 安定待ち
			while(device::SYSTEM::OSCOVFSR.PPLOVF() == 0) { asm("nop"); }

			// PPLLCR3: 1/4 (25MHz)
			device::SYSTEM::PPLLCR3 = device::SYSTEM::PPLLCR3.PPLCK.b(0b0011);

			// クロック関係書き込み不許可
			device::SYSTEM::PRCR = 0xA500;

			// ポート・マッピングを行う必要があるが、あえて、ここでは許可しない。
			// ※このヘッダーに、port_map.hpp の依存を無くす為。
			// deveice::port_map::turn_CLKOUT25M();

			return true;
		}
#endif
	};

	//-------------------------------------------------------------//
	/*!
		@brief  ソフト・リセットの起動
	*/
	//-------------------------------------------------------------//
	inline void assert_soft_reset()
	{
		device::SYSTEM::PRCR = 0xA502;
		device::SYSTEM::SWRR = 0xA501;
		device::SYSTEM::PRCR = 0xA500;
	}

	//-------------------------------------------------------------//
	/*!
		@brief  MTU マスタークロック取得
		@return MTU マスタークロック
	*/
	//-------------------------------------------------------------//
	inline uint32_t get_mtu_master_clock() noexcept
	{
#if defined(SIG_RX66T) || defined(SIG_RX72T)
		return clock_profile::PCLKC;
#else
		return clock_profile::PCLKA;
#endif
	}
}
<|endoftext|>


<commit_before>/**
 *  Extension.cpp
 *
 *  @author Emiel Bruijntjes <emiel.bruijntjes@copernica.com>
 *  @copyright 2013 Copernica BV
 */
#include "includes.h"

/**
 *  Set up namespace
 */
namespace Php {

/**
 *  If this extension is compiled for a PHP version with multi
 *  threading support, we need an additional header file
 */
#ifdef ZTS
#include "TSRM.h"
#endif

/**
 *  We're almost there, we now need to declare an instance of the
 *  structure defined above (if building for a single thread) or some
 *  sort of impossible to understand magic pointer-to-a-pointer (for
 *  multi-threading builds). We make this a static variable because
 *  this already is bad enough.
 */
ZEND_DECLARE_MODULE_GLOBALS(phpcpp)

/**
 *  Function that must be defined to initialize the "globals"
 *  We do not have to initialize anything, but PHP needs to call this
 *  method (crazy)
 *  @param  globals
 */
static void init_globals(zend_phpcpp_globals *globals) {}

/**
 *  The *startup() and *shutdown() callback functions are passed a module_number
 *  variable. However, there does not seem to be a decent API call in Zend to
 *  get back the original module_entry linked to this number. So we have to
 *  look up entries in a hash table to find the right module entry. To make things
 *  even worse, the records in this hash table are copies of the original
 *  zend_module_entry structure, so we can also not hide the C++ extension
 *  object pointer in the entry that we created ourselves.
 *
 *  We have an ugly solution, we keep track of a map of all C++ extension names
 *  and their associated extension object, and a map of all module number and
 *  the linked extension object.
 *
 *  @var map
 */
static std::map<std::string,ExtensionImpl*> name2extension;
static std::map<int,ExtensionImpl*> number2extension;

/**
 *  Handler function that is used in combination with zend_hash_apply()
 *
 *  This function is called when we need to find an extension object based on
 *  an extension number. We loop through the list of all registered modules, and
 *  for each module we check if we know the extension based on the name
 *
 *  @param  zend_module_entry
 */
static int match_module(zend_module_entry *entry)
{
    // check if there is an extension with this name
    auto iter = name2extension.find(entry->name);
    if (iter == name2extension.end()) return ZEND_HASH_APPLY_KEEP;

    // we have the extension, store in combination with the number
    number2extension[entry->module_number] = iter->second;

    // done
    return ZEND_HASH_APPLY_KEEP;
}

/**
 *  Find an extension based on the module number
 *  @param  number
 *  @param  tsrm_ls
 *  @return Extension*
 */
static ExtensionImpl *find(int number TSRMLS_DC)
{
    // do we already have an extension with this number?
    auto iter = number2extension.find(number);
    if (iter != number2extension.end()) return iter->second;

    // no, not yet, loop through all modules
    zend_hash_apply(&module_registry, (apply_func_t)match_module TSRMLS_CC);

    // find again
    iter = number2extension.find(number);
    if (iter == number2extension.end()) return nullptr;

    // found!
    return iter->second;
}

/**
 *  Function that is called when the extension initializes
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int         0 on success
 */
int ExtensionImpl::processStartup(int type, int module_number TSRMLS_DC)
{
    // initialize and allocate the "global" variables
    ZEND_INIT_MODULE_GLOBALS(phpcpp, init_globals, NULL);

    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // array contains ini settings
    _ini = new zend_ini_entry[extension->_data->iniVariables()+1];

    // the entry that we're filling
    int i=0;

    // Fill the php.ini entries
    extension->_data->iniVariables([this, &i, module_number](Ini &ini) {

        // initialize the function
        zend_ini_entry *entry = &_ini[i];

        // fill the property
        ini.fill(entry, module_number);

        // move on to the next iteration
        i++;
    });

    // last entry should be set to all zero's
    memset(&_ini[i], 0, sizeof(zend_ini_entry));

    // register ini entries in Zend core
    zend_register_ini_entries(_ini, module_number TSRMLS_CC);

    // initialize the extension
    extension->initialize(TSRMLS_C);

    // is the callback registered?
    if (extension->_onStartup) extension->_onStartup();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Function that is called when the extension is about to be stopped
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int
 */
int ExtensionImpl::processShutdown(int type, int module_number TSRMLS_DC)
{
    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // unregister the ini entries
    zend_unregister_ini_entries(module_number TSRMLS_CC);

    // is the callback registered?
    if (extension->_onShutdown) extension->_onShutdown();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Function that is called when a request starts
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int         0 on success
 */
int ExtensionImpl::processRequest(int type, int module_number TSRMLS_DC)
{
    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // is the callback registered?
    if (extension->_onRequest) extension->_onRequest();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Function that is called when a request is ended
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int         0 on success
 */
int ExtensionImpl::processIdle(int type, int module_number TSRMLS_DC)
{
    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // is the callback registered?
    if (extension->_onIdle) extension->_onIdle();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Constructor
 *  @param  data        Pointer to the extension object created by the extension programmer
 *  @param  name        Name of the extension
 *  @param  version     Version number
 */
ExtensionImpl::ExtensionImpl(Extension *data, const char *name, const char *version) : ExtensionBase(data)
{
    // keep extension pointer based on the name
    name2extension[name] = this;

    // assign all members (apart from the globals)
    _entry.size = sizeof(zend_module_entry);                       // size of the data
    _entry.zend_api = ZEND_MODULE_API_NO;                          // api number
    _entry.zend_debug = ZEND_DEBUG;                                // debug mode enabled?
    _entry.zts = USING_ZTS;                                        // is thread safety enabled?
    _entry.ini_entry = NULL;                                       // the php.ini record, will be filled by Zend engine
    _entry.deps = NULL;                                            // dependencies on other modules
    _entry.name = name;                                            // extension name
    _entry.functions = NULL;                                       // functions supported by this module (none for now)
    _entry.module_startup_func = &ExtensionImpl::processStartup;   // startup function for the whole extension
    _entry.module_shutdown_func = &ExtensionImpl::processShutdown; // shutdown function for the whole extension
    _entry.request_startup_func = &ExtensionImpl::processRequest;  // startup function per request
    _entry.request_shutdown_func = &ExtensionImpl::processIdle;    // shutdown function per request
    _entry.info_func = NULL;                                       // information for retrieving info
    _entry.version = version;                                      // version string
    _entry.globals_size = 0;                                       // size of the global variables
    _entry.globals_ctor = NULL;                                    // constructor for global variables
    _entry.globals_dtor = NULL;                                    // destructor for global variables
    _entry.post_deactivate_func = NULL;                            // unknown function
    _entry.module_started = 0;                                     // module is not yet started
    _entry.type = 0;                                               // temporary or persistent module, will be filled by Zend engine
    _entry.handle = NULL;                                          // dlopen() handle, will be filled by Zend engine
    _entry.module_number = 0;                                      // module number will be filled in by Zend engine
    _entry.build_id = (char *)ZEND_MODULE_BUILD_ID;                // check if extension and zend engine are compatible

    // things that only need to be initialized
#ifdef ZTS
    _entry.globals_id_ptr = NULL;
#else
    _entry.globals_ptr = NULL;
#endif

}

/**
 *  Destructor
 */
ExtensionImpl::~ExtensionImpl()
{
    // deallocate the php.ini entries
    if (_ini) delete[] _ini;

    // deallocate functions
    if (_entry.functions) delete[] _entry.functions;
}

/**
 *  Retrieve the module entry
 *  @return zend_module_entry
 */
zend_module_entry *ExtensionImpl::module()
{
    // check if functions we're already defined
    if (_entry.functions) return &_entry;

    // the number of functions
    int count = _data->functions();

    // skip if there are no functions
    if (count == 0) return &_entry;

    // allocate memory for the functions
    zend_function_entry *entries = new zend_function_entry[count + 1];

    // index being processed
    int i = 0;

    // apply a function to each function
    _data->functions([&i, entries](const std::string &prefix, Function &function) {

        // initialize the function
        function.initialize(prefix, &entries[i]);

        // move on to the next iteration
        i++;
    });

    // last entry should be set to all zeros
    zend_function_entry *last = &entries[count];

    // all should be set to zero
    memset(last, 0, sizeof(zend_function_entry));

    // store functions in entry object
    _entry.functions = entries;

    // return the entry
    return &_entry;
}

/**
 *  Initialize the extension after it was started
 *  @param  tsrm_ls
 */
void ExtensionImpl::initialize(TSRMLS_D)
{
    // we need to register each class, find out all classes
    _data->classes([TSRMLS_C](const std::string &prefix, ClassBase &c) {

        // forward to implementation class
        c.implementation()->initialize(&c, prefix TSRMLS_CC);
    });
}

/**
 *  End of namespace
 */
}

<commit_msg>fixed compile issue (issue #64)<commit_after>/**
 *  Extension.cpp
 *
 *  @author Emiel Bruijntjes <emiel.bruijntjes@copernica.com>
 *  @copyright 2013 Copernica BV
 */
#include "includes.h"

/**
 *  Set up namespace
 */
namespace Php {

/**
 *  If this extension is compiled for a PHP version with multi
 *  threading support, we need an additional header file
 */
#ifdef ZTS
#include "TSRM.h"
#endif

/**
 *  We're almost there, we now need to declare an instance of the
 *  structure defined above (if building for a single thread) or some
 *  sort of impossible to understand magic pointer-to-a-pointer (for
 *  multi-threading builds). We make this a static variable because
 *  this already is bad enough.
 */
ZEND_DECLARE_MODULE_GLOBALS(phpcpp)

/**
 *  Function that must be defined to initialize the "globals"
 *  We do not have to initialize anything, but PHP needs to call this
 *  method (crazy)
 *  @param  globals
 */
static void init_globals(zend_phpcpp_globals *globals) {}

/**
 *  The *startup() and *shutdown() callback functions are passed a module_number
 *  variable. However, there does not seem to be a decent API call in Zend to
 *  get back the original module_entry linked to this number. So we have to
 *  look up entries in a hash table to find the right module entry. To make things
 *  even worse, the records in this hash table are copies of the original
 *  zend_module_entry structure, so we can also not hide the C++ extension
 *  object pointer in the entry that we created ourselves.
 *
 *  We have an ugly solution, we keep track of a map of all C++ extension names
 *  and their associated extension object, and a map of all module number and
 *  the linked extension object.
 *
 *  @var map
 */
static std::map<std::string,ExtensionImpl*> name2extension;
static std::map<int,ExtensionImpl*> number2extension;

/**
 *  Handler function that is used in combination with zend_hash_apply()
 *
 *  This function is called when we need to find an extension object based on
 *  an extension number. We loop through the list of all registered modules, and
 *  for each module we check if we know the extension based on the name
 *
 *  @param  zend_module_entry
 */
static int match_module(zend_module_entry *entry)
{
    // check if there is an extension with this name
    auto iter = name2extension.find(entry->name);
    if (iter == name2extension.end()) return ZEND_HASH_APPLY_KEEP;

    // we have the extension, store in combination with the number
    number2extension[entry->module_number] = iter->second;

    // done
    return ZEND_HASH_APPLY_KEEP;
}

/**
 *  Find an extension based on the module number
 *  @param  number
 *  @param  tsrm_ls
 *  @return Extension*
 */
static ExtensionImpl *find(int number TSRMLS_DC)
{
    // do we already have an extension with this number?
    auto iter = number2extension.find(number);
    if (iter != number2extension.end()) return iter->second;

    // no, not yet, loop through all modules
    zend_hash_apply(&module_registry, (apply_func_t)match_module TSRMLS_CC);

    // find again
    iter = number2extension.find(number);
    if (iter == number2extension.end()) return nullptr;

    // found!
    return iter->second;
}

/**
 *  Function that is called when the extension initializes
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int         0 on success
 */
int ExtensionImpl::processStartup(int type, int module_number TSRMLS_DC)
{
    // initialize and allocate the "global" variables
    ZEND_INIT_MODULE_GLOBALS(phpcpp, init_globals, NULL);

    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // array contains ini settings
    auto *entries = extension->_ini = new zend_ini_entry[extension->_data->iniVariables()+1];

    // the entry that we're filling
    int i=0;

    // Fill the php.ini entries
    extension->_data->iniVariables([entries, &i, module_number](Ini &ini) {

        // initialize the function
        zend_ini_entry *entry = &entries[i];

        // fill the property
        ini.fill(entry, module_number);

        // move on to the next iteration
        i++;
    });

    // last entry should be set to all zero's
    memset(&entries[i], 0, sizeof(zend_ini_entry));

    // register ini entries in Zend core
    zend_register_ini_entries(entries, module_number TSRMLS_CC);

    // initialize the extension
    extension->initialize(TSRMLS_C);

    // is the callback registered?
    if (extension->_onStartup) extension->_onStartup();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Function that is called when the extension is about to be stopped
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int
 */
int ExtensionImpl::processShutdown(int type, int module_number TSRMLS_DC)
{
    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // unregister the ini entries
    zend_unregister_ini_entries(module_number TSRMLS_CC);

    // is the callback registered?
    if (extension->_onShutdown) extension->_onShutdown();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Function that is called when a request starts
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int         0 on success
 */
int ExtensionImpl::processRequest(int type, int module_number TSRMLS_DC)
{
    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // is the callback registered?
    if (extension->_onRequest) extension->_onRequest();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Function that is called when a request is ended
 *  @param  type        Module type
 *  @param  number      Module number
 *  @param  tsrm_ls
 *  @return int         0 on success
 */
int ExtensionImpl::processIdle(int type, int module_number TSRMLS_DC)
{
    // get the extension
    auto *extension = find(module_number TSRMLS_CC);

    // is the callback registered?
    if (extension->_onIdle) extension->_onIdle();

    // done
    return BOOL2SUCCESS(true);
}

/**
 *  Constructor
 *  @param  data        Pointer to the extension object created by the extension programmer
 *  @param  name        Name of the extension
 *  @param  version     Version number
 */
ExtensionImpl::ExtensionImpl(Extension *data, const char *name, const char *version) : ExtensionBase(data)
{
    // keep extension pointer based on the name
    name2extension[name] = this;

    // assign all members (apart from the globals)
    _entry.size = sizeof(zend_module_entry);                       // size of the data
    _entry.zend_api = ZEND_MODULE_API_NO;                          // api number
    _entry.zend_debug = ZEND_DEBUG;                                // debug mode enabled?
    _entry.zts = USING_ZTS;                                        // is thread safety enabled?
    _entry.ini_entry = NULL;                                       // the php.ini record, will be filled by Zend engine
    _entry.deps = NULL;                                            // dependencies on other modules
    _entry.name = name;                                            // extension name
    _entry.functions = NULL;                                       // functions supported by this module (none for now)
    _entry.module_startup_func = &ExtensionImpl::processStartup;   // startup function for the whole extension
    _entry.module_shutdown_func = &ExtensionImpl::processShutdown; // shutdown function for the whole extension
    _entry.request_startup_func = &ExtensionImpl::processRequest;  // startup function per request
    _entry.request_shutdown_func = &ExtensionImpl::processIdle;    // shutdown function per request
    _entry.info_func = NULL;                                       // information for retrieving info
    _entry.version = version;                                      // version string
    _entry.globals_size = 0;                                       // size of the global variables
    _entry.globals_ctor = NULL;                                    // constructor for global variables
    _entry.globals_dtor = NULL;                                    // destructor for global variables
    _entry.post_deactivate_func = NULL;                            // unknown function
    _entry.module_started = 0;                                     // module is not yet started
    _entry.type = 0;                                               // temporary or persistent module, will be filled by Zend engine
    _entry.handle = NULL;                                          // dlopen() handle, will be filled by Zend engine
    _entry.module_number = 0;                                      // module number will be filled in by Zend engine
    _entry.build_id = (char *)ZEND_MODULE_BUILD_ID;                // check if extension and zend engine are compatible

    // things that only need to be initialized
#ifdef ZTS
    _entry.globals_id_ptr = NULL;
#else
    _entry.globals_ptr = NULL;
#endif

}

/**
 *  Destructor
 */
ExtensionImpl::~ExtensionImpl()
{
    // deallocate the php.ini entries
    if (_ini) delete[] _ini;

    // deallocate functions
    if (_entry.functions) delete[] _entry.functions;
}

/**
 *  Retrieve the module entry
 *  @return zend_module_entry
 */
zend_module_entry *ExtensionImpl::module()
{
    // check if functions we're already defined
    if (_entry.functions) return &_entry;

    // the number of functions
    int count = _data->functions();

    // skip if there are no functions
    if (count == 0) return &_entry;

    // allocate memory for the functions
    zend_function_entry *entries = new zend_function_entry[count + 1];

    // index being processed
    int i = 0;

    // apply a function to each function
    _data->functions([&i, entries](const std::string &prefix, Function &function) {

        // initialize the function
        function.initialize(prefix, &entries[i]);

        // move on to the next iteration
        i++;
    });

    // last entry should be set to all zeros
    zend_function_entry *last = &entries[count];

    // all should be set to zero
    memset(last, 0, sizeof(zend_function_entry));

    // store functions in entry object
    _entry.functions = entries;

    // return the entry
    return &_entry;
}

/**
 *  Initialize the extension after it was started
 *  @param  tsrm_ls
 */
void ExtensionImpl::initialize(TSRMLS_D)
{
    // we need to register each class, find out all classes
    _data->classes([TSRMLS_C](const std::string &prefix, ClassBase &c) {

        // forward to implementation class
        c.implementation()->initialize(&c, prefix TSRMLS_CC);
    });
}

/**
 *  End of namespace
 */
}

<|endoftext|>


<commit_before>#include "QuaternionFloatingJoint.h"
#include <random>
#include "drakeGeometryUtil.h"

using namespace Eigen;
using namespace std;

QuaternionFloatingJoint::QuaternionFloatingJoint(const string& name, const Isometry3d& transform_to_parent_body) :
  DrakeJoint(name, transform_to_parent_body, 7, 6)
{
  // empty
}

QuaternionFloatingJoint::~QuaternionFloatingJoint()
{
  // empty
}

Isometry3d QuaternionFloatingJoint::jointTransform(const Eigen::Ref<const VectorXd>& q) const
{
  Isometry3d ret(Quaterniond(q[3], q[4], q[5], q[6]));
  ret.translation() << q[0], q[1], q[2];
  ret.makeAffine();
  return ret;
}

void QuaternionFloatingJoint::motionSubspace(const Eigen::Ref<const VectorXd>& q, MotionSubspaceType& motion_subspace, MatrixXd* dmotion_subspace) const
{
  motion_subspace.setIdentity(TWIST_SIZE, getNumVelocities());
  if (dmotion_subspace) {
    dmotion_subspace->setZero(motion_subspace.size(), getNumPositions());
  }
}

void QuaternionFloatingJoint::motionSubspaceDotTimesV(const Eigen::Ref<const VectorXd>& q, const Eigen::Ref<const VectorXd>& v,
    Vector6d& motion_subspace_dot_times_v,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdq,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdv) const
{
  motion_subspace_dot_times_v.setZero();
  if (dmotion_subspace_dot_times_vdq) {
    dmotion_subspace_dot_times_vdq->setZero(motion_subspace_dot_times_v.size(), getNumPositions());
  }
  if (dmotion_subspace_dot_times_vdv) {
    dmotion_subspace_dot_times_vdv->setZero(motion_subspace_dot_times_v.size(), getNumVelocities());
  }
}

void QuaternionFloatingJoint::randomConfiguration(Eigen::Ref<VectorXd>& q, std::default_random_engine& generator) const
{
  normal_distribution<double> normal;

  // position
  q[0] = normal(generator);
  q[1] = normal(generator);
  q[2] = normal(generator);

  // orientation
  Vector4d quat = uniformlyRandomQuat(generator);
  q[3] = quat(0);
  q[4] = quat(1);
  q[5] = quat(2);
  q[6] = quat(3);
}

void QuaternionFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.resize(getNumVelocities(), getNumPositions());

  auto quat = q.template middleRows<QUAT_SIZE>(SPACE_DIMENSION);
  Matrix3d R = quat2rotmat(quat);

  Vector4d quattilde;
  Matrix<double, SPACE_DIMENSION, QUAT_SIZE> M;
  Matrix<double, SPACE_DIMENSION, QUAT_SIZE> RTransposeM;
  Gradient<Vector4d, QUAT_SIZE, 1>::type dquattildedquat;
  if (dqdot_to_v) {
    Gradient<Vector4d, QUAT_SIZE, 2>::type ddquattildedquat;
    normalizeVec(quat, quattilde, &dquattildedquat, &ddquattildedquat);
    auto dR = dquat2rotmat(quat);
    Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type dM;
    quatdot2angularvelMatrix(quat, M, &dM);

    RTransposeM.noalias() = R.transpose() * M;
    auto dRTranspose = transposeGrad(dR, R.rows());
    auto dRTransposeM = matGradMultMat(R.transpose(), M, dRTranspose, dM);
    auto dRTransposeMdquattildedquat = matGradMultMat(RTransposeM, dquattildedquat, dRTransposeM, ddquattildedquat);
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
    setSubMatrixGradient<4>(*dqdot_to_v, dRTranspose, intRange<3>(3), intRange<3>(0), qdot_to_v.rows(), 3);
    setSubMatrixGradient<4>(*dqdot_to_v, dRTransposeMdquattildedquat, intRange<3>(0), intRange<4>(3), qdot_to_v.rows(), 3);
  }
  else {
    normalizeVec(quat, quattilde, &dquattildedquat);
    quatdot2angularvelMatrix(quat, M);
    RTransposeM.noalias() = R.transpose() * M;
  }
  qdot_to_v.block<3, 3>(0, 0).setZero();
  qdot_to_v.block<3, 4>(0, 3).noalias() = RTransposeM * dquattildedquat;
  qdot_to_v.block<3, 3>(3, 0) = R.transpose();
  qdot_to_v.block<3, 4>(3, 3).setZero();
}

void QuaternionFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.resize(getNumPositions(), getNumVelocities());

  auto quat = q.template middleRows<QUAT_SIZE>(SPACE_DIMENSION);
  Matrix3d R = quat2rotmat(quat);

  Matrix<double, QUAT_SIZE, SPACE_DIMENSION> M;
  if (dv_to_qdot) {
    auto dR = dquat2rotmat(quat);
    Gradient<decltype(M), QUAT_SIZE, 1>::type dM;
    angularvel2quatdotMatrix(quat, M, &dM);

    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());

    setSubMatrixGradient<4>(*dv_to_qdot, dR, intRange<3>(0), intRange<3>(3), v_to_qdot.rows(), 3);
    auto dMR = matGradMultMat(M, R, dM, dR);
    setSubMatrixGradient<4>(*dv_to_qdot, dMR, intRange<4>(3), intRange<3>(0), v_to_qdot.rows(), 3);
  }
  else {
    angularvel2quatdotMatrix(quat, M, (Gradient<decltype(M), QUAT_SIZE, 1>::type*) nullptr);
  }

  v_to_qdot.block<3, 3>(0, 0).setZero();
  v_to_qdot.block<3, 3>(0, 3) = R;
  v_to_qdot.block<4, 3>(3, 0).noalias() = M * R;
  v_to_qdot.block<4, 3>(3, 3).setZero();
}
<commit_msg>Removed superfluous template disambiguators.<commit_after>#include "QuaternionFloatingJoint.h"
#include <random>
#include "drakeGeometryUtil.h"

using namespace Eigen;
using namespace std;

QuaternionFloatingJoint::QuaternionFloatingJoint(const string& name, const Isometry3d& transform_to_parent_body) :
  DrakeJoint(name, transform_to_parent_body, 7, 6)
{
  // empty
}

QuaternionFloatingJoint::~QuaternionFloatingJoint()
{
  // empty
}

Isometry3d QuaternionFloatingJoint::jointTransform(const Eigen::Ref<const VectorXd>& q) const
{
  Isometry3d ret(Quaterniond(q[3], q[4], q[5], q[6]));
  ret.translation() << q[0], q[1], q[2];
  ret.makeAffine();
  return ret;
}

void QuaternionFloatingJoint::motionSubspace(const Eigen::Ref<const VectorXd>& q, MotionSubspaceType& motion_subspace, MatrixXd* dmotion_subspace) const
{
  motion_subspace.setIdentity(TWIST_SIZE, getNumVelocities());
  if (dmotion_subspace) {
    dmotion_subspace->setZero(motion_subspace.size(), getNumPositions());
  }
}

void QuaternionFloatingJoint::motionSubspaceDotTimesV(const Eigen::Ref<const VectorXd>& q, const Eigen::Ref<const VectorXd>& v,
    Vector6d& motion_subspace_dot_times_v,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdq,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdv) const
{
  motion_subspace_dot_times_v.setZero();
  if (dmotion_subspace_dot_times_vdq) {
    dmotion_subspace_dot_times_vdq->setZero(motion_subspace_dot_times_v.size(), getNumPositions());
  }
  if (dmotion_subspace_dot_times_vdv) {
    dmotion_subspace_dot_times_vdv->setZero(motion_subspace_dot_times_v.size(), getNumVelocities());
  }
}

void QuaternionFloatingJoint::randomConfiguration(Eigen::Ref<VectorXd>& q, std::default_random_engine& generator) const
{
  normal_distribution<double> normal;

  // position
  q[0] = normal(generator);
  q[1] = normal(generator);
  q[2] = normal(generator);

  // orientation
  Vector4d quat = uniformlyRandomQuat(generator);
  q[3] = quat(0);
  q[4] = quat(1);
  q[5] = quat(2);
  q[6] = quat(3);
}

void QuaternionFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.resize(getNumVelocities(), getNumPositions());

  auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
  Matrix3d R = quat2rotmat(quat);

  Vector4d quattilde;
  Matrix<double, SPACE_DIMENSION, QUAT_SIZE> M;
  Matrix<double, SPACE_DIMENSION, QUAT_SIZE> RTransposeM;
  Gradient<Vector4d, QUAT_SIZE, 1>::type dquattildedquat;
  if (dqdot_to_v) {
    Gradient<Vector4d, QUAT_SIZE, 2>::type ddquattildedquat;
    normalizeVec(quat, quattilde, &dquattildedquat, &ddquattildedquat);
    auto dR = dquat2rotmat(quat);
    Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type dM;
    quatdot2angularvelMatrix(quat, M, &dM);

    RTransposeM.noalias() = R.transpose() * M;
    auto dRTranspose = transposeGrad(dR, R.rows());
    auto dRTransposeM = matGradMultMat(R.transpose(), M, dRTranspose, dM);
    auto dRTransposeMdquattildedquat = matGradMultMat(RTransposeM, dquattildedquat, dRTransposeM, ddquattildedquat);
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
    setSubMatrixGradient<4>(*dqdot_to_v, dRTranspose, intRange<3>(3), intRange<3>(0), qdot_to_v.rows(), 3);
    setSubMatrixGradient<4>(*dqdot_to_v, dRTransposeMdquattildedquat, intRange<3>(0), intRange<4>(3), qdot_to_v.rows(), 3);
  }
  else {
    normalizeVec(quat, quattilde, &dquattildedquat);
    quatdot2angularvelMatrix(quat, M);
    RTransposeM.noalias() = R.transpose() * M;
  }
  qdot_to_v.block<3, 3>(0, 0).setZero();
  qdot_to_v.block<3, 4>(0, 3).noalias() = RTransposeM * dquattildedquat;
  qdot_to_v.block<3, 3>(3, 0) = R.transpose();
  qdot_to_v.block<3, 4>(3, 3).setZero();
}

void QuaternionFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.resize(getNumPositions(), getNumVelocities());

  auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
  Matrix3d R = quat2rotmat(quat);

  Matrix<double, QUAT_SIZE, SPACE_DIMENSION> M;
  if (dv_to_qdot) {
    auto dR = dquat2rotmat(quat);
    Gradient<decltype(M), QUAT_SIZE, 1>::type dM;
    angularvel2quatdotMatrix(quat, M, &dM);

    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());

    setSubMatrixGradient<4>(*dv_to_qdot, dR, intRange<3>(0), intRange<3>(3), v_to_qdot.rows(), 3);
    auto dMR = matGradMultMat(M, R, dM, dR);
    setSubMatrixGradient<4>(*dv_to_qdot, dMR, intRange<4>(3), intRange<3>(0), v_to_qdot.rows(), 3);
  }
  else {
    angularvel2quatdotMatrix(quat, M, (Gradient<decltype(M), QUAT_SIZE, 1>::type*) nullptr);
  }

  v_to_qdot.block<3, 3>(0, 0).setZero();
  v_to_qdot.block<3, 3>(0, 3) = R;
  v_to_qdot.block<4, 3>(3, 0).noalias() = M * R;
  v_to_qdot.block<4, 3>(3, 3).setZero();
}
<|endoftext|>