text:
#include <fstream>
#include <sstream>
#include <iostream>

#include "read.h"
#include "write.h"
#include "util.h"
#include "matrixgen.h"
#include "mathfns.h"

#include <iomanip>

#ifdef ARMA
#include <armadillo>
#endif // ARMA

void writeSamples(char const* sample_name, char const* trial_name, int vDim, std::vector<SAMPLE_DIMS> sample_dims, sample_fn fn) {

	if (!manage_folder_structure(SAMPLE_PATH, sample_name, trial_name))
		return;

	std::ofstream myfile;
	std::string post_file_name1, post_file_name2, file_name1, file_name2;

	char* file_extension1 = "1.csv", * file_extension2 = "2.csv";
	post_file_name1 = create_file_name(sample_name, trial_name, file_extension1);
	post_file_name2 = create_file_name(sample_name, trial_name, file_extension2);

	int thread_ID = 0;

	for (int s = 0; s < sample_dims.size(); s++) {

		std::stringstream ss1, ss2;
		ss1 << s + 1 << post_file_name1;
		file_name1 = ss1.str();
		ss2 << s + 1 << post_file_name2;
		file_name2 = ss2.str();

#ifdef MKL
		std::unique_ptr<double[]> ns1 = std::make_unique<double[]>(sample_dims[s].m1 * vDim);
		std::unique_ptr<double[]> ns2 = std::make_unique<double[]>(sample_dims[s].m2 * vDim);

		nRandomVectorsCreateMKL(sample_dims[s].m1, vDim, ns1.get(), fn, thread_ID);
		nRandomVectorsCreateMKL(sample_dims[s].m2, vDim, ns2.get(), fn, thread_ID);
#endif // MKL
#ifdef ARMA
		dmat ns1 = nRandomVectorsCreate(nsamples_ vDim, unifSampleFromDSphere);
		dmat ns2 = nRandomVectorsCreate(nsamples_, vDim, unifSampleFromDSphere);
#endif // ARMA

		int prec = std::numeric_limits<double>::digits10 + 2;
		/* Write First Dataset */
		if (!myfile.is_open()) {
			myfile.open(file_name1);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m1; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name1 << std::endl;

		/* Write Second Dataset*/
		if (!myfile.is_open()) {
			myfile.open(file_name2);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m2; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name2 << "\n" << std::endl;
	}
}

void writeSamples(char const* sample_name, char const* trial_name, int vDim, std::vector<SAMPLE_DIMS> sample_dims, sample_fn fn, double scale) {

	if (!manage_folder_structure(SAMPLE_PATH, sample_name, trial_name))
		return;

	std::ofstream myfile;
	std::string post_file_name1, post_file_name2, file_name1, file_name2;

	char* file_extension1 = "1.csv", * file_extension2 = "2.csv";
	post_file_name1 = create_file_name(sample_name, trial_name, file_extension1);
	post_file_name2 = create_file_name(sample_name, trial_name, file_extension2);

	int thread_ID = 0;

	for (int s = 0; s < sample_dims.size(); s++) {

		std::stringstream ss1, ss2;
		ss1 << s + 1 << post_file_name1;
		file_name1 = ss1.str();
		ss2 << s + 1 << post_file_name2;
		file_name2 = ss2.str();

#ifdef MKL
		std::unique_ptr<double[]> ns1 = std::make_unique<double[]>(sample_dims[s].m1 * vDim);
		std::unique_ptr<double[]> ns2 = std::make_unique<double[]>(sample_dims[s].m2 * vDim);

		nRandomVectorsCreateMKL(sample_dims[s].m1, vDim, ns1.get(), fn, thread_ID);
		nRandomVectorsCreateMKL(sample_dims[s].m2, vDim, ns2.get(), fn, thread_ID);
#endif // MKL
#ifdef ARMA
		dmat ns1 = nRandomVectorsCreate(nsamples_ vDim, unifSampleFromDSphere);
		dmat ns2 = nRandomVectorsCreate(nsamples_, vDim, unifSampleFromDSphere);
#endif // ARMA

		int prec = std::numeric_limits<double>::digits10 + 2;
		/* Write First Dataset */
		if (!myfile.is_open()) {
			myfile.open(file_name1);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m1; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name1 << std::endl;

		/* Write Second Dataset*/
		if (!myfile.is_open()) {
			myfile.open(file_name2);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m2; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name2 << "\n" << std::endl;
	}
}

void writeNLVMs(char const* nlvm_name, char const* Exn, char const* sample_name, char const* trial_name, std::vector<NLVM_DIMS> nlvm_dims,
	nlvmf fn) {

	std::ofstream myfile;
	std::string post_rfile_name1, rfile_name1, post_rfile_name2, rfile_name2, post_wfile_name, wfile_name;

	char* rfile_extension1 = "1.csv", * rfile_extension2 = "2.csv", * wfile_extension = ".csv";

	int thread_ID = 0;

	for (int s = 0; s < nlvm_dims.size(); s++) {

		post_rfile_name1 = create_file_name(sample_name, trial_name, rfile_extension1);
		post_rfile_name2 = create_file_name(sample_name, trial_name, rfile_extension2);

		std::stringstream ss1, ss2;
		ss1 << s + 1 << post_rfile_name1;
		rfile_name1 = ss1.str();
		ss2 << s + 1 << post_rfile_name2;
		rfile_name2 = ss2.str();

#ifdef MKL
		std::unique_ptr<double[]> nlvm_mat = std::make_unique<double[]>(nlvm_dims[s].m * nlvm_dims[s].n);
#ifdef EXTERN
		nlvm(nlvm_dims[s].m, nlvm_dims[s].n, nlvm_mat.get(), fn, sample_name, trial_name, rfile_name1.c_str(), rfile_name2.c_str(), thread_ID);
#endif // EXTERN
#endif // MKL

#ifdef ARMA
#endif // ARMA

		post_wfile_name = create_file_name(Exn, nlvm_name, sample_name, trial_name, wfile_extension);

		std::stringstream ss;
		ss << s + 1 << post_wfile_name;
		wfile_name = ss.str();

		int prec = std::numeric_limits<double>::digits10 + 2;
		if (!manage_folder_structure(EXAMPLE_PATH, Exn, trial_name))
			return;

		/* Write NLVM Matrix */
		if (!myfile.is_open()) {
			myfile.open(wfile_name);
		}
		std::cout << "Start writing " << wfile_name << std::endl;

		for (int i = 0; i < nlvm_dims[s].m; i++)
		{
			for (int j = 0; j < nlvm_dims[s].n; j++)
			{
				if (j < nlvm_dims[s].n - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat.get()[i * nlvm_dims[s].n + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat.get()[i * nlvm_dims[s].n + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << wfile_name << std::endl;
	}
}

void writeResults(char const* nlvm_name, char const* Exn, char const* result_name, char const* sample_name, char const* trial_name,
	std::vector<NLVM_DIMS> nlvm_dims) {

	std::ofstream myfile;
	std::string rfile_name, post_rfile_name, wfile_name;

	char* file_extension = ".csv";

	for (int s = 0; s < nlvm_dims.size(); s++)
	{
		std::unique_ptr<double[]> NLVM = std::make_unique<double[]>(nlvm_dims[s].m * nlvm_dims[s].n);
		std::unique_ptr<double[]> NLVM_CMPR = std::make_unique<double[]>(nlvm_dims[s].m * nlvm_dims[s].n);

		post_rfile_name = create_file_name(Exn, nlvm_name, sample_name, trial_name, file_extension);

		std::stringstream ss;
		ss << s + 1 << post_rfile_name;
		rfile_name = ss.str();

		if (changeFolder(EX, sample_name, trial_name,Exn) < 0)
			return;
		if (!check_if_file_exists(rfile_name.c_str()))
			return;

		std::string dummy =  get_current_dir();

		int thread_ID = 0;
		std::cout << "Start reading " << rfile_name << std::endl;
		std::vector<std::vector<double>> nlvm_vecvec{ readCSV(rfile_name.c_str(),thread_ID) };
		dVecVecToCArr(nlvm_vecvec, NLVM.get(), thread_ID);
		nlvm_vecvec.swap(std::vector<std::vector<double>>()); // ?
		std::cout << "Reading complete" << std::endl;

		//dmat armaX0 = vecVecToArmaMat(X);

		if (s < 1)
		{
			wfile_name = create_file_name(Exn, nlvm_name, result_name, sample_name, trial_name, file_extension);

			if (!manage_folder_structure(RESULTS_PATH, Exn, sample_name))
				return;
			if (check_if_file_exists(wfile_name.c_str()))
				return;
		}

		std::cout << "Start analyzing and writing " << std::endl;
		if (!myfile.is_open()) {
			myfile.open(wfile_name);
			myfile << "nrows,ncols,eps,epsRank,flag\n";
		}

		unsigned int epsRank;
		int no_conv_flag = 0;
		double tol = 0.001;
		std::vector<float> epsVec = { 0.2, 0.1, 0.07, 0.03, 0.01, 0.001 }; //SED
		epsVec = { 0.1, 0.07, 0.03, 0.01, 0.005, 0.001 }; //LINEARK
		epsVec = { 0.03, 0.01, 0.007, 0.005,0.0025, 0.001 }; //POLYNOMIALK d=2
		epsVec = { 0.25, 0.15, 0.05,0.1, 0.01,0.001 }; //COSINEK
		epsVec = { 0.1, 0.06, 0.03, 0.02, 0.01, 0.005}; ; //POLYNOMIALK d=3
		epsVec = {150000,100000,50000,25000, 10000, 1000}; //CHISQRK
		epsVec = { 2, 1, 0.5, 0.3, 0.17, 0.03 }; //L1LOSSMKL
		//epsVec = { 1.3, 1.0, 0.7, 0.4, 0.2, 0.05 }; // 01LOSSMKL

		for (auto eps : epsVec)
		{
			std::memcpy(NLVM_CMPR.get(), NLVM.get(), nlvm_dims[s].m * nlvm_dims[s].n * sizeof(double));
			epsRank = getEpsRank(nlvm_dims[s].m, nlvm_dims[s].n, NLVM_CMPR.get(), eps, tol, no_conv_flag);
			myfile << nlvm_dims[s].m << "," << nlvm_dims[s].n << "," << eps << "," << epsRank << "," << no_conv_flag << "\n";
		}
	}
	myfile.close();
}

ext: cc


text:
/**
 * \ file FixedDelayLineFilter.cpp
 */

#include <iostream>

#include <ATK/Delay/FixedDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_line100_delay_1_test )
{
  ATK::FixedDelayLineFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_delay(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_line100_delay_100_test )
{
  ATK::FixedDelayLineFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_delay(100), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_sinus_line100_delay50_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::FixedDelayLineFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(1);
  output.process(49);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(FixedDelayLineFilter_sinus_line100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FixedDelayLineFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(FixedDelayLineFilter_sinus_line1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FixedDelayLineFilter<double> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(FixedDelayLineFilter_sinus_delay_test )
{
  ATK::FixedDelayLineFilter<double> filter(128);
  filter.set_delay(10);
  BOOST_CHECK_EQUAL(filter.get_delay(), 10);
}

ext: cpp

max_stars_repo_name: D-J-Roberts/AudioTK

max_issues_repo_name: D-J-Roberts/AudioTK

max_forks_repo_name: D-J-Roberts/AudioTK


text:
// Copyright 2002 - 2008, 2010, 2011 National Technology Engineering
// Solutions of Sandia, LLC (NTESS). Under the terms of Contract
// DE-NA0003525 with NTESS, the U.S. Government retains certain rights
// in this software.
//
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#include <exception>
#include <fstream>
#include <set>
#include <typeinfo>

#if defined( STK_HAS_MPI )
#include <mpi.h>
#endif

#include <adapt/SerializeNodeRegistry.hpp>
#include <adapt/RefinerUtil.hpp>
#include <adapt/Refiner.hpp>
#include <adapt/NodeRegistry.hpp>
#include <percept/PEnums.hpp>

#include <boost/unordered_set.hpp>
#include <stk_mesh/base/MeshUtils.hpp>

namespace percept {

  using namespace std;
  using namespace percept;

#define EXTRA_PRINT_UR_GETBLOCKS 0

  static std::string strip_mult(const std::string& inp)
  {
    size_t pc = inp.find(":");
    if (pc == std::string::npos)
      return inp;
    else
      return inp.substr(0,pc);
  }

  BlockNamesType RefinerUtil::getBlockNames(const std::string& block_name_0, unsigned proc_rank, percept::PerceptMesh& eMesh, const std::string& geomFile)
  {
    std::string block_name = block_name_0;

    BlockNamesType blocks(percept::EntityRankEnd+1u);
    if (block_name.length() == 0)
      return blocks;

    if (block_name.substr(0, 5) == "file:")
      {
        //if (1) throw std::runtime_error("file: option Not implemented");
        std::string fileName = block_name.substr(5, block_name.length()-5);
        for (int iter=0; iter < 1000; ++iter)
          {
            if (fileName[0] == ' ')
              fileName = fileName.substr(1, fileName.length()-1);
            else
              break;
          }
        if (eMesh.get_rank() == 0)
          {
            std::cout << "block_names processing, found 'file:', will read from file = " << fileName << std::endl;
          }
        std::ifstream file(fileName.c_str());
        int line=0;
        block_name = "";
        std::string str_line;
        while(std::getline(file, str_line))
          {
            std::string block = str_line;
            if (eMesh.get_rank() == 0)
              std::cout << "file: line= " << line << " block= " << block << std::endl;
            if (block[0] != '#' && block.length())
              {
                if (line) block_name += ",";
                block_name += block;
                ++line;
              }
          }
        if (eMesh.get_rank() == 0)
          {
            std::cout << "block_names processing, after file read string is: " << block_name << std::endl;
          }
      }

    if (1)
      {
        std::string names = block_name;

        // pre-process to look for ".." range indicator

        std::string new_names = names;
        new_names = "";
        while(1)
          {
            if (!names.length())
              break;
            size_t ipos = names.find(',');
            bool last_one =  (ipos == std::string::npos);

            {
              std::string n1 = (last_one ? names : names.substr(0, ipos) );
              bool inc = true;
              //bool exc = false;
              if ('-' == n1[0])
                {
                  //exc = true;
                  inc = false;
                }
              else if ('+' == n1[0])
                {
                }
              else
                {
                  n1 = "+" + n1;
                }
              std::string plus_or_minus = (inc?"+":"-");
              std::string n2 = n1.substr(1, n1.length()-1);
              std::string id_string_start = "";
              std::string id_string_end = "";
              // leave open the possibility for other identifiers for range
              std::string dotdot = "..";
              int dotdot_len = dotdot.length();
              size_t pos_dotdot = n1.find(dotdot);
              if (pos_dotdot != std::string::npos)
                {
                  if (EXTRA_PRINT_UR_GETBLOCKS) std::cout << "tmp with .., n1= " << n1 << " n2= " << n2 << std::endl;

                  if (n1.length() > 6 && n1.substr(1,6) == "block_")
                    {
                      // +block_123..block_125
                      // 0123456789^1234567890
                      id_string_start = n1.substr(7, pos_dotdot-7);
                      id_string_end = n1.substr(pos_dotdot+dotdot_len+6, n1.length()-(pos_dotdot+dotdot_len+6));
                    }
                  else if (n1.length() > 8 && n1.substr(1,8) == "surface_")
                    {
                      // error
                    }
                  else
                    {
                      // +12..45
                      // 012^456
                      //std::cout << "tmp pos_dotdot= " << pos_dotdot << std::endl;

                      id_string_start = n1.substr(1, pos_dotdot-1);
                      id_string_end = n1.substr(pos_dotdot+dotdot_len+0, n1.length()-(pos_dotdot+dotdot_len+0));
                    }

                  std::string mult = "";
                  size_t NPOS = std::string::npos;
                  size_t cpos = id_string_end.find(":");
                  if (cpos != std::string::npos)
                    {
                      size_t xpos = id_string_end.find("x");
                      if (xpos == NPOS)
                        xpos = id_string_end.find("X");
                      VERIFY_OP_ON(xpos, !=, NPOS, "missing x or X in blocks specifier");
                      mult = id_string_end.substr(cpos);
                      id_string_end = id_string_end.substr(0, cpos);
                    }
                  if (EXTRA_PRINT_UR_GETBLOCKS) std::cout << "tmp with .., id_string_start= " << id_string_start << " id_string_end= " << id_string_end << " mult= " << mult << std::endl;

                  int id_start = 0;
                  int id_end = 0;
                  try {
                    id_start = boost::lexical_cast<int>(id_string_start);
                    id_end = boost::lexical_cast<int>(id_string_end);
                  }
                  catch (std::exception& X)
                    {
                      std::cout << "RefinerUtil::getBlockNames: exception: " << X.what() << std::endl;
                      std::cout << "RefinerUtil::getBlockNames: invalid range syntax in block_name: with .., id_string_start= "
                                << id_string_start << " id_string_end= " << id_string_end << std::endl;
                      throw std::runtime_error("invalid input syntax");
                    }
                  catch ( const std::exception * X )
                    {
                      std::cout << "RefinerUtil::getBlockNames: exception: " << X->what() << std::endl;
                      std::cout << "RefinerUtil::getBlockNames: invalid range syntax in block_name: with .., id_string_start= "
                                << id_string_start << " id_string_end= " << id_string_end << std::endl;
                      throw std::runtime_error("invalid input syntax");
                    }
                  catch( ... )
                    {
                      throw std::runtime_error("invalid input syntax");
                    }
                  if (EXTRA_PRINT_UR_GETBLOCKS)
                    {
                      std::cout << "tmp with .., id_string_start= " << id_string_start << " id_string_end= " << id_string_end << std::endl;
                      std::cout << "tmp with .., id_start= " << id_start << " id_end= " << id_end << std::endl;
                    }

                  for (int id=id_start; id <= id_end; id++)
                    {
                      new_names += plus_or_minus + (boost::lexical_cast<std::string>(id)) + mult + (id == id_end ? "" : ",");
                    }
                  if (!last_one)
                    new_names += ",";
                  if (EXTRA_PRINT_UR_GETBLOCKS)
                    std::cout << "tmp new_names with .. = " << new_names << std::endl;
                }
              else
                {
                  new_names += n1 + (last_one? "":",");
                  if (EXTRA_PRINT_UR_GETBLOCKS)
                    std::cout << "tmp new_names without .. = " << new_names << std::endl;
                }
              if (last_one)
                {
                  break;
                }
              else
                {
                  names = names.substr(ipos+1, names.length()-(ipos+1));
                }
            }
          }
        if (EXTRA_PRINT_UR_GETBLOCKS)
          std::cout << "tmp new_names after .. (range) processing = " << new_names << std::endl;

        names = new_names;
        std::string names_save = names;

        // post process to remove +name if -name exists
        bool has_plus = names.find('+') != std::string::npos;
        new_names = "";
        while(1)
          {
            if (!names.length())
              break;
            size_t pos_comma = names.find(',');
            bool last_one =  (pos_comma == std::string::npos);

            std::string n1 = (last_one ? names : names.substr(0, pos_comma) );

            bool inc = true;
            //bool exc = false;
            if ('-' == n1[0])
              {
                //exc = true;
                inc = false;
              }
            else if ('+' == n1[0])
              {
              }
            else
              {
                //error
              }
            std::string n2 = n1.substr(1, n1.length()-1);
            if (EXTRA_PRINT_UR_GETBLOCKS) std::cout << "n1= " << n1 << " n2= " << n2 << " strip_mult(n2)= " << strip_mult(n2) << std::endl;
            n2 = strip_mult(n2);
            // if (n2 == "block_910")
            //   {
            //     std::cout << "n1= " << n1 << " n2= " << n2 << std::endl;
            //   }

            if (inc)
              {

                size_t jpos = names_save.find("-"+n2);
                bool add_it = true;
                if (jpos != std::string::npos)
                  {
                    // don't add it
                    add_it = false;
                    // check for full match
                    std::string ns;
                    for (size_t ipos=jpos; ipos < names_save.length(); ++ipos)
                      {
                        if (names_save[ipos] == ',')
                          break;
                        ns += names_save[ipos];
                      }
                    std::string ns2 = ns.substr(1, ns.length()-1);
                    if (ns2 != n2)
                      add_it = true;
                    if (EXTRA_PRINT_UR_GETBLOCKS)
                      {
                        std::cout << "ns= " << ns << " ns2= " << ns2 << " n2= " << n2 << std::endl;
                      }
                  }
                if (add_it)
                  {
                    new_names += n1 + (last_one? "":",");
                  }
              }
            else
              {
                if (!has_plus)
                  new_names += n1 + (last_one? "":",");
              }

            if (last_one)
              {
                break;
              }
            else
              {
                names = names.substr(pos_comma+1, names.length()-(pos_comma+1));
              }
          }

        if (EXTRA_PRINT_UR_GETBLOCKS)
          std::cout << "tmp new_names after post-proc to remove +name if -name exists= " << new_names << std::endl;
        if (new_names.length() && !proc_rank)
          {
            std::cout << "RefinerUtil:: --block_name option after processing for removing -name= " << new_names << std::endl;
          }

        // final step
        names = new_names;
        while(1)
          {
            if (!names.length())
              break;
            size_t ipos = names.find(',');
            bool last_one =  (ipos == std::string::npos);

            {
              std::string n1 = (last_one ? names : names.substr(0, ipos) );

              bool inc = true;
              //bool exc = false;
              if ('-' == n1[0])
                {
                  //exc = true;
                  inc = false;
                }
              else if ('+' == n1[0])
                {
                }
              else
                {
                  n1 = "+" + n1;
                }
              std::string n2 = n1.substr(1, n1.length()-1);

              //std::cout << "n1= " << n1 << " n2= " << n2 << std::endl;
              if (n1.length() > 6 && n1.substr(1,6) == "block_")
                blocks[stk::topology::ELEMENT_RANK].push_back(n1);
              else if (n1.length() > 8 && n1.substr(1,8) == "surface_")
                blocks[eMesh.face_rank()].push_back(n1);
              else
                {
                  std::string pm = (inc?"+":"-");
                  blocks[stk::topology::ELEMENT_RANK].push_back(pm+"block_"+n2);
                }
              if (last_one)
                {
                  break;
                }
              else
                {
                  names = names.substr(ipos+1, names.length()-(ipos+1));
                }
            }
          }
        if (EXTRA_PRINT_UR_GETBLOCKS)
          std::cout << "tmp RefinerUtil::getBlockNames: blocks = " << blocks << std::endl;
      }

    // change so all -block get removed and replaced with + only
    for (unsigned irank=eMesh.side_rank(); irank <= eMesh.element_rank(); irank++)
      {
        bool found_minus = false, found_plus = false;
        for (unsigned ib=0; ib < blocks[irank].size(); ++ib)
          {
            if (blocks[irank][ib][0] == '-')
              {
                found_minus = true;
              }
            if (blocks[irank][ib][0] == '+')
              {
                found_plus = true;
              }
          }
        if (found_minus && found_plus)
          {
            std::ostringstream errmsg;
            errmsg << "You've specified an inconsistent combination of block names, mixing -block_n... with +block_m...\n";
            errmsg << "You can only specify -block_n if there are no +block_m's or if the +block_m's contain the -block_n\n";
            errmsg << "   to be deleted, e.g. +1,2,3.7,-4\n";
            errmsg << "Curent processed blocks list = " << blocks;
            errmsg << "\nSpecified on input: " << block_name << std::endl;
            throw std::runtime_error(errmsg.str());
          }

        if (found_minus)
          {
            std::vector<std::string> new_names;
            stk::mesh::PartVector all_parts = eMesh.get_fem_meta_data()->get_parts();
            for (stk::mesh::PartVector::iterator i_part = all_parts.begin(); i_part != all_parts.end(); ++i_part)
              {
                stk::mesh::Part *  part = *i_part ;
                if (eMesh.is_auto_or_geom_part(geomFile, part))
                  continue;

                if (part->primary_entity_rank() == irank)
                  {
                    std::string partNamePlus = "+" + part->name();
                    std::string partNameMinus = "-" + part->name();
                    bool remove_it = false;
                    for (unsigned ib=0; ib < blocks[irank].size(); ++ib)
                      {
                        if (partNameMinus == blocks[irank][ib])
                          {
                            remove_it = true;
                            break;
                          }
                      }
                    if (!remove_it)
                      {
                        new_names.push_back(partNamePlus);
                      }
                  }
              }
            if (EXTRA_PRINT_UR_GETBLOCKS && !eMesh.get_rank())
              std::cout << "RefinerUtil::getBlockNames: irank= " << irank << " old blocks = \n" << blocks[irank] << "\n new= \n" << new_names << std::endl;
            blocks[irank] = new_names;
          }
      }

    return blocks;
  }

  static void make_parallel_consistent(PerceptMesh& eMesh, std::vector<std::string>& blocks)
  {
    char buf[1024];
    stk::CommSparse comm_all(eMesh.parallel());

    unsigned proc_size = comm_all.parallel_size();

    // pack
    for (unsigned pr=0; pr < proc_size; pr++)
      {
        comm_all.send_buffer( pr ).pack< unsigned > (blocks.size());
        for (unsigned ib=0; ib < blocks.size(); ib++)
          {
            comm_all.send_buffer( pr ).pack< unsigned > ( blocks[ib].length());
            comm_all.send_buffer( pr ).pack< char > (blocks[ib].c_str(), blocks[ib].length());
          }
      }
    // allocateBuffers
    {
      comm_all.allocate_buffers();
    }
    // pack
    for (unsigned pr=0; pr < proc_size; pr++)
      {
        comm_all.send_buffer( pr ).pack< unsigned > (blocks.size());
        for (unsigned ib=0; ib < blocks.size(); ib++)
          {
            comm_all.send_buffer( pr ).pack< unsigned > ( blocks[ib].length());
            comm_all.send_buffer( pr ).pack< char > (blocks[ib].c_str(), blocks[ib].length());
          }
      }
    // communicate
    comm_all.communicate();

    // unpack
    std::set<std::string> blocks_new(blocks.begin(), blocks.end());
    for (unsigned pr=0; pr < proc_size; pr++)
      {
        unsigned bsize=0;
        comm_all.recv_buffer( pr ).unpack< unsigned > (bsize);
        for (unsigned ib=0; ib < bsize; ib++)
          {
            unsigned len=0;
            comm_all.recv_buffer( pr ).unpack< unsigned > ( len );
            comm_all.recv_buffer( pr ).unpack< char > (buf, len);
            std::string str(buf, len);
            blocks_new.insert(str);
          }
      }
    blocks.resize(0);
    blocks.assign(blocks_new.begin(), blocks_new.end());
    std::sort(blocks.begin(), blocks.end());
    //std::cout << "make_parallel_consistent blocks= " << blocks << std::endl;
  }

  /**
   * This method looks for surfaces of blocks specified in @param blocks and if it finds
   * any surfaces (sidesets), they are added to the blocks so they get refined properly.
   * If a surface is shared by more than one block, and both blocks are not in the @param blocks list,
   *   an error is thrown.
   *
   * algorithm:
   *   for sides in side_part_map
   *     if all elem_part in side_part_map[side] are in blocks, ok, else throw
   *     if side not in blocks[side_rank], add it
   *
   */

#define DEBUG_CORRECT_BLOCKS_1 0

  BlockNamesType RefinerUtil::correctBlockNamesForPartPartConsistency(percept::PerceptMesh& eMesh, BlockNamesType& blocks, const std::string& geomFile)
  {
    if (blocks[stk::topology::ELEMENT_RANK].size() == 0)
      return blocks;

    SidePartMap side_part_map;
    Refiner::get_side_part_relations(eMesh, false, side_part_map);

    stk::mesh::EntityRank subDimRank = eMesh.side_rank();

    SidePartMap::iterator iter;
    for (iter = side_part_map.begin(); iter != side_part_map.end(); iter++)
      {
        stk::mesh::Part *side_part = iter->first;
        std::string side_part_name = side_part->name();
        const stk::mesh::PartVector *side_pv  = side_part->attribute<stk::mesh::PartVector>();
        stk::mesh::PartVector side_pv1;
        if (side_pv) side_pv1 = *side_pv;
        side_pv1.push_back(side_part);

        stk::mesh::PartVector elem_pv1;

        const stk::mesh::PartVector& epv = iter->second;
        for (unsigned iepv=0; iepv < epv.size(); iepv++)
          {
            stk::mesh::Part *elem_part = epv[iepv];
            std::string elem_part_name = elem_part->name();
            const stk::mesh::PartVector *elem_pv  = elem_part->attribute<stk::mesh::PartVector>();
            elem_pv1.push_back(elem_part);
            if (elem_pv) elem_pv1.insert(elem_pv1.end(), elem_pv->begin(), elem_pv->end());
          }

        for (unsigned iside_pv = 0; iside_pv < side_pv1.size(); iside_pv++)
          {
            stk::mesh::Part *surfacePart = side_pv1[iside_pv];
            const CellTopologyData * surfacePart_topo_data = eMesh.get_cell_topology(*surfacePart);
            CellTopology surf_topo(surfacePart_topo_data);
            if (!surfacePart_topo_data)
              continue;

            bool at_least_one_elem_part_in_block_names = false;
            for (unsigned ielem_pv = 0; ielem_pv < elem_pv1.size(); ielem_pv++)
              {
                stk::mesh::Part *elementPart = elem_pv1[ielem_pv];
                stk::mesh::Part *part = elementPart;
                std::string partNamePlus = "+" + part->name();

                std::vector<std::string>::iterator partInBlocks = std::find(blocks[eMesh.element_rank()].begin(), blocks[eMesh.element_rank()].end(), partNamePlus);
                if (partInBlocks != blocks[eMesh.element_rank()].end())
                  {
                    at_least_one_elem_part_in_block_names = true;
                    break;
                  }
              }

            if (!at_least_one_elem_part_in_block_names)
              continue;

            // if at_least_one_elem_part_in_block_names then all must be in there
            for (unsigned ielem_pv = 0; ielem_pv < elem_pv1.size(); ielem_pv++)
              {
                stk::mesh::Part *elementPart = elem_pv1[ielem_pv];
                stk::mesh::Part *part = elementPart;
                std::string partNamePlus = "+" + part->name();

                std::vector<std::string>::iterator partInBlocks = std::find(blocks[eMesh.element_rank()].begin(), blocks[eMesh.element_rank()].end(), partNamePlus);
                bool found_it = partInBlocks != blocks[eMesh.element_rank()].end();

                if (!found_it)
                  {
                    std::ostringstream errmsg;
                    errmsg << "ERROR: correctBlockNamesForPartPartConsistency: found a surface (" + surfacePart->name() + ") that shares a block (" + part->name() + ") that is not in the specified\n"
                      " list of blocks to be refined.  Re-run by adding the missing block(s), which are = \n";
                    for (unsigned jelem_pv = 0; jelem_pv < elem_pv1.size(); jelem_pv++)
                      {
                        stk::mesh::Part *jelementPart = elem_pv1[jelem_pv];
                        stk::mesh::Part *jpart = jelementPart;
                        std::string jpartNamePlus = "+" + jpart->name();
                        std::vector<std::string>::iterator jpartInBlocks = std::find(blocks[eMesh.element_rank()].begin(), blocks[eMesh.element_rank()].end(), jpartNamePlus);
                        if (jpartInBlocks == blocks[eMesh.element_rank()].end())
                          {
                            errmsg << " " << jpart->name();
                          }
                      }
                    errmsg << std::endl;
                  }

                if (surfacePart_topo_data && part->primary_entity_rank() == eMesh.element_rank() && surfacePart->primary_entity_rank() == subDimRank)
                  {
                    std::string surfacePartNamePlus = "+" + surfacePart->name();
                    std::vector<std::string>::iterator surfacePartInBlocks = std::find(blocks[subDimRank].begin(), blocks[subDimRank].end(), surfacePartNamePlus);
                    // if this surface is already in the list, skip it
                    if (surfacePartInBlocks != blocks[subDimRank].end())
                      {
                        continue;
                      }
                    bool isBoundarySurface= true; // by definition, the side part map is map of sides to shared element parts

                    if (isBoundarySurface)
                      {
                        blocks[subDimRank].push_back(std::string("+"+surfacePart->name()));
                      }
                  }
              }
          }
      }

    for (unsigned ibr=0; ibr < blocks.size(); ibr++)
      {
        make_parallel_consistent(eMesh,blocks[ibr]);
      }

    return blocks;
  }

  void RefinerUtil::remove_existing_nodes(PerceptMesh& eMesh)
  {
    const stk::mesh::BucketVector & buckets = eMesh.get_bulk_data()->buckets( eMesh.node_rank() );

    stk::mesh::Part* new_nodes_part = eMesh.get_non_const_part("refine_new_nodes_part");
    VERIFY_OP_ON(new_nodes_part, != , 0, "new_nodes_part");

    std::vector<stk::mesh::Part*> remove_parts(1, new_nodes_part);
    std::vector<stk::mesh::Part*> add_parts;
    std::vector<stk::mesh::Entity> node_vec;

    stk::mesh::Selector removePartSelector(*new_nodes_part & eMesh.get_fem_meta_data()->locally_owned_part() );
    for ( stk::mesh::BucketVector::const_iterator k = buckets.begin() ; k != buckets.end() ; ++k ) {

      stk::mesh::Bucket & bucket = **k ;
      if (removePartSelector(bucket)) {

        const unsigned num_entity_in_bucket = bucket.size();
        for (unsigned ientity = 0; ientity < num_entity_in_bucket; ientity++) {

          stk::mesh::Entity node = bucket[ientity];
          node_vec.push_back(node);
        }
      }
    }
    for (unsigned ii=0; ii < node_vec.size(); ii++) {
      eMesh.get_bulk_data()->change_entity_parts( node_vec[ii], add_parts, remove_parts );
    }
  }

  void RefinerUtil::add_new_nodes(PerceptMesh& eMesh)
  {
    const stk::mesh::BucketVector & buckets = eMesh.get_bulk_data()->buckets( eMesh.node_rank() );

    stk::mesh::Part* new_nodes_part = eMesh.get_non_const_part("refine_new_nodes_part");
    VERIFY_OP_ON(new_nodes_part, != , 0, "new_nodes_part");

    std::vector<stk::mesh::Entity> new_nodes;
    for ( stk::mesh::BucketVector::const_iterator k = buckets.begin() ; k != buckets.end() ; ++k ) {

      stk::mesh::Bucket & bucket = **k ;
      if (bucket.owned()) {

        const unsigned num_entity_in_bucket = bucket.size();
        for (unsigned ientity = 0; ientity < num_entity_in_bucket; ientity++) {

          stk::mesh::Entity node = bucket[ientity];
          NewNodesType_type *ndata = stk::mesh::field_data(*eMesh.m_new_nodes_field, node);
          if (ndata && ndata[0] != 0) {
            new_nodes.push_back(node);
          }
        }
      }
    }

    std::vector<stk::mesh::Part*> add_parts(1, new_nodes_part);
    std::vector<stk::mesh::Part*> remove_parts;
    for (unsigned ind = 0; ind < new_nodes.size(); ind++) {
      eMesh.get_bulk_data()->change_entity_parts( new_nodes[ind], add_parts, remove_parts );
    }
  }

  void RefinerUtil::collect_locally_owned_entities(PerceptMesh& eMesh,  stk::mesh::EntityRank rank,  std::vector<stk::mesh::Entity>& elements)
  {
    const stk::mesh::BucketVector & buckets = eMesh.get_bulk_data()->buckets( rank );
    for ( stk::mesh::BucketVector::const_iterator k = buckets.begin() ; k != buckets.end() ; ++k )
      {
        stk::mesh::Bucket & bucket = **k ;
        const unsigned num_elements_in_bucket = bucket.size();
        if (bucket.owned())
          {
            for (unsigned iElement = 0; iElement < num_elements_in_bucket; iElement++)
              {
                elements.push_back(bucket[iElement]);
              }
          }
      }
  }

  std::string rank_to_string(stk::mesh::EntityRank rank)
  {
    std::ostringstream orank;
    orank << rank;
    return orank.str();
  }

  void RefinerUtil::get_parent_entity_and_id(PerceptMesh& eMesh, stk::mesh::EntityRank rank, stk::mesh::Entity& element, stk::mesh::Entity& parent_elem, bool debug)
  {
    ParentElementType_type *parent_element_id_from_field = NULL;

    if (is_matching_rank(*eMesh.m_parent_element_field, element))
      {
        parent_element_id_from_field = stk::mesh::field_data( *eMesh.m_parent_element_field , element );
      }
    else if (eMesh.m_parent_element_field_side && is_matching_rank(*eMesh.m_parent_element_field_side, element))
      {
        parent_element_id_from_field = stk::mesh::field_data( *eMesh.m_parent_element_field_side , element );
      }
    VERIFY_OP_ON(parent_element_id_from_field, !=, 0, "error parent element field not set on element");

    stk::mesh::EntityId parent_element_id = static_cast<stk::mesh::EntityId>(parent_element_id_from_field[0]);

    if (parent_element_id)
      parent_elem = eMesh.get_bulk_data()->get_entity(rank, parent_element_id);

    if (debug && rank == eMesh.side_rank())
      {
        std::cout << "RefinerUtil::rebuild_family_tree_child: side= " << eMesh.identifier(element) << " parent_element_id_from_field= "
                  << parent_element_id << " parent.valid= " << eMesh.is_valid(parent_elem);
        eMesh.print(element);
      }

    if (parent_element_id)
      {

        if (rank == eMesh.side_rank())
          {
            //VERIFY_OP_ON(eMesh.is_valid(parent_elem), ==, false, "root cause");
            parent_elem = stk::mesh::Entity();

            stk::mesh::EntityId id_new = 0;
            stk::mesh::ConnectivityOrdinal ord;
            eMesh.decipher_exodus_side_id(parent_element_id, id_new, ord);
            VERIFY_OP_ON(id_new, !=, 0, "bad id_new");
            if (id_new)
              {
                stk::mesh::Entity element_owner = eMesh.get_bulk_data()->get_entity(eMesh.element_rank(), id_new);

                VERIFY_OP_ON(eMesh.is_valid(element_owner), ==, true, "bad parent side elem");
                percept::MyPairIterRelation elem_to_side_rels (eMesh, element_owner, rank);
                for (unsigned jj=0; jj < elem_to_side_rels.size(); ++jj)
                  {
                    if (elem_to_side_rels[jj].relation_ordinal() == ord)
                      {
                        parent_elem = elem_to_side_rels[jj].entity();
                        break;
                      }
                  }
              }
          }

        VERIFY_OP_ON(eMesh.is_valid(parent_elem), ==, true, "bad parent found, rank= "+rank_to_string(rank));
        VERIFY_OP_ON(parent_elem, !=, element, "hmmm, parent=element");
      }
  }

  void RefinerUtil::find_or_set_parent_child_relation(PerceptMesh& eMesh, stk::mesh::EntityRank rank, stk::mesh::Entity& element, stk::mesh::Entity& parent_elem, size_t& i_ft, std::vector<stk::mesh::Entity>& ft_new_elements)
  {
    const stk::mesh::EntityRank FAMILY_TREE_RANK = static_cast<stk::mesh::EntityRank>(stk::topology::ELEMENT_RANK + 1u);

    stk::mesh::Entity family_tree = stk::mesh::Entity();
    bool found = false;
    unsigned ordinal = 0;

    if (eMesh.hasFamilyTree(parent_elem)) {

      //percept::MyPairIterRelation family_tree_relations (eMesh, element, FAMILY_TREE_RANK);
      percept::MyPairIterRelation parent_to_family_tree_relations (eMesh, parent_elem, FAMILY_TREE_RANK);
      VERIFY_OP_ON(parent_to_family_tree_relations.size(), > , 0, "hmmm");
      VERIFY_OP_ON(parent_to_family_tree_relations.size(), <=, 2, "bad family tree relations");

      bool isParentAlready = false;
      if (parent_to_family_tree_relations.size() == 1) {

        unsigned parent_elem_ft_level_0 = eMesh.getFamilyTreeRelationIndex(FAMILY_TREE_LEVEL_0, parent_elem);
        family_tree = parent_to_family_tree_relations[parent_elem_ft_level_0].entity();
      }
      else if (parent_to_family_tree_relations.size() == 2) {

        unsigned parent_elem_ft_level_1 = eMesh.getFamilyTreeRelationIndex(FAMILY_TREE_LEVEL_1, parent_elem);
        family_tree = parent_to_family_tree_relations[parent_elem_ft_level_1].entity();
        isParentAlready = true;
      }

      VERIFY_OP_ON(eMesh.is_valid(family_tree), ==, true, "hmmm");
      percept::MyPairIterRelation family_tree_relations (eMesh, family_tree, eMesh.entity_rank(parent_elem));
      VERIFY_OP_ON(family_tree_relations.size(), > , 0, "hmmm");
      if (family_tree_relations[0].entity() == parent_elem) {

        isParentAlready = true;
        VERIFY_OP_ON(eMesh.isParentElement(parent_elem), ==, true, "hmmm");
      }
      for (unsigned i = 1; i < family_tree_relations.size(); i++) {

        //if (family_tree_relations[i].relation_ordinal() == (ordinal + 1))
        if (family_tree_relations[i].entity() == element) {
          found = true;
          break;
        }
      }
      if (!found && isParentAlready) {
        VERIFY_OP_ON(family_tree_relations.size(), > , 0, "hmmm");
        ordinal = family_tree_relations.size() - 1;
      }
    }
    if (!found) {
      if (i_ft >= ft_new_elements.size()) {
        throw std::runtime_error("ran out of ft_new_elements");
      }
      stk::mesh::Entity familyTreeNewElement = ft_new_elements[i_ft++];
      UniformRefinerPatternBase::set_parent_child_relations(eMesh, parent_elem, element, familyTreeNewElement, ordinal);
    }
  }

  void RefinerUtil::rebuild_family_tree(PerceptMesh& eMesh, bool debug)
  {
    VERIFY_OP_ON(eMesh.m_parent_element_field_set, == , true, "parent_element_field_set");
    VERIFY_OP_ON(eMesh.m_new_nodes_field_set, == , true, "new_nodes_field_set");

    eMesh.get_bulk_data()->modification_begin();

    remove_existing_nodes(eMesh);
    add_new_nodes(eMesh);

    stk::mesh::fixup_ghosted_to_shared_nodes(*eMesh.get_bulk_data());

    eMesh.get_bulk_data()->modification_end();

    for (stk::mesh::EntityRank rank = eMesh.side_rank(); rank <= eMesh.element_rank(); ++rank) {

      std::vector<stk::mesh::Entity> ft_new_elements;
      const stk::mesh::EntityRank FAMILY_TREE_RANK = static_cast<stk::mesh::EntityRank>(stk::topology::ELEMENT_RANK + 1u);

      std::vector<stk::mesh::Entity> elements;
      collect_locally_owned_entities(eMesh, rank, elements);

      size_t num_elem_needed = elements.size();

      eMesh.get_bulk_data()->modification_begin();

      eMesh.createEntities( FAMILY_TREE_RANK, 2*num_elem_needed, ft_new_elements);
      size_t i_ft = 0;

      for (size_t ii=0; ii < elements.size(); ++ii) {

        stk::mesh::Entity element = elements[ii];
        stk::mesh::Entity parent_elem = stk::mesh::Entity();

        get_parent_entity_and_id(eMesh, rank, element, parent_elem, debug);

        if (!eMesh.is_valid(parent_elem))
          continue;

        find_or_set_parent_child_relation(eMesh, rank, element, parent_elem, i_ft, ft_new_elements);
      }

      std::vector<stk::mesh::Entity> ft_delete;
      for (unsigned ii=i_ft; ii < ft_new_elements.size(); ++ii) {

        if (eMesh.get_bulk_data()->has_no_relations(ft_new_elements[ii]))
          ft_delete.push_back(ft_new_elements[ii]);
      }
      for (unsigned ii=0; ii < ft_delete.size(); ++ii) {

        if (!eMesh.get_bulk_data()->destroy_entity(ft_delete[ii]))
          throw std::runtime_error("bad destroy of family_tree");
      }
      stk::mesh::fixup_ghosted_to_shared_nodes(*eMesh.get_bulk_data());

      eMesh.get_bulk_data()->modification_end();
    }

    // get parts correct
    eMesh.get_bulk_data()->modification_begin();
    Refiner::set_active_part(eMesh);
    stk::mesh::fixup_ghosted_to_shared_nodes(*eMesh.get_bulk_data());
    eMesh.get_bulk_data()->modification_end();
  }

  void RefinerUtil::save_node_registry(PerceptMesh& eMesh, NodeRegistry& nodeRegistry, const std::string& msg, bool doComm)
  {
    if (eMesh.m_node_registry_field == 0)
      return;

    if (1)
      {
        std::vector<stk::mesh::Entity> vec;
        stk::mesh::Selector sel = eMesh.get_fem_meta_data()->universal_part();
        stk::mesh::get_selected_entities(sel , eMesh.get_bulk_data()->buckets(eMesh.node_rank()), vec);

        SubDimCell_SDCEntityType subDimEntity(&eMesh);

        // FIXME
        for (size_t ii=0; ii < vec.size(); ++ii)
          {
            stk::mesh::Entity node = vec[ii];

            VERIFY_OP_ON(eMesh.entity_rank(node), ==, eMesh.node_rank(), "bad rank");
            double *node_data = stk::mesh::field_data(*eMesh.m_node_registry_field, node);
            for (unsigned kk=0; kk < NUM_NR_FIELD_SLOTS; ++kk)
              {
                node_data[kk] = 0.0;
              }
          }
      }

    SubDimCellToDataMap& map = nodeRegistry.getMap();
    SubDimCellToDataMap::iterator iter;

    for (iter = map.begin(); iter != map.end(); ++iter)
      {
        const SubDimCell_SDCEntityType& subDimEntity = (*iter).first;
        SubDimCellData& nodeId_elementOwnderId = (*iter).second;

        // tuple storage: SDC_DATA_GLOBAL_NODE_IDS, SDC_DATA_OWNING_ELEMENT_KEY,  SDC_DATA_OWNING_SUBDIM_RANK, SDC_DATA_OWNING_SUBDIM_ORDINAL, SDC_DATA_SPACING
        NodeIdsOnSubDimEntityType& nodeIds_onSE      = nodeId_elementOwnderId.get<SDC_DATA_GLOBAL_NODE_IDS>();

        stk::mesh::EntityKey       owningElementKey  = nodeId_elementOwnderId.get<SDC_DATA_OWNING_ELEMENT_KEY>();
        stk::mesh::EntityId        owningElementId   = owningElementKey.id();
        (void)owningElementId;
        stk::mesh::EntityRank      owningElementRank = owningElementKey.rank();
        //VERIFY_OP_ON(owningElementRank, ==, eMesh.element_rank(), "bad owningElementRank");
        unsigned               owningSubDimRank  = nodeId_elementOwnderId.get<SDC_DATA_OWNING_SUBDIM_RANK>();
        unsigned               owningSubDimOrd   = nodeId_elementOwnderId.get<SDC_DATA_OWNING_SUBDIM_ORDINAL>();
        VERIFY_OP_ON(owningSubDimOrd, >, 0, "hmm 2");
        --owningSubDimOrd;
        unsigned owningSubDimSize = subDimEntity.size();
        (void)owningSubDimSize;

        //Double3                    sdcSpacing        = nodeId_elementOwnderId.get<SDC_DATA_SPACING>();

        stk::mesh::Entity owningElement = eMesh.get_bulk_data()->get_entity(owningElementKey.rank(), owningElementKey.id());
        if (!eMesh.is_valid(owningElement))
          {
            continue;
          }

        for (unsigned jj = 0; jj < nodeIds_onSE.size(); ++jj)
          {
            stk::mesh::Entity node = nodeIds_onSE[jj];

            if (!eMesh.is_valid(node))
              {
                continue;
              }

            VERIFY_OP_ON(eMesh.entity_rank(node), ==, eMesh.node_rank(), "bad rank");
            NodeRegistryFieldType_type *node_data = stk::mesh::field_data(*eMesh.m_node_registry_field, node);
            if (!node_data)
              {
                continue;
              }

            node_data[NR_FIELD_OWNING_ELEMENT_ID]     = static_cast<NodeRegistryFieldType_type>(owningElementKey.id());
            node_data[NR_FIELD_OWNING_ELEMENT_RANK]   = static_cast<NodeRegistryFieldType_type>(owningElementRank);
            node_data[NR_FIELD_MARK]                  = static_cast<NodeRegistryFieldType_type>(nodeIds_onSE.m_mark);
            node_data[NR_FIELD_OWNING_SUBDIM_RANK]    = static_cast<NodeRegistryFieldType_type>(owningSubDimRank);
            node_data[NR_FIELD_OWNING_SUBDIM_ORDINAL] = static_cast<NodeRegistryFieldType_type>(owningSubDimOrd + 1);
            node_data[NR_FIELD_OWNING_SUBDIM_SIZE]    = static_cast<NodeRegistryFieldType_type>(owningSubDimSize);

          }
      }

    if (doComm)
      {
        std::vector< const stk::mesh::FieldBase *> fields;
        fields.push_back(eMesh.m_node_registry_field);
        stk::mesh::copy_owned_to_shared(*eMesh.get_bulk_data(), fields);
      }
  }

  void RefinerUtil::rebuild_node_registry(PerceptMesh& eMesh, NodeRegistry& nodeRegistry, bool initNR, PerceptMesh* eMeshNR, NodeRegistry *compareNR, bool skipEmpty)
  {
    if (eMesh.m_node_registry_field == 0)
      return;

    if (initNR)
      {
        nodeRegistry.initialize();
        nodeRegistry.getMap().clear();
        nodeRegistry.init_comm_all();
      }

    if (1)
      {
        std::vector< const stk::mesh::FieldBase *> fields;
        fields.push_back(eMesh.m_node_registry_field);
        stk::mesh::copy_owned_to_shared(*eMesh.get_bulk_data(), fields);
      }

    std::vector<stk::mesh::Entity> vec;
    stk::mesh::Selector sel = eMesh.get_fem_meta_data()->universal_part();
    stk::mesh::get_selected_entities(sel , eMesh.get_bulk_data()->buckets(eMesh.node_rank()), vec);

    SubDimCell_SDCEntityType subDimEntity(&eMesh);

    for (size_t inode=0; inode < vec.size(); ++inode)
      {
        stk::mesh::Entity node = vec[inode];

        double *node_data = stk::mesh::field_data(*eMesh.m_node_registry_field, node);

        stk::mesh::EntityId  owningElementId (  static_cast<stk::mesh::EntityId>(node_data[NR_FIELD_OWNING_ELEMENT_ID]) );
        stk::mesh::EntityRank  owningElementRank (  static_cast<stk::mesh::EntityRank>(node_data[NR_FIELD_OWNING_ELEMENT_RANK]) );
        stk::mesh::EntityKey owningElementKey(owningElementRank, owningElementId);
        stk::mesh::Entity owningElement = eMesh.get_bulk_data()->get_entity(owningElementRank, owningElementId);
        if (!eMesh.is_valid(owningElement))
          {
            continue;
          }
        VERIFY_OP_ON(eMesh.is_valid(owningElement), ==, true, "bad owningElement");
        stk::mesh::EntityRank owningSubDimRank = static_cast<stk::mesh::EntityRank>(node_data[NR_FIELD_OWNING_SUBDIM_RANK]);
        unsigned              owningSubDimOrd  = static_cast<unsigned>(node_data[NR_FIELD_OWNING_SUBDIM_ORDINAL]);
        VERIFY_OP_ON(owningSubDimOrd, >, 0, "hmmm 3");
        --owningSubDimOrd;
        unsigned              owningSubDimSize = static_cast<unsigned>(node_data[NR_FIELD_OWNING_SUBDIM_SIZE]);

        bool foundGhostNode = nodeRegistry.getSubDimEntity(subDimEntity, owningElement, owningSubDimRank, owningSubDimOrd);
        if (foundGhostNode && nodeRegistry.getCheckForGhostedNodes())
          {
            //std::cout << eMesh.rank() << " rebuild_node_registry foundGhostNode= " << foundGhostNode << std::endl;
            continue;
          }
        VERIFY_OP_ON(subDimEntity.size(), ==, owningSubDimSize, "bad owningSubDimSize");

        for (unsigned ii=0; ii < subDimEntity.size(); ++ii)
          {
            VERIFY_OP_ON(eMesh.is_valid(subDimEntity[ii]), ==, true, "bad node in rebuild_node_registry");
          }
        static SubDimCellData empty_SubDimCellData;
        SubDimCellData* nodeId_elementOwnderId_ptr = nodeRegistry.getFromMapPtr(subDimEntity);
        SubDimCellData& nodeId_elementOwnderId = (nodeId_elementOwnderId_ptr ? *nodeId_elementOwnderId_ptr : empty_SubDimCellData);
        bool is_empty = nodeId_elementOwnderId_ptr == 0;

        if (is_empty)
          {
            SubDimCellData data( NodeIdsOnSubDimEntityType(1, stk::mesh::Entity(), nodeRegistry.NR_MARK_NONE),
                                 owningElementKey, owningSubDimRank, owningSubDimOrd + 1);
            NodeIdsOnSubDimEntityType& nid_new = data.get<SDC_DATA_GLOBAL_NODE_IDS>();
            nid_new.resize(1);
            nid_new[0] = node;
            nid_new.m_entity_id_vector[0] = eMesh.id(node);
            nid_new.m_mark = static_cast<unsigned>(node_data[NR_FIELD_MARK]);

            nodeRegistry.putInMap(subDimEntity,  data);
          }
        else
          {
            NodeIdsOnSubDimEntityType& nodeIds_onSE = nodeId_elementOwnderId.get<SDC_DATA_GLOBAL_NODE_IDS>();
            nodeIds_onSE.push_back(node);
            nodeIds_onSE.m_entity_id_vector.push_back(eMesh.id(node));
            nodeIds_onSE.m_mark = static_cast<unsigned>(node_data[NR_FIELD_MARK]);
            VERIFY_OP_ON(nodeIds_onSE.size(), ==, 1, "bad size on proc: "+eMesh.rank());
          }
      }
  }

}

max_stars_repo_name: jrood-nrel/percept

max_issues_repo_name: jrood-nrel/percept

max_forks_repo_name: jrood-nrel/percept


text:
#include <stan/math/torsten/test/unit/test_fixture_twocpt_effcpt.hpp>
#include <stan/math/torsten/test/unit/test_functors.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <boost/mp11.hpp>

TYPED_TEST_SUITE_P(test_twocpt_effcpt);
TYPED_TEST_P(test_twocpt_effcpt, multiple_bolus) {
  this -> compare_solvers_val();
  this -> compare_solvers_adj(this -> amt, 1.e-8, "AMT");
  this -> compare_solvers_adj(this -> rate, 1.e-8, "RATE");
  this -> compare_solvers_adj(this -> ii, 1.e-8, "II");
  this -> compare_solvers_adj(this -> theta[0], 5.e-6, "theta");
  this -> compare_solvers_adj(this -> biovar[0], 1.e-6, "bioavailability");
  this -> compare_solvers_adj(this -> tlag[0], 1.e-8, "lag time");
}

TYPED_TEST_P(test_twocpt_effcpt, multiple_bolus_addl) {
  this -> ii[0] = 1.55;
  this -> compare_solvers_val();
  this -> compare_solvers_adj(this -> amt, 1.e-8, "AMT");
  this -> compare_solvers_adj(this -> rate, 1.e-8, "RATE");
  this -> compare_solvers_adj(this -> ii, 5.e-6, "II");
  this -> compare_solvers_adj(this -> theta[0], 5.e-6, "theta");
  this -> compare_solvers_adj(this -> biovar[0], 1.e-6, "bioavailability");
  this -> compare_solvers_adj(this -> tlag[0], 1.e-8, "lag time");
}

REGISTER_TYPED_TEST_SUITE_P(test_twocpt_effcpt,
                            multiple_bolus,
                            multiple_bolus_addl);

using test_types = boost::mp11::mp_product<
  std::tuple,
  ::testing::Types<pmx_solve_twocpt_effcpt_functor>, // solver 1
  ::testing::Types<pmx_solve_linode_functor,
                   pmx_solve_rk45_functor>, // solver 2
  ::testing::Types<double>,  // TIME
  ::testing::Types<double, stan::math::var_value<double>>,  // AMT
  ::testing::Types<double, stan::math::var_value<double>> , // RATE
  ::testing::Types<double> , // II
  ::testing::Types<double, stan::math::var_value<double>> , // PARAM
  ::testing::Types<double> , // BIOVAR
  ::testing::Types<double> , // TLAG
  ::testing::Types<torsten::PMXTwocptEffCptODE>             // ODE
    >;

INSTANTIATE_TYPED_TEST_SUITE_P(PMX, test_twocpt_effcpt, test_types);


text:
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "UKF/Core.h"
#include "cukf.h"

/*
This file implements the same UKF as was used for the 2014 UAV Challenge.
There are a number of efficiency and performance improvements that could be
made with the new library structure, but the main purpose of this is for
direct comparison of performance of the new library against the old
implementation.
*/

enum SFWA_States {
    LatLon,
    Altitude,
    Velocity,
    Acceleration,
    Attitude,
    AngularVelocity,
    AngularAcceleration,
    WindVelocity,
    GyroBias
};

using SFWA_StateVector = UKF::StateVector<
    UKF::Field<LatLon, UKF::Vector<2>>,                 /* Latitude and longitude (rad) */
    UKF::Field<Altitude, real_t>,                       /* Altitude above the WGS84 ellipsoid (m) */
    UKF::Field<Velocity, UKF::Vector<3>>,               /* Velocity (NED frame, m/s) */
    UKF::Field<Acceleration, UKF::Vector<3>>,           /* Acceleration (body frame, m/s^2) */
    UKF::Field<Attitude, UKF::Quaternion>,              /* Attitude as a quaternion (NED frame to body frame) */
    UKF::Field<AngularVelocity, UKF::Vector<3>>,        /* Angular velocity (body frame, rad/s) */
    UKF::Field<AngularAcceleration, UKF::Vector<3>>,    /* Angular acceleration (body frame, rad/s^2) */
    UKF::Field<WindVelocity, UKF::Vector<3>>,           /* Wind velocity (NED frame, m/s) */
    UKF::Field<GyroBias, UKF::Vector<3>>                /* Gyro bias (body frame, rad/s) */
>;

/* WGS84 reference ellipsoid constants. */
#define WGS84_A (6378137.0)
#define WGS84_B (6356752.314245)
#define WGS84_A2 (WGS84_A*WGS84_A)
#define WGS84_B2 (WGS84_B*WGS84_B)
#define WGS84_AB2 (WGS84_A2*WGS84_B2)

#define G_ACCEL (9.80665)
#define RHO (1.225)

/* SFWA vehicle dynamics model. Not used for this comparison. */
UKF::Vector<6> x8_dynamics_model(const SFWA_StateVector &state, const UKF::Vector<3> &control) {
    UKF::Vector<6> output;

    /* Cache state data for convenience */
    UKF::Quaternion attitude = state.get_field<Attitude>();
    real_t yaw_rate = state.get_field<AngularVelocity>()[2],
           pitch_rate = state.get_field<AngularVelocity>()[1],
           roll_rate = state.get_field<AngularVelocity>()[0];

    /* External axes */
    UKF::Vector<3> airflow = attitude * (state.get_field<WindVelocity>() - state.get_field<Velocity>());

    /*
    Calculate axial airflow
    */
    real_t airflow_x2, airflow_y2, airflow_z2, airflow_v2;
    airflow_x2 = airflow[0]*airflow[0];
    airflow_y2 = airflow[1]*airflow[1];
    airflow_z2 = airflow[2]*airflow[2];
    airflow_v2 = airflow_x2 + airflow_y2 + airflow_z2;

    /*
    Determine motor thrust and torque.
    */
    real_t rpm = control[0] * 12000.0, thrust,
           ve2 = (0.0025 * 0.0025) * rpm * rpm;
    /* 1 / 3.8kg times area * density of air */
    thrust = (ve2 - airflow_v2) *
             (0.26315789473684 * 0.5 * RHO * 0.02);

    /*
    Calculate airflow in the horizontal and vertical planes, as well as
    pressure
    */
    real_t v_inv, vertical_v, vertical_v_inv, qbar;

    qbar = (RHO * 0.5) * airflow_v2;
    v_inv = 1.0 / std::sqrt(std::max(1.0, airflow_v2));

    vertical_v = std::sqrt(airflow_x2 + airflow_z2);
    vertical_v_inv = 1.0 / std::max(1.0, vertical_v);

    /* Work out sin/cos of alpha and beta */
    real_t sin_alpha, cos_alpha, sin_beta, cos_beta, sin_cos_alpha;

    sin_beta = airflow[1] * v_inv;
    cos_beta = vertical_v * v_inv;

    sin_alpha = -airflow[2] * vertical_v_inv;
    cos_alpha = -airflow[0] * vertical_v_inv;

    sin_cos_alpha = sin_alpha * cos_alpha;

    /* Work out aerodynamic forces in wind frame */
    real_t lift, drag, side_force;

    /* 0.26315789473684 is the reciprocal of mass (3.8kg) */
    lift = (qbar * 0.26315789473684) * (0.8 * sin_cos_alpha + 0.18);
    drag = (qbar * 0.26315789473684) *
           (0.05 + 0.7 * sin_alpha * sin_alpha);
    side_force = (qbar * 0.26315789473684) * 0.2 * sin_beta * cos_beta;

    /* Convert aerodynamic forces from wind frame to body frame */
    real_t x_aero_f = lift * sin_alpha - drag * cos_alpha -
                             side_force * sin_beta,
           z_aero_f = lift * cos_alpha + drag * sin_alpha,
           y_aero_f = side_force * cos_beta;

    output.segment<3>(0) << UKF::Vector<3>(x_aero_f + thrust, y_aero_f, -z_aero_f) +
        (attitude * UKF::Vector<3>(0, 0, G_ACCEL));

    /* Determine moments */
    real_t pitch_moment, yaw_moment, roll_moment,
           left_aileron = control[1] - 0.5, right_aileron = control[2] - 0.5;
    pitch_moment = 0.0 - 0.0 * sin_alpha - 0.0 * pitch_rate -
                   0.1 * (left_aileron + right_aileron) * vertical_v * 0.1;
    roll_moment = 0.05 * sin_beta - 0.1 * roll_rate +
                  0.15 * (left_aileron - right_aileron) * vertical_v * 0.1;
    yaw_moment = -0.02 * sin_beta - 0.05 * yaw_rate -
                 0.02 * (std::abs(left_aileron) + std::abs(right_aileron)) *
                 vertical_v * 0.1;
    pitch_moment *= qbar;
    roll_moment *= qbar;
    yaw_moment *= qbar;

    /*
    Calculate angular acceleration (tau / inertia tensor).
    Inertia tensor is:
        0.3 0 -0.0334
        0 0.17 0
        -0.0334 0 0.405
    So inverse is:
        3.36422 0 0.277444
        0 5.88235 0
        0.277444 0 2.49202
    */
    output.segment<3>(3) << UKF::Vector<3>(
        (3.364222 * roll_moment + 0.27744448 * yaw_moment),
        10.8823528 * pitch_moment,
        (0.27744448 * roll_moment + 2.4920163 * yaw_moment));

    return output;
}

namespace UKF {

/* SFWA state vector process model. */
template <> template <>
SFWA_StateVector SFWA_StateVector::derivative<>() const {
    SFWA_StateVector output;

    /* Calculate the normal and meridional radii of curvature. */
    real_t lat = get_field<LatLon>()[0];
    real_t tempA = WGS84_A*std::cos(lat), tempB = WGS84_B*std::sin(lat),
           temp = tempA * tempA + tempB * tempB,
           temp_sqrt = std::sqrt(temp);
    real_t M = WGS84_AB2 / (temp_sqrt * temp);
    real_t N = WGS84_A2 / temp_sqrt;

    /*
    Calculate change in position. Using the small angle approximation, this
    becomes very simple – no trig required for latitude derivative, and one
    cosine function for longitude derivative.
    */
    UKF::Vector<3> vel = get_field<Velocity>();
    output.set_field<LatLon>(Vector<2>(
        vel[0] / (M + get_field<Altitude>()),
        (vel[1] / (N + get_field<Altitude>())) * std::cos(lat)));
    output.set_field<Altitude>(-vel[2]);

    /* Calculate change in velocity. */
    output.set_field<Velocity>(get_field<Attitude>().conjugate() * get_field<Acceleration>());

    /* Change in linear acceleration is zero. */
    output.set_field<Acceleration>(UKF::Vector<3>(0, 0, 0));

    /* Calculate change in attitude. */
    UKF::Quaternion omega_q;
    omega_q.vec() = get_field<AngularVelocity>() * 0.5;
    omega_q.w() = 0;
    output.set_field<Attitude>(omega_q.conjugate() * get_field<Attitude>());

    /* Calculate change in angular velocity (just angular acceleration). */
    output.set_field<AngularVelocity>(get_field<AngularAcceleration>());

    /* Change in angular acceleration is zero. */
    output.set_field<AngularAcceleration>(UKF::Vector<3>(0, 0, 0));

    /* Change in wind velocity is zero. */
    output.set_field<WindVelocity>(UKF::Vector<3>(0, 0, 0));

    /* Change in gyro bias is zero. */
    output.set_field<GyroBias>(UKF::Vector<3>(0, 0, 0));

    return output;
}

}

enum SFWA_Measurements {
    Accelerometer,
    Gyroscope,
    Magnetometer,
    GPS_Position,
    GPS_Velocity,
    Airspeed,
    PressureAltitude
};

using SFWA_MeasurementVector = UKF::DynamicMeasurementVector<
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Gyroscope, UKF::Vector<3>>,
    UKF::Field<Magnetometer, UKF::Vector<3>>,
    UKF::Field<GPS_Position, UKF::Vector<3>>,
    UKF::Field<GPS_Velocity, UKF::Vector<3>>,
    UKF::Field<Airspeed, real_t>,
    UKF::Field<PressureAltitude, real_t>
>;

/*
Hard code the magnetic field vector to the WMM value for
-37.954690, 145.237575 for the purposes of this comparison. Value is in
microtesla.
*/
static UKF::Vector<3> local_mag_field = UKF::Vector<3>(21.2584, 4.4306, -55.9677);

using SFWA_UKF = UKF::Core<
    SFWA_StateVector,
    SFWA_MeasurementVector,
    UKF::IntegratorRK4
>;

namespace UKF {
/* SFWA measurement model. */
template <> template <>
UKF::Vector<3> SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, Accelerometer>(const SFWA_StateVector& state) {
    return state.get_field<Acceleration>() + state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -G_ACCEL);
}

template <> template <>
UKF::Vector<3> SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, Gyroscope>(const SFWA_StateVector& state) {
    return state.get_field<AngularVelocity>() + state.get_field<GyroBias>();
}

template <> template <>
UKF::Vector<3> SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, Magnetometer>(const SFWA_StateVector& state) {
    return state.get_field<Attitude>() * local_mag_field;
}

template <> template <>
UKF::Vector<3> SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, GPS_Position>(const SFWA_StateVector& state) {
    return UKF::Vector<3>(state.get_field<LatLon>()[0], state.get_field<LatLon>()[1], state.get_field<Altitude>());
}

template <> template <>
UKF::Vector<3> SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, GPS_Velocity>(const SFWA_StateVector& state) {
    return state.get_field<Velocity>();
}

template <> template <>
real_t SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, Airspeed>(const SFWA_StateVector& state) {
    return (state.get_field<Attitude>() * (state.get_field<Velocity>() - state.get_field<WindVelocity>()))[0];
}

template <> template <>
real_t SFWA_MeasurementVector::expected_measurement
<SFWA_StateVector, PressureAltitude>(const SFWA_StateVector& state) {
    return state.get_field<Altitude>();
}

}
static SFWA_UKF ukf;
static SFWA_MeasurementVector meas;

/*
The following functions provide a ctypes-compatible interface for ease of
testing.
*/

void ukf_init() {
    ukf.state.set_field<LatLon>(UKF::Vector<2>(-0.662434, 2.534874));
    ukf.state.set_field<Altitude>(0);
    ukf.state.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));
    ukf.state.set_field<Acceleration>(UKF::Vector<3>(0, 0, 0));
    ukf.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    ukf.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
    ukf.state.set_field<AngularAcceleration>(UKF::Vector<3>(0, 0, 0));
    ukf.state.set_field<WindVelocity>(UKF::Vector<3>(0, 0, 0));
    ukf.state.set_field<GyroBias>(UKF::Vector<3>(0, 0, 0));
    ukf.covariance = SFWA_StateVector::CovarianceMatrix::Zero();
    ukf.covariance.diagonal() <<
        1, 1, 10000, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 10, 10, 10,
        1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 100, 100, 100, 0.01, 0.01, 0.01;
    ukf.measurement_covariance <<1, 1, 1, 0.01, 0.01, 0.01, 10, 10, 10, 1e-20, 1e-20, 1, 1, 1, 1, 1, 1;

    local_mag_field = UKF::Vector<3>(21.2584, 4.4306, -55.9677);

    ukf.process_noise_covariance = SFWA_StateVector::CovarianceMatrix::Zero();
    ukf.process_noise_covariance.diagonal() <<
        1e-17, 1e-17, 1e-4,     /* lat, lon, alt */
        2e-3, 2e-3, 2e-3,       /* velocity N, E, D */
        2e-2, 2e-2, 2e-2,       /* acceleration x, y, z */
        7e-8, 7e-8, 7e-8,       /* attitude roll, pitch, yaw */
        1e-3, 1e-3, 1e-3,       /* angular velocity roll, pitch, yaw */
        1e-3, 1e-3, 1e-3,       /* angular acceleration roll, pitch, yaw */
        1e-7, 1e-7, 1e-7,       /* wind velocity N, E, D -- NOTE: from FCS armed mode */
        1e-12, 1e-12, 1e-12;    /* gyro bias x, y, z -- NOTE: from FCS armed mode */
}

void ukf_set_position(real_t lat, real_t lon, real_t alt) {
    ukf.state.set_field<LatLon>(UKF::Vector<2>(lat, lon));
    ukf.state.set_field<Altitude>(alt);
}

void ukf_set_velocity(real_t x, real_t y, real_t z) {
    ukf.state.set_field<Velocity>(UKF::Vector<3>(x, y, z));
}

void ukf_set_acceleration(real_t x, real_t y, real_t z) {
    ukf.state.set_field<Acceleration>(UKF::Vector<3>(x, y, z));
}

void ukf_set_attitude(real_t w, real_t x, real_t y, real_t z) {
    ukf.state.set_field<Attitude>(UKF::Quaternion(w, x, y, z));
}

void ukf_set_angular_velocity(real_t x, real_t y, real_t z) {
    ukf.state.set_field<AngularVelocity>(UKF::Vector<3>(x, y, z));
}

void ukf_set_angular_acceleration(real_t x, real_t y, real_t z) {
    ukf.state.set_field<AngularAcceleration>(UKF::Vector<3>(x, y, z));
}

void ukf_set_wind_velocity(real_t x, real_t y, real_t z) {
    ukf.state.set_field<WindVelocity>(UKF::Vector<3>(x, y, z));
}

void ukf_set_gyro_bias(real_t x, real_t y, real_t z) {
    ukf.state.set_field<GyroBias>(UKF::Vector<3>(x, y, z));
}

void ukf_get_state(struct ukf_state_t *in) {
    in->position[0] = ukf.state.get_field<LatLon>()[0];
    in->position[1] = ukf.state.get_field<LatLon>()[1];
    in->position[2] = ukf.state.get_field<Altitude>();
    in->velocity[0] = ukf.state.get_field<Velocity>()[0];
    in->velocity[1] = ukf.state.get_field<Velocity>()[1];
    in->velocity[2] = ukf.state.get_field<Velocity>()[2];
    in->acceleration[0] = ukf.state.get_field<Acceleration>()[0];
    in->acceleration[1] = ukf.state.get_field<Acceleration>()[1];
    in->acceleration[2] = ukf.state.get_field<Acceleration>()[2];
    in->attitude[0] = ukf.state.get_field<Attitude>().x();
    in->attitude[1] = ukf.state.get_field<Attitude>().y();
    in->attitude[2] = ukf.state.get_field<Attitude>().z();
    in->attitude[3] = ukf.state.get_field<Attitude>().w();
    in->angular_velocity[0] = ukf.state.get_field<AngularVelocity>()[0];
    in->angular_velocity[1] = ukf.state.get_field<AngularVelocity>()[1];
    in->angular_velocity[2] = ukf.state.get_field<AngularVelocity>()[2];
    in->angular_acceleration[0] = ukf.state.get_field<AngularAcceleration>()[0];
    in->angular_acceleration[1] = ukf.state.get_field<AngularAcceleration>()[1];
    in->angular_acceleration[2] = ukf.state.get_field<AngularAcceleration>()[2];
    in->wind_velocity[0] = ukf.state.get_field<WindVelocity>()[0];
    in->wind_velocity[1] = ukf.state.get_field<WindVelocity>()[1];
    in->wind_velocity[2] = ukf.state.get_field<WindVelocity>()[2];
    in->gyro_bias[0] = ukf.state.get_field<GyroBias>()[0];
    in->gyro_bias[1] = ukf.state.get_field<GyroBias>()[1];
    in->gyro_bias[2] = ukf.state.get_field<GyroBias>()[2];
}

void ukf_set_state(struct ukf_state_t *in) {
    ukf.state.set_field<LatLon>(UKF::Vector<2>(in->position[0], in->position[1]));
    ukf.state.set_field<Altitude>(in->position[2]);
    ukf.state.set_field<Velocity>(
        UKF::Vector<3>(in->velocity[0], in->velocity[1], in->velocity[2]));
    ukf.state.set_field<Acceleration>(
        UKF::Vector<3>(in->acceleration[0], in->acceleration[1], in->acceleration[2]));
    ukf.state.set_field<Attitude>(
        UKF::Quaternion(in->attitude[3], in->attitude[0], in->attitude[1], in->attitude[2]));
    ukf.state.set_field<AngularVelocity>(
        UKF::Vector<3>(in->angular_velocity[0], in->angular_velocity[1], in->angular_velocity[2]));
    ukf.state.set_field<AngularAcceleration>(
        UKF::Vector<3>(in->angular_acceleration[0], in->angular_acceleration[1], in->angular_acceleration[2]));
    ukf.state.set_field<WindVelocity>(
        UKF::Vector<3>(in->wind_velocity[0], in->wind_velocity[1], in->wind_velocity[2]));
    ukf.state.set_field<GyroBias>(
        UKF::Vector<3>(in->gyro_bias[0], in->gyro_bias[1], in->gyro_bias[2]));
}

void ukf_get_state_covariance(
        real_t state_covariance[SFWA_StateVector::covariance_size()*SFWA_StateVector::covariance_size()]) {
    Eigen::Map<typename SFWA_StateVector::CovarianceMatrix> covariance_map(state_covariance);
    covariance_map = ukf.covariance;
}

void ukf_get_state_covariance_diagonal(
        real_t state_covariance_diagonal[SFWA_StateVector::covariance_size()]) {
    Eigen::Map<UKF::Vector<SFWA_StateVector::covariance_size()>> covariance_map(state_covariance_diagonal);
    covariance_map = ukf.covariance.diagonal();
}

void ukf_get_state_error(real_t state_error[SFWA_StateVector::covariance_size()]) {
    Eigen::Map<typename SFWA_StateVector::StateVectorDelta> error_map(state_error);
    error_map = ukf.covariance.cwiseAbs().rowwise().sum().cwiseSqrt();
}

void ukf_sensor_clear() {
    meas = SFWA_MeasurementVector();
}

void ukf_sensor_set_accelerometer(real_t x, real_t y, real_t z) {
    meas.set_field<Accelerometer>(UKF::Vector<3>(x, y, z));
}

void ukf_sensor_set_gyroscope(real_t x, real_t y, real_t z) {
    meas.set_field<Gyroscope>(UKF::Vector<3>(x, y, z));
}

void ukf_sensor_set_magnetometer(real_t x, real_t y, real_t z) {
    meas.set_field<Magnetometer>(UKF::Vector<3>(x, y, z));
}

void ukf_sensor_set_gps_position(real_t lat, real_t lon, real_t alt) {
    meas.set_field<GPS_Position>(UKF::Vector<3>(lat, lon, alt));
}

void ukf_sensor_set_gps_velocity(real_t x, real_t y, real_t z) {
    meas.set_field<GPS_Velocity>(UKF::Vector<3>(x, y, z));
}

void ukf_sensor_set_pitot_tas(real_t tas) {
    meas.set_field<Airspeed>(tas);
}

void ukf_sensor_set_barometer_amsl(real_t amsl) {
    meas.set_field<PressureAltitude>(amsl);
}

void ukf_set_params(struct ukf_ioboard_params_t *in) {
    local_mag_field << in->mag_field[0], in->mag_field[1], in->mag_field[2];
    ukf.measurement_covariance <<
        in->accel_covariance[0], in->accel_covariance[1], in->accel_covariance[2],
        in->gyro_covariance[0], in->gyro_covariance[1], in->gyro_covariance[2],
        in->mag_covariance[0], in->mag_covariance[1], in->mag_covariance[2],
        in->gps_position_covariance[0], in->gps_position_covariance[1], in->gps_position_covariance[2],
        in->gps_velocity_covariance[0], in->gps_velocity_covariance[1], in->gps_velocity_covariance[2],
        in->pitot_covariance,
        in->barometer_amsl_covariance;
}

void ukf_choose_dynamics(enum ukf_model_t t) {

}

void ukf_set_custom_dynamics_model(ukf_model_function_t func) {

}

void ukf_iterate(float dt, real_t control_vector[UKF_CONTROL_DIM]) {
    ukf.step(dt, meas);
}

void ukf_set_process_noise(real_t process_noise_covariance[SFWA_StateVector::covariance_size()]) {
    Eigen::Map<typename SFWA_StateVector::StateVectorDelta> covariance_map(process_noise_covariance);
    ukf.process_noise_covariance = SFWA_StateVector::CovarianceMatrix::Zero();
    ukf.process_noise_covariance.diagonal() << covariance_map;
}

uint32_t ukf_config_get_state_dim() {
    return SFWA_StateVector::covariance_size();
}

uint32_t ukf_config_get_measurement_dim() {
    return SFWA_MeasurementVector::max_size();
}

uint32_t ukf_config_get_control_dim() {
    return UKF_CONTROL_DIM;
}

enum ukf_precision_t ukf_config_get_precision() {
    return UKF_PRECISION_DOUBLE;
}

max_stars_repo_name: rafaelrietmann/ukf


text:
#include "csentence.h"
#include "common.h"
#include "globalVars.h"
#include "kbGraph.h"
#include "wdict.h"

// Tokenizer
#include <boost/tokenizer.hpp>
#include <boost/lexical_cast.hpp>

#include<boost/tuple/tuple.hpp> // for "tie"

namespace ukb {
	using namespace std;
	using namespace boost;

	static CWord::cwtype cast_int_cwtype(int i) {
		CWord::cwtype res;

		switch(i) {
		case 0:
			res = CWord::cw_ctxword;
			break;
		case 1:
			res = CWord::cw_tgtword;
			break;
		case 2:
			res = CWord::cw_concept;
			break;
		case 3:
			res = CWord::cw_tgtword_nopv;
			break;
		case 4:
			res = CWord::cw_ctxword_nopv;
			break;
		default:
			res = CWord::cw_error;
			break;
		}
		return res;
	}

	static bool cwtype_is_tgtword(CWord::cwtype t) {
		return t == CWord::cw_tgtword || t == CWord::cw_tgtword_nopv;
	}

	bool CWord::is_tgtword() const {
		return cwtype_is_tgtword(m_type);
	}

	void CWord::empty_synsets() {
		std::vector<std::string>().swap(m_syns);
		std::vector<std::pair<Kb::vertex_descriptor, float> >().swap(m_V);
		std::vector<float>().swap(m_ranks);
		m_linkw_factor = 1;
		m_disamb = false;
	}

	void CWord::swap(CWord & o) {
		m_w.swap(o.m_w);
		m_id.swap(o.m_id);
		m_pos.swap(o.m_pos);
		std::swap(m_weight, o.m_weight);
		m_syns.swap(o.m_syns);
		m_V.swap(o.m_V);
		m_ranks.swap(o.m_ranks);
		std::swap(m_linkw_factor, o.m_linkw_factor);
		std::swap(m_type, o.m_type);
		std::swap(m_disamb, o.m_disamb);
	}

	size_t CWord::link_dict_concepts(const string & lemma, const string & pos) {

		size_t new_c = 0;

		set<Kb::vertex_descriptor> syns_S;
		for(size_t i = 0, m = m_V.size(); i < m; ++i) {
			syns_S.insert(m_V[i].first);
		}

		WDict_entries entries = WDict::instance().get_entries(lemma, pos);
		if (!entries.size()) return 0;

		vector<size_t> sidxV; // synset index vector
		for(size_t i = 0, m = entries.size(); i < m; ++i)
			sidxV.push_back(i);

		if(glVars::dict::use_shuffle) {
			// Shuffle index vector
			boost::random_number_generator<boost::mt19937, long int> rand_dist(glVars::rnd::urng);
			std::random_shuffle(sidxV.begin(), sidxV.end(), rand_dist);
		}

		for(size_t i= 0, m = sidxV.size(); i < m; ++i) {
			size_t idx = sidxV[i];
			Kb::vertex_descriptor syn_v = entries.get_entry(idx);
			if (!syns_S.insert(syn_v).second) continue; // Synset previously there
			const string & syn_str = entries.get_entry_str(idx);
			float syn_freq = entries.get_freq(idx);
			m_syns.push_back(syn_str);
			m_V.push_back(make_pair(syn_v, syn_freq));
			new_c++;
		}

		if (new_c == 0) return 0;

		// (Re)calculate m_linkw_factor
		float wlink = 0.0;
		for(vector<pair<Kb::vertex_descriptor, float> >::iterator it = m_V.begin(), end = m_V.end();
			it != end; ++it) wlink += it->second;
		if (wlink == 0) return 0;
		m_linkw_factor = 1.0 / wlink;

		// Update ranks
		vector<float>(m_syns.size(), 0.0).swap(m_ranks);
		return new_c;
	}

	CWord::CWord(const string & w_, const string & id_, const string & pos_, cwtype type_, float wght_)
		: m_w(w_), m_id(id_), m_pos(pos_), m_weight(wght_), m_type(type_) {

		string empty_str;

		if (m_type == cw_concept) {
			Kb::vertex_descriptor u;
			bool P;
			tie(u, P) = ukb::Kb::instance().get_vertex_by_name(m_w);
			if (!P) {
				throw std::logic_error("CWord concept " + m_w + " not in KB");
			}
			m_syns.push_back(m_w);
			m_V.push_back(make_pair(u, 1.0f));
			m_ranks.push_back(0.0f);
			m_linkw_factor = 1.0;
			return;
		}
		m_linkw_factor = 0;

		if (m_type == cw_tgtword_nopv or m_type == cw_ctxword_nopv) return; // if nopv we are done

		// m_type == cw_ctxword or cw_tgtword:
		if (!link_dict_concepts(m_w, m_pos)) {
			// empty CWord
			empty_synsets();
		}
		m_disamb = (1 == m_syns.size()); // monosemous words are disambiguated
	}

	void CWord::attach_lemma(const string & lemma, const string & pos) {
		if (!m_w.size())
			throw std::logic_error("CWord::attach_lemma error: can't attach lemma to an empty CWord.");
		if (m_type == cw_concept)
			throw std::logic_error("CWord::attach_lemma error: can't attach lemma to a CWord of type cw_concept.");
		std::string npos = pos;
		if (!npos.size()) npos = std::string("#");
		link_dict_concepts(lemma, npos);
	}

	// (Re)Set cword and link it to new concepts.
	// (used in tgt_noppv type cwords)
	// return: number of new concepts

	size_t CWord::set_concepts(map<string, float> & C) {

		std::vector<std::string> syns;
		std::vector<std::pair<Kb::vertex_descriptor, float> > V;
		Kb::vertex_descriptor u;
		bool P;
		float total_w = 0.0f;
		size_t N = 0;
		for(map<string, float>::iterator it = C.begin(), end = C.end();
			it != end; ++it) {
			tie(u, P) = ukb::Kb::instance().get_vertex_by_name(it->first);
			if (!P) {
				if (glVars::debug::warning)
					// No synset for that word.
					cerr << "reset_concepts: " + it->first + " not in KB\n";
				continue;
			}
			N++;
			syns.push_back(it->first);
			V.push_back(make_pair(u, it->second));
			total_w += it->second;
		}
		if (total_w == 0.0f) {
			this->empty_synsets();
			return 0;
		}

		// Update concepts
		m_linkw_factor = 1.0 / total_w;
		m_syns.swap(syns);
		m_V.swap(V);
		vector<float>(N, 0.0).swap(m_ranks);
		m_disamb = (N == 1); // monosemous words are disambiguated
		return N;
	}

	CWord & CWord::operator=(const CWord & cw_) {
		if (&cw_ != this) {
			m_w = cw_.m_w;
			m_id = cw_.m_id;
			m_weight = cw_.m_weight;
			m_pos = cw_.m_pos;
			m_syns = cw_.m_syns;
			m_V = cw_.m_V;
			m_ranks = cw_.m_ranks;
			m_disamb = cw_.m_disamb;
		}
		return *this;
	}

	string CWord::wpos() const {

		if (is_synset()) return word();
		string wpos(word());
		if(m_pos.size() == 0) return wpos;
		wpos.append("#");
		wpos.append(m_pos);
		return wpos;
	};

	ostream & CWord::debug(ostream & o) const  {

		o << m_w << "#";
		if (m_pos.size())
			o << m_pos;
		o << "#" << m_id << "#" << m_type;
		o << "#" << lexical_cast<string>(m_weight) << "#" << lexical_cast<string>(m_linkw_factor) << "\t";
		for(size_t i = 0; i < m_syns.size(); i++) {
			o << "[" << m_syns[i] << ", " << m_V[i].first << ", " << m_V[i].second << "] ";
		}
		return o;
	}

	struct CWSort {

		CWSort(const vector<float> & _v) : v(_v) {}
		int operator () (const int & i, const int & j) {
			// Descending order
			return v[i] > v[j];
		}
		const vector<float> & v;
	};

	void CWord::disamb_cword() {

		size_t n = m_syns.size();
		if (!n) return;
		if (n == 1) {
			m_disamb = true; // Monosemous words are disambiguated
			return;
		}

		vector<string> syns(n);
		vector<float> ranks(n);
		vector<int> idx(n);

		for(size_t i=0; i < n; ++i)
			idx[i] = i;
		sort(idx.begin(), idx.end(), CWSort(m_ranks));

		for(size_t i=0; i < n; ++i) {
			syns[i]  = m_syns[idx[i]];
			ranks[i] = m_ranks[idx[i]];
		}
		if(ranks[0] == ranks[n-1]) return; // If all ranks have same value the word is not disambiguated
		syns.swap(m_syns);
		ranks.swap(m_ranks);
		m_disamb = true;
	}

	std::ostream& operator<<(std::ostream & o, const CWord & cw_) {

		//Kb::boost_graph_t & g = ukb::Kb::instance().graph();

		return cw_.write(o, cw_.m_id, cw_.m_type);
	}

	std::ostream & CWord::write(std::ostream & o, const string & id_, CWord::cwtype t_) const {

		o << m_w << "#";
		if (m_pos.size())
			o << m_pos;
		o << "#" << id_ << "#" << t_;
		return o;
	}

	ostream & cw_aw_print_best(ostream & o,
							   const vector<string> & syns,
							   const vector<float> & ranks) {
		size_t n = syns.size();
		o << " " << syns[0];
		for(size_t i = 1; i != n; ++i) {
			if (ranks[i] != ranks[0]) break;
			o << " " << syns[i];
		}
		return o;
	}

	ostream & cw_aw_print_all(ostream & o,
							  const vector<string> & syns,
							  const vector<float> & ranks) {
		float norm_factor = 1.0;
		if (glVars::output::norm_ranks) {
			float rsum = 0.0;
			for(size_t i = 0; i != syns.size(); ++i) {
				rsum += ranks[i];
			}
			if (rsum) norm_factor *= 1.0 / rsum;
		}
		for(size_t i = 0; i != syns.size(); ++i) {
			o << " " << syns[i] << "/" << ranks[i]*norm_factor;
		}
		return o;
	}

	ostream & CWord::print_cword(ostream & o, const std::string &id) const {

		o << id << " ";
		if(!glVars::output::allranks) cw_aw_print_best(o, m_syns, m_ranks);
		else cw_aw_print_all(o, m_syns, m_ranks);
		o << " !! " << m_w << "\n";
		return o;
	}

	////////////////////////////////////////////////////////////////
	// CSentence

	CSentence::CSentence(const std::string & id, const std::string & ctx_str) :
		m_tgtN(0), m_weight(0.0), m_w_factor(0.0f), m_id(id) {
		if (m_id.empty()) throw std::runtime_error(string("empty id"));
		vector<string> ctx;
		vector<CWord> V;
		char_separator<char> sep(" \t");
		tokenizer<char_separator<char> > tok_ctx(ctx_str, sep);
		copy(tok_ctx.begin(), tok_ctx.end(), back_inserter(ctx));
		if (ctx.size() == 0) return;
		try {
			push_ctx(ctx);
		} catch (ukb::wdict_error & e) {
			throw e;
		} catch (std::exception & e) {
			throw std::runtime_error(string("context error: ") + e.what());
		}
	}

	struct ctw_parse_t {
		string lemma;
		string pos;
		string id;
		int dist; // See cwtype enum.
		float w;

		ctw_parse_t() : lemma(), pos(), id(), dist(0), w(1.0) {}

	};

	ctw_parse_t parse_ctw(const string & word) {

		ctw_parse_t res;

		char_separator<char> wsep("#", "", keep_empty_tokens);
		tokenizer<char_separator<char> > wtok(word, wsep);
		tokenizer<char_separator<char> >::iterator it = wtok.begin();
		tokenizer<char_separator<char> >::iterator end = wtok.end();
		if (it == end) return res; // empty line
		vector<string> fields;
		copy(it, end, back_inserter(fields));
		size_t m = fields.size();
		if (m != 4 && m != 5) {
			throw std::logic_error(word + " : too few fields.");
		}
		res.lemma = fields[0];
		res.pos = fields[1];
		if (!glVars::input::filter_pos || !res.pos.size()) res.pos = std::string("#");
		res.id = fields[2];
		try {
			res.dist = lexical_cast<int>(fields[3]);
			if (m == 5)
				res.w = lexical_cast<float>(fields[4]);
		} catch (boost::bad_lexical_cast &) {
			throw std::logic_error(word + " : Parsing error.");
		}

		if (res.w < 0.0) {
			throw std::logic_error(word + " : Negative weight.");
		}
		return res;
	}

	// NOTE: destroys new_cw
	void CSentence::push_cw(CWord & new_cw,
							 map<string, size_t > & CW,
							 bool is_nopv) {
		map<string, size_t >::iterator cwit;
		bool P;
		string id = new_cw.id();
		CWord::cwtype type = new_cw.type();
		bool is_tgtword = new_cw.is_tgtword();
		float w = new_cw.m_weight;

		tie(cwit, P) = CW.insert(make_pair(new_cw.wpos(), m_vuniq.size()));
		if (P) {
			m_vuniq.push_back(CWord());
			m_vuniq.back().swap(new_cw);
		} else {
			CWord & old = m_vuniq[cwit->second];
			// check type compatibility
			bool old_is_nopv = old.type() == CWord::cw_tgtword_nopv || old.type() == CWord::cw_ctxword_nopv;
			if ((is_nopv && !old_is_nopv) ||
				(!is_nopv && old_is_nopv))
				throw std::logic_error(string("push error: word " + old.wpos() + " has incompatible types"));
			// if current is target word, set original to target word
			if (is_tgtword)
				old.m_type = type;
			old.m_weight += w; // aggregate weights
		}
		m_tokens.push_back(cwtoken_t(id, type, cwit->second));
		if (is_tgtword) m_tgtN++;
		m_weight += w;
	}

	void CSentence::push_ctx(const vector<string> & ctx) {

		map<string, size_t > CW; // words inserted so far
		bool last_is_nopv = false;
		CWord last_nopv;
		map<string, float> nopv_concepts;
		vector<string>::const_iterator end = ctx.end();
		for(vector<string>::const_iterator it = ctx.begin();
			it != end or last_is_nopv; ++it) {
			try {
				if (it == end) {
					// last check to fill last nopv
					last_is_nopv = false;
					if (last_nopv.set_concepts(nopv_concepts)) { // false means no concepts attached
						map<string, float>().swap(nopv_concepts);
						push_cw(last_nopv, CW, true);
					}
					break;
				}
				ctw_parse_t ctwp = parse_ctw(*it);
				if (ctwp.lemma.size() == 0) {
					throw std::logic_error(*it + " has no lemma.");
				}
				CWord::cwtype cw_type = cast_int_cwtype(ctwp.dist);
				if (cw_type == CWord::cw_error) {
					throw std::logic_error(*it + " fourth field is invalid.");
				}
				if(!glVars::input::weight)
					ctwp.w = 1.0;

				if (last_is_nopv) {
					// there is a nopv element 'active'
					if (cw_type == CWord::cw_concept) {
						// attach concept to nopv
						nopv_concepts.insert(make_pair(ctwp.lemma, ctwp.w));
						continue;
					}
					last_is_nopv = false;
					// new elem is not concept, so push last nopv
					if (last_nopv.set_concepts(nopv_concepts)) {
						map<string, float>().swap(nopv_concepts);
						--it; // if push_cw throws, make sure current word is processed again
						push_cw(last_nopv, CW, true);
						++it; // did not throw anyway
					}
				}

				// last_is_nopv == false
				CWord new_cw(ctwp.lemma, ctwp.id, ctwp.pos, cw_type, ctwp.w);
				if (cw_type == CWord::cw_tgtword_nopv or cw_type == CWord::cw_ctxword_nopv) {
					// New nopv cword
					new_cw.swap(last_nopv);
					last_is_nopv = true;
					continue;
				}
				if (!new_cw.size()) {
					if (glVars::debug::warning)
						// No synset for that word.
						cerr << "W:" << *it << " can't be mapped to KB.\n";
					continue;
				}
				push_cw(new_cw, CW, false);
			} catch (ukb::wdict_error & e) {
				throw e;
			} catch (std::logic_error & e) {
				string msg(e.what());
				if (!glVars::input::swallow) throw std::runtime_error(msg);
				if (glVars::debug::warning) {
					cerr << msg << "\n";
				}
			}
		}
		if (m_vuniq.size()) {
			if (m_weight == 0.0)
				throw std::logic_error(string("Context with zero weight"));
			m_w_factor = 1.0 / m_weight;
		}
	}

	std::ostream& operator<<(std::ostream & o, const CSentence & cs_) {
		o << cs_.m_id << endl;
		vector<CSentence::cwtoken_t>::const_iterator cw_it = cs_.m_tokens.begin();
		vector<CSentence::cwtoken_t>::const_iterator cw_end = cs_.m_tokens.end();

		cw_end--;
		for(; cw_it != cw_end; ++cw_it) {
			cs_.m_vuniq[cw_it->idx].write(o, cw_it->id, cw_it->t);
			o << " ";
		}
		cs_.m_vuniq[cw_end->idx].write(o, cw_end->id, cw_end->t);
		o << " ";
		return o;
	}

	std::ostream & CSentence::debug(std::ostream & o) const {
		o << m_id << " tgtN:" << lexical_cast<string>(m_tgtN) <<
			" W:" << lexical_cast<string>(m_weight) << "(" << lexical_cast<string>(m_w_factor) << ")" << endl;
		o << "Unique:\n";
		for(vector<CWord>::const_iterator it = m_vuniq.begin(), end=m_vuniq.end();
			it != end; ++it) {
			o << "  ";
			it->debug(o);
			o << endl;
		}
		o << "Tokens:" << endl;
		vector<cwtoken_t>::const_iterator cw_it = m_tokens.begin();
		vector<cwtoken_t>::const_iterator cw_end = m_tokens.end();
		for(; cw_it != cw_end; ++cw_it) {
			o << "[" << cw_it->idx << ", " << cw_it->id << ", " << cw_it->t << "] ";
		}
		o << endl;
		return o;
	}

	std::ostream & CSentence::print_csent(std::ostream & o) const {

		if (!m_tgtN) return o;

		vector<cwtoken_t>::const_iterator cw_it = m_tokens.begin();
		vector<cwtoken_t>::const_iterator cw_end = m_tokens.end();

		for(; cw_it != cw_end; ++cw_it) {
			const CWord & cw = m_vuniq[cw_it->idx];
			if (cw.size() == 0) continue;
			if (!cwtype_is_tgtword(cw_it->t)) continue;
			if (!cw.is_disambiguated() && !glVars::output::ties) continue;
			if (cw.is_monosemous() && !glVars::output::monosemous) continue; // Don't output monosemous words
			o << m_id << " ";
			cw.print_cword(o, cw_it->id);
		}
		return o;
	}

	////////////////////////////////////////////////////////////////////////////////
	//
	// PageRank in Kb

	// Update pv of synsets pointed by CWord of token type

	// cw is the cword node
	// For each v pointed in V, (i.e., cw->v should be in the graph), init the PV with:
	//
	// PV[v] += normalized_cw_w * e[cw->v] / Sum_{cw->u}(e[cw->u]) =
	//        = factor * e[cw->v]
	//
	// Note:
	//
	// Can not be used when glVars::prank::poslightw is set

	size_t update_pv_cw(const vector<pair<Kb::vertex_descriptor, float> > & m_V,
						float factor,
						vector<float> & pv) {

		// Sum edge weights

		size_t inserted = 0;
		// Uppdate PV
		for(vector<pair<Kb::vertex_descriptor, float> >::const_iterator it = m_V.begin(), end = m_V.end();
			it != end; ++it) {
			inserted++;
			pv[it->first] += it->second * factor;
		}
		return inserted;
	}

	// Get personalization vector giving an csentence. onlyC variant.

	int pv_from_cs_onlyC(const CSentence & cs,
						 vector<float> & pv,
						 CSentence::const_iterator exclude_word_it) {

		Kb & kb = ukb::Kb::instance();

		if (kb.size() == pv.size()) {
			std::fill(pv.begin(), pv.end(), 0.0);
		} else {
			vector<float> (kb.size(), 0.0).swap(pv);
		}

		Kb::vertex_descriptor u;
		int inserted_i = 0;

		// put pv to the synsets of words
		for(vector<CWord>::const_iterator it = cs.ubegin(), end = cs.uend();
			it != end; ++it) {
			if (it == exclude_word_it) continue;
			const CWord & cw = *it;
			float cw_w = cw.get_weight() * cs.weigth_factor();
			if (cw.type() == CWord::cw_concept) {
				u = cw.V_vector().at(0).first;
				pv[u] += cw_w;
				inserted_i++;
			} else {
				inserted_i += update_pv_cw(cw.V_vector(),
										   cw_w * cw.get_linkw_factor(),
										   pv);
			}
		}
		return inserted_i;
	}

	// Given a CSentence apply Personalized PageRank and obtain obtain it's
	// Personalized PageRank Vector (PPV)
	//
	// Initial V is computed by activating the words of the context

	bool calculate_kb_ppr(const CSentence & cs,
						  vector<float> & ranks) {

		return calculate_kb_ppr_by_word(cs, cs.uend(), ranks);
	}

	// given a word (pointed by tgtw_it),
	// 1. put a ppv in the synsets of the rest of words.
	// 2. Pagerank
	//
	// Note: tgtw_it can point to end(), effectively turning this function into
	// a 'standard' PPR

	bool calculate_kb_ppr_by_word(const CSentence & cs,
								  CSentence::const_iterator tgtw_it,
								  vector<float> & ranks) {

		Kb & kb = ukb::Kb::instance();
		vector<float> pv;
		int aux = pv_from_cs_onlyC(cs, pv, tgtw_it);
		// Execute PageRank
		if (aux) {
			kb.pageRank_ppv(pv, ranks);
			if (glVars::csentence::disamb_minus_static) {
				const vector<float> & staticV = kb.static_prank();
				for(size_t i = 0, n = staticV.size();
					i != n; ++i) {
					ranks[i] = staticV[i] - ranks[i];
				}
			}
		}
		return aux;
	}

	//
	// Given a previously disambiguated CSentence (all synsets of words
	// have a rank), calculate a kb prgaRank where PPV is formed by
	// 'activating' just the synsets of CWords with appropiate
	// (normalized) rank
	//

	bool calculate_kb_ppv_csentence(CSentence & cs, vector<float> & res) {

		if (!cs.has_tgtwords()) return false; // no target words

		Kb & kb = ukb::Kb::instance();
		bool aux;

		// Initialize result vector
		vector<float> (kb.size(), 0.0).swap(res);

		// Initialize PPV vector
		vector<float> ppv(kb.size(), 0.0);

		vector<CWord>::iterator cw_it = cs.ubegin();
		vector<CWord>::iterator cw_end = cs.uend();
		float K = 0.0;
		for(; cw_it != cw_end; ++cw_it) {
			for(size_t i = 0; i != cw_it->size(); ++i) {
				Kb::vertex_descriptor u;
				tie(u, aux) = kb.get_vertex_by_name(cw_it->syn(i));
				if (aux) {
					ppv[u] = cw_it->rank(i);
					K += cw_it->rank(i);
				}
			}
		}

		if (K == 0.0) return false;
		// Normalize PPV vector
		float div = 1.0 / K;
		for(vector<float>::iterator rit = ppv.begin(); rit != ppv.end(); ++rit)
			*rit *= div;

		// Execute PageRank
		kb.pageRank_ppv(ppv, res);
		return true;
	}

	//
	// Disambiguate a CSentence given a vector of ranks
	//

	bool disamb_csentence_kb(CSentence & cs,
							 const vector<float> & ranks) {

		if (!cs.has_tgtwords()) return false; // no target words

		vector<CWord>::iterator cw_it = cs.ubegin();
		vector<CWord>::iterator cw_end = cs.uend();
		for(; cw_it != cw_end; ++cw_it) {
			if (!cw_it->is_tgtword()) continue;
			cw_it->rank_synsets(ranks, false);
			cw_it->disamb_cword();
		}
		return true;
	}
}