<commit_before>//===- fpcmp.cpp - A fuzzy "cmp" that permits floating point noise --------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file was developed by the LLVM research group and is distributed under
// the University of Illinois Open Source License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// fpcmp is a tool that basically works like the 'cmp' tool, except that it can
// tolerate errors due to floating point noise, with the -r option.
//
//===----------------------------------------------------------------------===//

#include "Support/CommandLine.h"
#include "Support/FileUtilities.h"
#include "Config/fcntl.h"
#include "Config/sys/mman.h"
#include <iostream>
#include <cmath>

using namespace llvm;

namespace {
  cl::opt<std::string>
  File1(cl::Positional, cl::desc("<input file #1>"), cl::Required);
  cl::opt<std::string>
  File2(cl::Positional, cl::desc("<input file #2>"), cl::Required);

  cl::opt<double>
  RelTolerance("r", cl::desc("Relative error tolerated"), cl::init(0));
  cl::opt<double>
  AbsTolerance("a", cl::desc("Absolute error tolerated"), cl::init(0));
}

/// OpenFile - mmap the specified file into the address space for reading, and
/// return the length and address of the buffer.
static void OpenFile(const std::string &Filename, unsigned &Len, char* &BufPtr){
  int FD = open(Filename.c_str(), O_RDONLY);
  if (FD == -1 || (Len = getFileSize(Filename)) == ~0U) {
    std::cerr << "Error: cannot open file '" << Filename << "'\n";
    exit(2);
  }

  // mmap in the file all at once...
  BufPtr = (char*)mmap(0, Len, PROT_READ, MAP_PRIVATE, FD, 0);

  if (BufPtr == (char*)MAP_FAILED) {
    std::cerr << "Error: cannot open file '" << Filename << "'\n";
    exit(2);
  }
}

static bool isNumberChar(char C) {
  switch (C) {
  case '0': case '1': case '2': case '3': case '4':
  case '5': case '6': case '7': case '8': case '9':
  case '.': case '+': case '-':
  case 'e':
  case 'E': return true;
  default: return false;
  }
}

static char *BackupNumber(char *Pos, char *FirstChar) {
  while (Pos > FirstChar && isNumberChar(Pos[-1]))
    --Pos;
  return Pos;
}

static void CompareNumbers(char *&F1P, char *&F2P, char *F1End, char *F2End) {
  char *F1NumEnd, *F2NumEnd;
  double V1 = strtod(F1P, &F1NumEnd);
  double V2 = strtod(F2P, &F2NumEnd);

  if (F1NumEnd == F1P || F2NumEnd == F2P) {
    std::cerr << "Comparison failed, not a numeric difference.\n";
    exit(1);
  }

  // Check to see if these are inside the absolute tolerance
  if (AbsTolerance < std::abs(V1-V2)) {
    // Nope, check the relative tolerance...
    double Diff;
    if (V2)
      Diff = std::abs(V1/V2 - 1.0);
    else if (V1)
      Diff = std::abs(V2/V1 - 1.0);
    else
      Diff = 0;  // Both zero.
    if (Diff > RelTolerance) {
      std::cerr << "Compared: " << V1 << " and " << V2 << ": diff = "
                << Diff << "\n";
      std::cerr << "Out of tolerance: rel/abs: " << RelTolerance
                << "/" << AbsTolerance << "\n";
      exit(1);
    }
  }

  // Otherwise, advance our read pointers to the end of the numbers.
  F1P = F1NumEnd;  F2P = F2NumEnd;
}

int main(int argc, char **argv) {
  cl::ParseCommandLineOptions(argc, argv);

  // mmap in the files.
  unsigned File1Len, File2Len;
  char *File1Start, *File2Start;
  OpenFile(File1, File1Len, File1Start);
  OpenFile(File2, File2Len, File2Start);

  // Okay, now that we opened the files, scan them for the first difference.
  char *File1End = File1Start+File1Len;
  char *File2End = File2Start+File2Len;
  char *F1P = File1Start;
  char *F2P = File2Start;

  while (1) {
    // Scan for the end of file or first difference.
    while (F1P < File1End && F2P < File2End && *F1P == *F2P)
      ++F1P, ++F2P;

    if (F1P >= File1End || F2P >= File2End) break;

    // Okay, we must have found a difference.  Backup to the start of the
    // current number each stream is at so that we can compare from the
    // beginning.
    F1P = BackupNumber(F1P, File1Start);
    F2P = BackupNumber(F2P, File2Start);

    // Now that we are at the start of the numbers, compare them, exiting if
    // they don't match.
    CompareNumbers(F1P, F2P, File1End, File2End);
  }

  // Okay, we reached the end of file.  If both files are at the end, we
  // succeeded.
  if (F1P >= File1End && F2P >= File2End) return 0;

  // Otherwise, we might have run off the end due to a number, backup and retry.
  F1P = BackupNumber(F1P, File1Start);
  F2P = BackupNumber(F2P, File2Start);

  // Now that we are at the start of the numbers, compare them, exiting if
  // they don't match.
  CompareNumbers(F1P, F2P, File1End, File2End);

  // If we found the end, we succeeded.
  if (F1P >= File1End && F2P >= File2End) return 0;

  return 1;
}

<commit_msg>mmap of a zero length file returns null on some platforms, so hack around it.<commit_after>//===- fpcmp.cpp - A fuzzy "cmp" that permits floating point noise --------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file was developed by the LLVM research group and is distributed under
// the University of Illinois Open Source License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// fpcmp is a tool that basically works like the 'cmp' tool, except that it can
// tolerate errors due to floating point noise, with the -r option.
//
//===----------------------------------------------------------------------===//

#include "Support/CommandLine.h"
#include "Support/FileUtilities.h"
#include "Config/fcntl.h"
#include "Config/sys/mman.h"
#include <iostream>
#include <cmath>

using namespace llvm;

namespace {
  cl::opt<std::string>
  File1(cl::Positional, cl::desc("<input file #1>"), cl::Required);
  cl::opt<std::string>
  File2(cl::Positional, cl::desc("<input file #2>"), cl::Required);

  cl::opt<double>
  RelTolerance("r", cl::desc("Relative error tolerated"), cl::init(0));
  cl::opt<double>
  AbsTolerance("a", cl::desc("Absolute error tolerated"), cl::init(0));
}

/// OpenFile - mmap the specified file into the address space for reading, and
/// return the length and address of the buffer.
static void OpenFile(const std::string &Filename, unsigned &Len, char* &BufPtr){
  int FD = open(Filename.c_str(), O_RDONLY);
  if (FD == -1 || (Len = getFileSize(Filename)) == ~0U) {
    std::cerr << "Error: cannot open file '" << Filename << "'\n";
    exit(2);
  }

  // mmap in the file all at once...
  BufPtr = (char*)mmap(0, Len, PROT_READ, MAP_PRIVATE, FD, 0);

  if (BufPtr == (char*)MAP_FAILED) {
    std::cerr << "Error: cannot open file '" << Filename << "'\n";
    exit(2);
  }

  // If mmap decided that the files were empty, it might have returned a
  // null pointer. If so, make a new, fake pointer -- it shouldn't matter
  // what it contains, because Len is 0, and it should never be read.
  if (BufPtr == 0 && Len == 0)
    BufPtr = new char[1];
}

static bool isNumberChar(char C) {
  switch (C) {
  case '0': case '1': case '2': case '3': case '4':
  case '5': case '6': case '7': case '8': case '9':
  case '.': case '+': case '-':
  case 'e':
  case 'E': return true;
  default: return false;
  }
}

static char *BackupNumber(char *Pos, char *FirstChar) {
  while (Pos > FirstChar && isNumberChar(Pos[-1]))
    --Pos;
  return Pos;
}

static void CompareNumbers(char *&F1P, char *&F2P, char *F1End, char *F2End) {
  char *F1NumEnd, *F2NumEnd;
  double V1 = strtod(F1P, &F1NumEnd);
  double V2 = strtod(F2P, &F2NumEnd);

  if (F1NumEnd == F1P || F2NumEnd == F2P) {
    std::cerr << "Comparison failed, not a numeric difference.\n";
    exit(1);
  }

  // Check to see if these are inside the absolute tolerance
  if (AbsTolerance < std::abs(V1-V2)) {
    // Nope, check the relative tolerance...
    double Diff;
    if (V2)
      Diff = std::abs(V1/V2 - 1.0);
    else if (V1)
      Diff = std::abs(V2/V1 - 1.0);
    else
      Diff = 0;  // Both zero.
    if (Diff > RelTolerance) {
      std::cerr << "Compared: " << V1 << " and " << V2 << ": diff = "
                << Diff << "\n";
      std::cerr << "Out of tolerance: rel/abs: " << RelTolerance
                << "/" << AbsTolerance << "\n";
      exit(1);
    }
  }

  // Otherwise, advance our read pointers to the end of the numbers.
  F1P = F1NumEnd;  F2P = F2NumEnd;
}

int main(int argc, char **argv) {
  cl::ParseCommandLineOptions(argc, argv);

  // mmap in the files.
  unsigned File1Len, File2Len;
  char *File1Start, *File2Start;
  OpenFile(File1, File1Len, File1Start);
  OpenFile(File2, File2Len, File2Start);

  // Okay, now that we opened the files, scan them for the first difference.
  char *File1End = File1Start+File1Len;
  char *File2End = File2Start+File2Len;
  char *F1P = File1Start;
  char *F2P = File2Start;

  while (1) {
    // Scan for the end of file or first difference.
    while (F1P < File1End && F2P < File2End && *F1P == *F2P)
      ++F1P, ++F2P;

    if (F1P >= File1End || F2P >= File2End) break;

    // Okay, we must have found a difference.  Backup to the start of the
    // current number each stream is at so that we can compare from the
    // beginning.
    F1P = BackupNumber(F1P, File1Start);
    F2P = BackupNumber(F2P, File2Start);

    // Now that we are at the start of the numbers, compare them, exiting if
    // they don't match.
    CompareNumbers(F1P, F2P, File1End, File2End);
  }

  // Okay, we reached the end of file.  If both files are at the end, we
  // succeeded.
  if (F1P >= File1End && F2P >= File2End) return 0;

  // Otherwise, we might have run off the end due to a number, backup and retry.
  F1P = BackupNumber(F1P, File1Start);
  F2P = BackupNumber(F2P, File2Start);

  // Now that we are at the start of the numbers, compare them, exiting if
  // they don't match.
  CompareNumbers(F1P, F2P, File1End, File2End);

  // If we found the end, we succeeded.
  if (F1P >= File1End && F2P >= File2End) return 0;

  return 1;
}

<|endoftext|>


<commit_before>/*
 * Chromaprint -- Audio fingerprinting toolkit
 * Copyright (C) 2011  Lukas Lalinsky <lalinsky@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
 * USA
 */

#include "utils.h"
#include "fft_lib_vdsp.h"

using namespace std;
using namespace Chromaprint;

FFTLib::FFTLib(int frame_size, double *window)
	: m_frame_size(frame_size),
	  m_window(window)
{
	double log2n = log2(frame_size);
	assert(log2n == int(log2n));
	m_log2n = int(log2n);
	m_input = new float[frame_size];
	m_a.realp = new float[frame_size / 2];
	m_a.imagp = new float[frame_size / 2];
	m_setup = vDSP_create_fftsetup(m_log2n, 0);
}

FFTLib::~FFTLib()
{
	vDSP_destroy_fftsetup(m_setup);
	delete[] m_a.realp;
	delete[] m_a.imagp;
	delete[] m_input;
}

void FFTLib::ComputeFrame(CombinedBuffer<short>::Iterator input, double *output)
{
	ApplyWindow(input, m_window, m_input, m_frame_size, 1.0);
	// XXX we can avoid this ctoz call by changing ApplyWindow, is it worth it?
	vDSP_ctoz((DSPComplex *)m_input, 2, &m_a, 1, m_frame_size / 2);
	vDSP_fft_zrip(m_setup, &m_a, 1, m_log2n, FFT_FORWARD);
	output[0] = m_a.realp[0] * m_a.realp[0];
	output[m_frame_size / 2] = m_a.imagp[0] * m_a.imagp[0];
	output += 1;
	for (int i = 1; i < m_frame_size / 2; i++) {
		*output++ = m_a.realp[i] * m_a.realp[i] + m_a.imagp[i] * m_a.imagp[i];
	}
}

<commit_msg>Reordered initialization list to remove compiler warning<commit_after>/*
 * Chromaprint -- Audio fingerprinting toolkit
 * Copyright (C) 2011  Lukas Lalinsky <lalinsky@gmail.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
 * USA
 */

#include "utils.h"
#include "fft_lib_vdsp.h"

using namespace std;
using namespace Chromaprint;

FFTLib::FFTLib(int frame_size, double *window)
	: m_window(window),
	  m_frame_size(frame_size)
{
	double log2n = log2(frame_size);
	assert(log2n == int(log2n));
	m_log2n = int(log2n);
	m_input = new float[frame_size];
	m_a.realp = new float[frame_size / 2];
	m_a.imagp = new float[frame_size / 2];
	m_setup = vDSP_create_fftsetup(m_log2n, 0);
}

FFTLib::~FFTLib()
{
	vDSP_destroy_fftsetup(m_setup);
	delete[] m_a.realp;
	delete[] m_a.imagp;
	delete[] m_input;
}

void FFTLib::ComputeFrame(CombinedBuffer<short>::Iterator input, double *output)
{
	ApplyWindow(input, m_window, m_input, m_frame_size, 1.0);
	// XXX we can avoid this ctoz call by changing ApplyWindow, is it worth it?
	vDSP_ctoz((DSPComplex *)m_input, 2, &m_a, 1, m_frame_size / 2);
	vDSP_fft_zrip(m_setup, &m_a, 1, m_log2n, FFT_FORWARD);
	output[0] = m_a.realp[0] * m_a.realp[0];
	output[m_frame_size / 2] = m_a.imagp[0] * m_a.imagp[0];
	output += 1;
	for (int i = 1; i < m_frame_size / 2; i++) {
		*output++ = m_a.realp[i] * m_a.realp[i] + m_a.imagp[i] * m_a.imagp[i];
	}
}

<|endoftext|>


<commit_before>/*

Copyright (c) 2003-2008, Arvid Norberg
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:

    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in
      the documentation and/or other materials provided with the distribution.
    * Neither the name of the author nor the names of its
      contributors may be used to endorse or promote products derived
      from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

*/

#include "libtorrent/pch.hpp"

#include "libtorrent/file_storage.hpp"
#include "libtorrent/utf8.hpp"
#include <boost/bind.hpp>

namespace libtorrent
{
	file_storage::file_storage()
		: m_piece_length(0)
		, m_total_size(0)
		, m_num_pieces(0)
	{}

	int file_storage::piece_size(int index) const
	{
		TORRENT_ASSERT(index >= 0 && index < num_pieces());
		if (index == num_pieces()-1)
		{
			int size = int(total_size()
				- size_type(num_pieces() - 1) * piece_length());
			TORRENT_ASSERT(size > 0);
			TORRENT_ASSERT(size <= piece_length());
			return int(size);
		}
		else
			return piece_length();
	}

	void file_storage::set_name(std::wstring const& n)
	{
		std::string utf8;
		wchar_utf8(n, utf8);
		m_name = utf8;
	}

	void file_storage::rename_file(int index, std::string const& new_filename)
	{
		TORRENT_ASSERT(index >= 0 && index < int(m_files.size()));
		m_files[index].path = new_filename;
	}

	void file_storage::rename_file(int index, std::wstring const& new_filename)
	{
		TORRENT_ASSERT(index >= 0 && index < int(m_files.size()));
		std::string utf8;
		wchar_utf8(new_filename, utf8);
		m_files[index].path = utf8;
	}

	file_storage::iterator file_storage::file_at_offset(size_type offset) const
	{
		// TODO: do a binary search
		std::vector<file_entry>::const_iterator i;
		for (i = begin(); i != end(); ++i)
		{
			if (i->offset <= offset && i->offset + i->size > offset)
				return i;
		}
		return i;
	}

	namespace
	{
		bool compare_file_offset(file_entry const& lhs, file_entry const& rhs)
		{
			return lhs.offset + lhs.size < rhs.offset + rhs.size;
		}
	}

	std::vector<file_slice> file_storage::map_block(int piece, size_type offset
		, int size) const
	{
		TORRENT_ASSERT(num_files() > 0);
		std::vector<file_slice> ret;

		// find the file iterator and file offset
		file_entry target;
		target.offset = piece * (size_type)m_piece_length + offset;
		target.size = 0;
		TORRENT_ASSERT(target.offset + size <= m_total_size);

		std::vector<file_entry>::const_iterator file_iter = std::upper_bound(
			begin(), end(), target, compare_file_offset);

		if (file_iter == end()) return ret;

		size_type file_offset = file_iter->offset;
		for (; size > 0; file_offset -= file_iter->size, ++file_iter)
		{
			TORRENT_ASSERT(file_iter != end());
			if (file_offset < file_iter->size)
			{
				file_slice f;
				f.file_index = file_iter - begin();
				f.offset = file_offset + file_iter->file_base;
				f.size = (std::min)(file_iter->size - file_offset, (size_type)size);
				size -= f.size;
				file_offset += f.size;
				ret.push_back(f);
			}

			TORRENT_ASSERT(size >= 0);
		}
		return ret;
	}

	peer_request file_storage::map_file(int file_index, size_type file_offset
		, int size) const
	{
		TORRENT_ASSERT(file_index < num_files());
		TORRENT_ASSERT(file_index >= 0);
		size_type offset = file_offset + at(file_index).offset;

		peer_request ret;
		ret.piece = int(offset / piece_length());
		ret.start = int(offset - ret.piece * piece_length());
		ret.length = size;
		return ret;
	}

	void file_storage::add_file(fs::wpath const& file, size_type size, int flags)
	{
		std::string utf8;
		wchar_utf8(file.string(), utf8);
		add_file(utf8, size, flags);
	}

	void file_storage::add_file(fs::path const& file, size_type size, int flags)
	{
		TORRENT_ASSERT(size >= 0);
#if BOOST_VERSION < 103600
		if (!file.has_branch_path())
#else
		if (!file.has_parent_path())
#endif
		{
			// you have already added at least one file with a
			// path to the file (branch_path), which means that
			// all the other files need to be in the same top
			// directory as the first file.
			TORRENT_ASSERT(m_files.empty());
			m_name = file.string();
		}
		else
		{
			if (m_files.empty())
				m_name = *file.begin();
		}
		TORRENT_ASSERT(m_name == *file.begin());
		m_files.push_back(file_entry());
		file_entry& e = m_files.back();
		e.size = size;
		e.path = file;
		e.offset = m_total_size;
		e.pad_file = bool(flags & pad_file);
		e.hidden_attribute = bool(flags & attribute_hidden);
		e.executable_attribute = bool(flags & attribute_executable);
		m_total_size += size;
	}

	void file_storage::add_file(file_entry const& ent)
	{
#if BOOST_VERSION < 103600
		if (!ent.path.has_branch_path())
#else
		if (!ent.path.has_parent_path())
#endif
		{
			// you have already added at least one file with a
			// path to the file (branch_path), which means that
			// all the other files need to be in the same top
			// directory as the first file.
			TORRENT_ASSERT(m_files.empty());
			m_name = ent.path.string();
		}
		else
		{
			if (m_files.empty())
				m_name = *ent.path.begin();
		}
		m_files.push_back(ent);
		file_entry& e = m_files.back();
		e.offset = m_total_size;
		m_total_size += ent.size;
	}

	void file_storage::optimize(int pad_file_limit)
	{
		// the main purpuse of padding is to optimize disk
		// I/O. This is a conservative memory page size assumption
		int alignment = 8*1024;

		// it doesn't make any sense to pad files that
		// are smaller than one piece
		if (pad_file_limit >= 0 && pad_file_limit < alignment)
			pad_file_limit = alignment;

		// put the largest file at the front, to make sure
		// it's aligned
		std::vector<file_entry>::iterator i = std::max_element(m_files.begin(), m_files.end()
			, boost::bind(&file_entry::size, _1) < boost::bind(&file_entry::size, _2));

		using std::iter_swap;
		iter_swap(i, m_files.begin());

		size_type off = 0;
		int padding_file = 0;
		for (std::vector<file_entry>::iterator i = m_files.begin();
			i != m_files.end(); ++i)
		{
			if (pad_file_limit >= 0
				&& (off & (alignment-1)) != 0
				&& i->size > pad_file_limit
				&& i->pad_file == false)
			{
				// if we have pad files enabled, and this file is
				// not piece-aligned and the file size exceeds the
				// limit, and it's not a padding file itself.
				// so add a padding file in front of it
				int pad_size = alignment - (off & (alignment-1));

				// find the largest file that fits in pad_size
				std::vector<file_entry>::iterator best_match = m_files.end();
				for (std::vector<file_entry>::iterator j = i+1; j < m_files.end(); ++j)
				{
					if (j->size > pad_size) continue;
					if (best_match == m_files.end() || j->size > best_match->size)
						best_match = j;
				}

				if (best_match != m_files.end())
				{
					// we found one
					file_entry e = *best_match;
					m_files.erase(best_match);
					i = m_files.insert(i, e);
					i->offset = off;
					off += i->size;
					continue;
				}

				// we could not find a file that fits in pad_size
				// add a padding file

				file_entry e;
				i = m_files.insert(i, e);
				i->size = pad_size;
				i->offset = off;
				i->file_base = 0;
				char name[10];
				sprintf(name, "%d", padding_file);
				i->path = *(i+1)->path.begin();
				i->path /= "_____padding_file_";
				i->path /= name;
				i->pad_file = true;
				off += pad_size;
				++padding_file;
				++i;
			}
			i->offset = off;
			off += i->size;
		}
		m_total_size = off;
	}
}

<commit_msg>map_file fix<commit_after>/*

Copyright (c) 2003-2008, Arvid Norberg
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:

    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in
      the documentation and/or other materials provided with the distribution.
    * Neither the name of the author nor the names of its
      contributors may be used to endorse or promote products derived
      from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

*/

#include "libtorrent/pch.hpp"

#include "libtorrent/file_storage.hpp"
#include "libtorrent/utf8.hpp"
#include <boost/bind.hpp>

namespace libtorrent
{
	file_storage::file_storage()
		: m_piece_length(0)
		, m_total_size(0)
		, m_num_pieces(0)
	{}

	int file_storage::piece_size(int index) const
	{
		TORRENT_ASSERT(index >= 0 && index < num_pieces());
		if (index == num_pieces()-1)
		{
			int size = int(total_size()
				- size_type(num_pieces() - 1) * piece_length());
			TORRENT_ASSERT(size > 0);
			TORRENT_ASSERT(size <= piece_length());
			return int(size);
		}
		else
			return piece_length();
	}

	void file_storage::set_name(std::wstring const& n)
	{
		std::string utf8;
		wchar_utf8(n, utf8);
		m_name = utf8;
	}

	void file_storage::rename_file(int index, std::string const& new_filename)
	{
		TORRENT_ASSERT(index >= 0 && index < int(m_files.size()));
		m_files[index].path = new_filename;
	}

	void file_storage::rename_file(int index, std::wstring const& new_filename)
	{
		TORRENT_ASSERT(index >= 0 && index < int(m_files.size()));
		std::string utf8;
		wchar_utf8(new_filename, utf8);
		m_files[index].path = utf8;
	}

	file_storage::iterator file_storage::file_at_offset(size_type offset) const
	{
		// TODO: do a binary search
		std::vector<file_entry>::const_iterator i;
		for (i = begin(); i != end(); ++i)
		{
			if (i->offset <= offset && i->offset + i->size > offset)
				return i;
		}
		return i;
	}

	namespace
	{
		bool compare_file_offset(file_entry const& lhs, file_entry const& rhs)
		{
			return lhs.offset < rhs.offset;
		}
	}

	std::vector<file_slice> file_storage::map_block(int piece, size_type offset
		, int size) const
	{
		TORRENT_ASSERT(num_files() > 0);
		std::vector<file_slice> ret;

		if (m_files.empty()) return ret;

		// find the file iterator and file offset
		file_entry target;
		target.offset = piece * (size_type)m_piece_length + offset;
		TORRENT_ASSERT(target.offset + size <= m_total_size);
		TORRENT_ASSERT(!compare_file_offset(target, m_files.front()));

		std::vector<file_entry>::const_iterator file_iter = std::upper_bound(
			begin(), end(), target, compare_file_offset);

		TORRENT_ASSERT(file_iter != begin());
		--file_iter;

		size_type file_offset = offset - file_iter->offset;
		for (; size > 0; file_offset -= file_iter->size, ++file_iter)
		{
			TORRENT_ASSERT(file_iter != end());
			if (file_offset < file_iter->size)
			{
				file_slice f;
				f.file_index = file_iter - begin();
				f.offset = file_offset + file_iter->file_base;
				f.size = (std::min)(file_iter->size - file_offset, (size_type)size);
				size -= f.size;
				file_offset += f.size;
				ret.push_back(f);
			}

			TORRENT_ASSERT(size >= 0);
		}
		return ret;
	}

	peer_request file_storage::map_file(int file_index, size_type file_offset
		, int size) const
	{
		TORRENT_ASSERT(file_index < num_files());
		TORRENT_ASSERT(file_index >= 0);
		size_type offset = file_offset + at(file_index).offset;

		peer_request ret;
		ret.piece = int(offset / piece_length());
		ret.start = int(offset - ret.piece * piece_length());
		ret.length = size;
		return ret;
	}

	void file_storage::add_file(fs::wpath const& file, size_type size, int flags)
	{
		std::string utf8;
		wchar_utf8(file.string(), utf8);
		add_file(utf8, size, flags);
	}

	void file_storage::add_file(fs::path const& file, size_type size, int flags)
	{
		TORRENT_ASSERT(size >= 0);
#if BOOST_VERSION < 103600
		if (!file.has_branch_path())
#else
		if (!file.has_parent_path())
#endif
		{
			// you have already added at least one file with a
			// path to the file (branch_path), which means that
			// all the other files need to be in the same top
			// directory as the first file.
			TORRENT_ASSERT(m_files.empty());
			m_name = file.string();
		}
		else
		{
			if (m_files.empty())
				m_name = *file.begin();
		}
		TORRENT_ASSERT(m_name == *file.begin());
		m_files.push_back(file_entry());
		file_entry& e = m_files.back();
		e.size = size;
		e.path = file;
		e.offset = m_total_size;
		e.pad_file = bool(flags & pad_file);
		e.hidden_attribute = bool(flags & attribute_hidden);
		e.executable_attribute = bool(flags & attribute_executable);
		m_total_size += size;
	}

	void file_storage::add_file(file_entry const& ent)
	{
#if BOOST_VERSION < 103600
		if (!ent.path.has_branch_path())
#else
		if (!ent.path.has_parent_path())
#endif
		{
			// you have already added at least one file with a
			// path to the file (branch_path), which means that
			// all the other files need to be in the same top
			// directory as the first file.
			TORRENT_ASSERT(m_files.empty());
			m_name = ent.path.string();
		}
		else
		{
			if (m_files.empty())
				m_name = *ent.path.begin();
		}
		m_files.push_back(ent);
		file_entry& e = m_files.back();
		e.offset = m_total_size;
		m_total_size += ent.size;
	}

	void file_storage::optimize(int pad_file_limit)
	{
		// the main purpuse of padding is to optimize disk
		// I/O. This is a conservative memory page size assumption
		int alignment = 8*1024;

		// it doesn't make any sense to pad files that
		// are smaller than one piece
		if (pad_file_limit >= 0 && pad_file_limit < alignment)
			pad_file_limit = alignment;

		// put the largest file at the front, to make sure
		// it's aligned
		std::vector<file_entry>::iterator i = std::max_element(m_files.begin(), m_files.end()
			, boost::bind(&file_entry::size, _1) < boost::bind(&file_entry::size, _2));

		using std::iter_swap;
		iter_swap(i, m_files.begin());

		size_type off = 0;
		int padding_file = 0;
		for (std::vector<file_entry>::iterator i = m_files.begin();
			i != m_files.end(); ++i)
		{
			if (pad_file_limit >= 0
				&& (off & (alignment-1)) != 0
				&& i->size > pad_file_limit
				&& i->pad_file == false)
			{
				// if we have pad files enabled, and this file is
				// not piece-aligned and the file size exceeds the
				// limit, and it's not a padding file itself.
				// so add a padding file in front of it
				int pad_size = alignment - (off & (alignment-1));

				// find the largest file that fits in pad_size
				std::vector<file_entry>::iterator best_match = m_files.end();
				for (std::vector<file_entry>::iterator j = i+1; j < m_files.end(); ++j)
				{
					if (j->size > pad_size) continue;
					if (best_match == m_files.end() || j->size > best_match->size)
						best_match = j;
				}

				if (best_match != m_files.end())
				{
					// we found one
					file_entry e = *best_match;
					m_files.erase(best_match);
					i = m_files.insert(i, e);
					i->offset = off;
					off += i->size;
					continue;
				}

				// we could not find a file that fits in pad_size
				// add a padding file

				file_entry e;
				i = m_files.insert(i, e);
				i->size = pad_size;
				i->offset = off;
				i->file_base = 0;
				char name[10];
				sprintf(name, "%d", padding_file);
				i->path = *(i+1)->path.begin();
				i->path /= "_____padding_file_";
				i->path /= name;
				i->pad_file = true;
				off += pad_size;
				++padding_file;
				++i;
			}
			i->offset = off;
			off += i->size;
		}
		m_total_size = off;
	}
}

<|endoftext|>


<commit_before>/**
  * Touhou Community Reliant Automatic Patcher
  * Tasogare Frontier support plugin
  *
  * ----
  *
  * Patching of ACT and NUT files.
  */

#include <thcrap.h>
#include <algorithm>
#include <libs/135tk/Act-Nut-lib/exports.hpp>
#include "thcrap_tasofro.h"
#include "act-nut.h"

void json_object_numkeys_foreach(json_t *obj, std::function<void (int key, json_t *value)> function)
{
	std::vector<int> keys;

	for (void *iter = json_object_iter(obj); iter != nullptr; iter = json_object_iter_next(obj, iter)) {
		const char *key = json_object_iter_key(iter);
		keys.push_back(atoi(key));
	}

	std::sort(keys.begin(), keys.end());
	for (int key : keys) {
		std::string key_s = std::to_string(key);
		function(key, json_object_get(obj, key_s.c_str()));
	}
}

static void patch_SQFunctionProto(Nut::SQFunctionProto *func, json_t *add_literals, json_t *replace_instructions, json_t *insert_instructions, json_t *remove_instructions)
{
	// Add literals to the function. They can be used by instructions in insert_instructions and set_instructions.
	if (add_literals) {
		size_t i;
		json_t *value;
		json_flex_array_foreach(add_literals, i, value) {
			func->addLiteral(json_string_value(value));
		}
	}

	// Remove instructions. They aren't removed (only replaced by nop instructions), so you don't need to change any offset.
	if (remove_instructions) {
		Nut::vector *instructions = dynamic_cast<Nut::vector*>((*func)["Instructions"]);

		size_t i;
		json_t *value;
		json_flex_array_foreach(remove_instructions, i, value) {
			size_t line = (size_t)json_integer_value(value);
			if (line >= instructions->size()) {
				log_printf("Act/Nut: insert_instructions: instruction number too big (%u)\n", line);
				continue;
			}

			Nut::SQInstruction *instruction = dynamic_cast<Nut::SQInstruction*>((*instructions)[line]);
			// line instructions are used only when a Squirrel debugger is present. We use them as nop.
			*(*instruction)["0"] = "line";
			*(*instruction)["2"] = "0";
		}
	}

	// Replace instructions. Basically the same as setting "/Instructions/X" from the root object,
	// but this one is applied when it makes the most sense: after literals insertion (so they are
	// available), but before instructions insertion (so the instruction offsets don't change).
	if (replace_instructions) {
		Nut::vector *instructions = dynamic_cast<Nut::vector*>((*func)["Instructions"]);

		const char *key;
		json_t *value;
		json_object_foreach(replace_instructions, key, value) {
			ActNut::Object *instruction = (*instructions)[key];
			if (!instruction) {
				log_printf("Act/Nut: set_instructions: instruction not found (%s)\n", key);
				continue;
			}

			*instruction = json_string_value(value);
		}
	}

	// Insert new instructions.
	// The offsets are always relative to the original file.
	// Note that you will need to fix jump targets if you insert instructions between a jump and its target.
	if (insert_instructions) {
		size_t offset_for_instructions = 0;

		json_object_numkeys_foreach(insert_instructions, [func, &offset_for_instructions](int instruction_number, json_t *instruction_flexarray) {
			size_t i;
			json_t *instruction;
			json_flex_array_foreach(instruction_flexarray, i, instruction) {
				func->insertInstruction(instruction_number + offset_for_instructions, json_string_value(instruction));
				offset_for_instructions++;
			}
		});
	}
}

static void patch_actnut_as_object(ActNut::Object* elem, json_t *json)
{
	json_t *add_literals = json_object_get(json, "add_literals");
	json_t *replace_instructions = json_object_get(json, "replace_instructions");
	json_t *insert_instructions = json_object_get(json, "insert_instructions");
	json_t *remove_instructions = json_object_get(json, "remove_instructions");

	if (add_literals || replace_instructions || insert_instructions || remove_instructions) {
		Nut::SQFunctionProto *func = dynamic_cast<Nut::SQFunctionProto*>(elem);
		if (func) {
			patch_SQFunctionProto(func, add_literals, replace_instructions, insert_instructions, remove_instructions);
		}
		else {
			log_print("Act/Nut: function modificator found for a non-function element\n");
		}
	}
}

static void patch_actnut_as_string(ActNut::Object *elem, json_t *json)
{
	std::string text;
	size_t i;
	json_t *line;

	json_flex_array_foreach(json, i, line) {
		if (text.length() > 0) {
			text += "\n";
		}
		text += json_string_value(line);
	}
	*elem = text;
}

int patch_act_nut(ActNut::Object *actnutobj, void *file_out, size_t size_out, json_t *patch)
{
	if (patch == nullptr) {
		// Nothing to do
		return 0;
	}

	if (actnutobj == nullptr) {
		log_print("Act/Nut: parsing failed\n");
		return -1;
	}

	const char *key;
	json_t *value;
	json_object_foreach(patch, key, value) {
		ActNut::Object *child;
		if (strcmp(key, "/") == 0) {
			child = actnutobj;
		}
		else {
			child = actnutobj->getChild(key);
		}
		if (!child) {
			log_printf("Act/Nut: key %s not found\n", key);
			continue;
		}

		if (json_is_object(value)) {
			patch_actnut_as_object(child, value);
		}
		else if (json_is_string(value) || json_is_array(value)) {
			patch_actnut_as_string(child, value);
		}
		else {
			log_printf("Act/Nut: %s: unsupported json type\n", key);
		}
	}

	ActNut::MemoryBuffer *buf = ActNut::new_MemoryBuffer(ActNut::MemoryBuffer::SHARE, (uint8_t *)file_out, size_out, false);
	if (!actnutobj->writeValue(*buf)) {
		log_print("Act/Nut: writing failed\n");
		ActNut::delete_buffer(buf);
		ActNut::delete_object(actnutobj);
		return -1;
	}
	ActNut::delete_buffer(buf);
	ActNut::delete_object(actnutobj);
	return 1;
}

int patch_act(void *file_inout, size_t size_out, size_t size_in, const char*, json_t *patch)
{
	Act::File *file = Act::read_act_from_bytes((uint8_t *)file_inout, size_in);
	return patch_act_nut(file, file_inout, size_out, patch);
}

int patch_nut(void *file_inout, size_t size_out, size_t size_in, const char*, json_t *patch)
{
	Nut::Stream *stream = Nut::read_nut_from_bytes((uint8_t *)file_inout, size_in);
	return patch_act_nut(stream, file_inout, size_out, patch);
}
<commit_msg>thcrap_tasofro: fix memory leak in ACT/NUT patching<commit_after>/**
  * Touhou Community Reliant Automatic Patcher
  * Tasogare Frontier support plugin
  *
  * ----
  *
  * Patching of ACT and NUT files.
  */

#include <thcrap.h>
#include <algorithm>
#include <libs/135tk/Act-Nut-lib/exports.hpp>
#include "thcrap_tasofro.h"
#include "act-nut.h"

void json_object_numkeys_foreach(json_t *obj, std::function<void (int key, json_t *value)> function)
{
	std::vector<int> keys;

	for (void *iter = json_object_iter(obj); iter != nullptr; iter = json_object_iter_next(obj, iter)) {
		const char *key = json_object_iter_key(iter);
		keys.push_back(atoi(key));
	}

	std::sort(keys.begin(), keys.end());
	for (int key : keys) {
		std::string key_s = std::to_string(key);
		function(key, json_object_get(obj, key_s.c_str()));
	}
}

static void patch_SQFunctionProto(Nut::SQFunctionProto *func, json_t *add_literals, json_t *replace_instructions, json_t *insert_instructions, json_t *remove_instructions)
{
	// Add literals to the function. They can be used by instructions in insert_instructions and set_instructions.
	if (add_literals) {
		size_t i;
		json_t *value;
		json_flex_array_foreach(add_literals, i, value) {
			func->addLiteral(json_string_value(value));
		}
	}

	// Remove instructions. They aren't removed (only replaced by nop instructions), so you don't need to change any offset.
	if (remove_instructions) {
		Nut::vector *instructions = dynamic_cast<Nut::vector*>((*func)["Instructions"]);

		size_t i;
		json_t *value;
		json_flex_array_foreach(remove_instructions, i, value) {
			size_t line = (size_t)json_integer_value(value);
			if (line >= instructions->size()) {
				log_printf("Act/Nut: insert_instructions: instruction number too big (%u)\n", line);
				continue;
			}

			Nut::SQInstruction *instruction = dynamic_cast<Nut::SQInstruction*>((*instructions)[line]);
			// line instructions are used only when a Squirrel debugger is present. We use them as nop.
			*(*instruction)["0"] = "line";
			*(*instruction)["2"] = "0";
		}
	}

	// Replace instructions. Basically the same as setting "/Instructions/X" from the root object,
	// but this one is applied when it makes the most sense: after literals insertion (so they are
	// available), but before instructions insertion (so the instruction offsets don't change).
	if (replace_instructions) {
		Nut::vector *instructions = dynamic_cast<Nut::vector*>((*func)["Instructions"]);

		const char *key;
		json_t *value;
		json_object_foreach(replace_instructions, key, value) {
			ActNut::Object *instruction = (*instructions)[key];
			if (!instruction) {
				log_printf("Act/Nut: set_instructions: instruction not found (%s)\n", key);
				continue;
			}

			*instruction = json_string_value(value);
		}
	}

	// Insert new instructions.
	// The offsets are always relative to the original file.
	// Note that you will need to fix jump targets if you insert instructions between a jump and its target.
	if (insert_instructions) {
		size_t offset_for_instructions = 0;

		json_object_numkeys_foreach(insert_instructions, [func, &offset_for_instructions](int instruction_number, json_t *instruction_flexarray) {
			size_t i;
			json_t *instruction;
			json_flex_array_foreach(instruction_flexarray, i, instruction) {
				func->insertInstruction(instruction_number + offset_for_instructions, json_string_value(instruction));
				offset_for_instructions++;
			}
		});
	}
}

static void patch_actnut_as_object(ActNut::Object* elem, json_t *json)
{
	json_t *add_literals = json_object_get(json, "add_literals");
	json_t *replace_instructions = json_object_get(json, "replace_instructions");
	json_t *insert_instructions = json_object_get(json, "insert_instructions");
	json_t *remove_instructions = json_object_get(json, "remove_instructions");

	if (add_literals || replace_instructions || insert_instructions || remove_instructions) {
		Nut::SQFunctionProto *func = dynamic_cast<Nut::SQFunctionProto*>(elem);
		if (func) {
			patch_SQFunctionProto(func, add_literals, replace_instructions, insert_instructions, remove_instructions);
		}
		else {
			log_print("Act/Nut: function modificator found for a non-function element\n");
		}
	}
}

static void patch_actnut_as_string(ActNut::Object *elem, json_t *json)
{
	std::string text;
	size_t i;
	json_t *line;

	json_flex_array_foreach(json, i, line) {
		if (text.length() > 0) {
			text += "\n";
		}
		text += json_string_value(line);
	}
	*elem = text;
}

int patch_act_nut(ActNut::Object *actnutobj, void *file_out, size_t size_out, json_t *patch)
{
	if (actnutobj == nullptr) {
		log_print("Act/Nut: parsing failed\n");
		return -1;
	}

	const char *key;
	json_t *value;
	json_object_foreach(patch, key, value) {
		ActNut::Object *child;
		if (strcmp(key, "/") == 0) {
			child = actnutobj;
		}
		else {
			child = actnutobj->getChild(key);
		}
		if (!child) {
			log_printf("Act/Nut: key %s not found\n", key);
			continue;
		}

		if (json_is_object(value)) {
			patch_actnut_as_object(child, value);
		}
		else if (json_is_string(value) || json_is_array(value)) {
			patch_actnut_as_string(child, value);
		}
		else {
			log_printf("Act/Nut: %s: unsupported json type\n", key);
		}
	}

	ActNut::MemoryBuffer *buf = ActNut::new_MemoryBuffer(ActNut::MemoryBuffer::SHARE, (uint8_t *)file_out, size_out, false);
	if (!actnutobj->writeValue(*buf)) {
		log_print("Act/Nut: writing failed\n");
		ActNut::delete_buffer(buf);
		ActNut::delete_object(actnutobj);
		return -1;
	}
	ActNut::delete_buffer(buf);
	ActNut::delete_object(actnutobj);
	return 1;
}

int patch_act(void *file_inout, size_t size_out, size_t size_in, const char*, json_t *patch)
{
	if (patch == nullptr) {
		// Nothing to do
		return 0;
	}

	Act::File *file = Act::read_act_from_bytes((uint8_t *)file_inout, size_in);
	return patch_act_nut(file, file_inout, size_out, patch);
}

int patch_nut(void *file_inout, size_t size_out, size_t size_in, const char*, json_t *patch)
{
	if (patch == nullptr) {
		// Nothing to do
		return 0;
	}

	Nut::Stream *stream = Nut::read_nut_from_bytes((uint8_t *)file_inout, size_in);
	return patch_act_nut(stream, file_inout, size_out, patch);
}
<|endoftext|>


<commit_before>#include <vector>
#include <cassert>
#include <numeric>
#include <typeinfo>

#include <agency/experimental/zip.hpp>

template<class Tuple>
void assign_second_to_first(const Tuple& t)
{
  agency::detail::get<0>(t) = agency::detail::get<1>(t);
}

void test()
{
  using namespace agency::experimental;

  std::vector<int> v0(4);
  std::vector<int> v1(4);
  std::vector<int> v2(4);

  std::iota(v0.begin(), v0.end(), 0);
  std::iota(v1.begin(), v1.end(), 4);
  std::iota(v2.begin(), v2.end(), 8);

  auto z = zip(v0,v1,v2);

  {
    // test iterator_tuple()
    auto iter_tuple = z.begin().iterator_tuple();
    auto expected = agency::detail::make_tuple(v0.begin(), v1.begin(), v2.begin());
    assert(iter_tuple == expected);

    using tuple_type = decltype(iter_tuple);
    assert(std::tuple_size<tuple_type>::value == 3);
  }

  {
    // test iterator equality
    assert(z.begin() == z.begin());
  }

  {
    // test iterator inequality
    assert(z.begin() != z.end());

    auto z1 = zip(v0,v1);
    auto z2 = zip(v1,v2);

    assert(z1.begin() != z2.begin());
  }

  {
    // test iterator increment
    auto iter = z.begin();

    assert(*iter == agency::detail::make_tuple(0, 4, 8));

    ++iter;
    assert(*iter == agency::detail::make_tuple(1, 5, 9));

    ++iter;
    assert(*iter == agency::detail::make_tuple(2, 6, 10));

    ++iter;
    assert(*iter == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test iterator add assign
    auto iter = z.begin();
    assert(*iter == agency::detail::make_tuple(0, 4, 8));

    iter += 1;
    assert(*iter == agency::detail::make_tuple(1, 5, 9));

    iter += 1;
    assert(*iter == agency::detail::make_tuple(2, 6, 10));

    iter += 1;
    assert(*iter == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test iterator/sentinel difference
    auto size = z.end() - z.begin();

    assert(size == 4);
  }

  {
    // test iterator dereference
    assert(*z.begin() == agency::detail::make_tuple(0, 4, 8));
  }

  {
    // test assign through iterator dereference
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    std::vector<int> v3(4,0);
    std::vector<int> v4(4,0);

    auto z1 = zip(v1,v2);
    auto z2 = zip(v3,v4);

    auto i1 = z1.begin();
    auto i2 = z2.begin();

    *i2 = *i1;
    ++i2;
    ++i1;

    *i2 = *i1;
    ++i2;
    ++i1;

    *i2 = *i1;
    ++i2;
    ++i1;

    *i2 = *i1;

    assert(v3[0] == 0);
    assert(v3[1] == 1);
    assert(v3[2] == 2);
    assert(v3[3] == 3);

    assert(v4[0] == 4);
    assert(v4[1] == 5);
    assert(v4[2] == 6);
    assert(v4[3] == 7);
  }

  {
    // test iterator index
    auto iter = z.begin();
    assert(iter[0] == agency::detail::make_tuple(0, 4, 8));
    assert(iter[1] == agency::detail::make_tuple(1, 5, 9));
    assert(iter[2] == agency::detail::make_tuple(2, 6, 10));
    assert(iter[3] == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test assign through iterator index
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    std::vector<int> v3(4,0);
    std::vector<int> v4(4,0);

    auto z1 = zip(v1,v2);
    auto z2 = zip(v3,v4);

    for(int i = 0; i < z1.size(); ++i)
    {
      z2[i] = z1[i];
    }

    assert(v3[0] == 0);
    assert(v3[1] == 1);
    assert(v3[2] == 2);
    assert(v3[3] == 3);

    assert(v4[0] == 4);
    assert(v4[1] == 5);
    assert(v4[2] == 6);
    assert(v4[3] == 7);
  }

  {
    // test size
    assert(z.size() == 4);
  }

  {
    // test index
    assert(z[0] == agency::detail::make_tuple(0, 4, 8));
    assert(z[1] == agency::detail::make_tuple(1, 5, 9));
    assert(z[2] == agency::detail::make_tuple(2, 6, 10));
    assert(z[3] == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test assign through index
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    std::vector<int> v3(4,0);
    std::vector<int> v4(4,0);

    auto z1 = zip(v1,v2);
    auto z2 = zip(v3,v4);

    for(int i = 0; i < z.size(); ++i)
    {
      z2[i] = z1[i];
    }

    assert(v3[0] == 0);
    assert(v3[1] == 1);
    assert(v3[2] == 2);
    assert(v3[3] == 3);

    assert(v4[0] == 4);
    assert(v4[1] == 5);
    assert(v4[2] == 6);
    assert(v4[3] == 7);
  }

  {
    // test assign second to first
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    auto z1 = zip(v1,v2);

    for(int i = 0; i < z.size(); ++i)
    {
      assign_second_to_first(z1[i]);
    }

    assert(v1[0] == 4);
    assert(v1[1] == 5);
    assert(v1[2] == 6);
    assert(v1[3] == 7);

    assert(v2[0] == 4);
    assert(v2[1] == 5);
    assert(v2[2] == 6);
    assert(v2[3] == 7);
  }
}

int main()
{
  test();

  std::cout << "OK" << std::endl;

  return 0;
}

<commit_msg>Add a couple of tests for zip_sentinel<commit_after>#include <vector>
#include <cassert>
#include <numeric>
#include <typeinfo>

#include <agency/experimental/zip.hpp>

template<class Tuple>
void assign_second_to_first(const Tuple& t)
{
  agency::detail::get<0>(t) = agency::detail::get<1>(t);
}

void test()
{
  using namespace agency::experimental;

  std::vector<int> v0(4);
  std::vector<int> v1(4);
  std::vector<int> v2(4);

  std::iota(v0.begin(), v0.end(), 0);
  std::iota(v1.begin(), v1.end(), 4);
  std::iota(v2.begin(), v2.end(), 8);

  auto z = zip(v0,v1,v2);

  {
    // test iterator_tuple()
    auto iter_tuple = z.begin().iterator_tuple();
    auto expected = agency::detail::make_tuple(v0.begin(), v1.begin(), v2.begin());
    assert(iter_tuple == expected);

    using tuple_type = decltype(iter_tuple);
    assert(std::tuple_size<tuple_type>::value == 3);
  }

  {
    // test iterator equality
    assert(z.begin() == z.begin());
  }

  {
    // test iterator inequality
    auto z1 = zip(v0,v1);
    auto z2 = zip(v1,v2);

    assert(z1.begin() != z2.begin());
  }

  {
    // test sentinel equality
    assert(z.end() == z.end());
  }

  {
    // test iterator/sentinel inequality
    assert(z.begin() != z.end());
  }

  {
    // test iterator/sentinel difference
    assert(z.end() - z.begin() == 4);
  }

  {
    // test iterator increment
    auto iter = z.begin();

    assert(*iter == agency::detail::make_tuple(0, 4, 8));

    ++iter;
    assert(*iter == agency::detail::make_tuple(1, 5, 9));

    ++iter;
    assert(*iter == agency::detail::make_tuple(2, 6, 10));

    ++iter;
    assert(*iter == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test iterator add assign
    auto iter = z.begin();
    assert(*iter == agency::detail::make_tuple(0, 4, 8));

    iter += 1;
    assert(*iter == agency::detail::make_tuple(1, 5, 9));

    iter += 1;
    assert(*iter == agency::detail::make_tuple(2, 6, 10));

    iter += 1;
    assert(*iter == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test iterator/sentinel difference
    auto size = z.end() - z.begin();

    assert(size == 4);
  }

  {
    // test iterator dereference
    assert(*z.begin() == agency::detail::make_tuple(0, 4, 8));
  }

  {
    // test assign through iterator dereference
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    std::vector<int> v3(4,0);
    std::vector<int> v4(4,0);

    auto z1 = zip(v1,v2);
    auto z2 = zip(v3,v4);

    auto i1 = z1.begin();
    auto i2 = z2.begin();

    *i2 = *i1;
    ++i2;
    ++i1;

    *i2 = *i1;
    ++i2;
    ++i1;

    *i2 = *i1;
    ++i2;
    ++i1;

    *i2 = *i1;

    assert(v3[0] == 0);
    assert(v3[1] == 1);
    assert(v3[2] == 2);
    assert(v3[3] == 3);

    assert(v4[0] == 4);
    assert(v4[1] == 5);
    assert(v4[2] == 6);
    assert(v4[3] == 7);
  }

  {
    // test iterator index
    auto iter = z.begin();
    assert(iter[0] == agency::detail::make_tuple(0, 4, 8));
    assert(iter[1] == agency::detail::make_tuple(1, 5, 9));
    assert(iter[2] == agency::detail::make_tuple(2, 6, 10));
    assert(iter[3] == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test assign through iterator index
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    std::vector<int> v3(4,0);
    std::vector<int> v4(4,0);

    auto z1 = zip(v1,v2);
    auto z2 = zip(v3,v4);

    for(int i = 0; i < z1.size(); ++i)
    {
      z2[i] = z1[i];
    }

    assert(v3[0] == 0);
    assert(v3[1] == 1);
    assert(v3[2] == 2);
    assert(v3[3] == 3);

    assert(v4[0] == 4);
    assert(v4[1] == 5);
    assert(v4[2] == 6);
    assert(v4[3] == 7);
  }

  {
    // test size
    assert(z.size() == 4);
  }

  {
    // test index
    assert(z[0] == agency::detail::make_tuple(0, 4, 8));
    assert(z[1] == agency::detail::make_tuple(1, 5, 9));
    assert(z[2] == agency::detail::make_tuple(2, 6, 10));
    assert(z[3] == agency::detail::make_tuple(3, 7, 11));
  }

  {
    // test assign through index
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    std::vector<int> v3(4,0);
    std::vector<int> v4(4,0);

    auto z1 = zip(v1,v2);
    auto z2 = zip(v3,v4);

    for(int i = 0; i < z.size(); ++i)
    {
      z2[i] = z1[i];
    }

    assert(v3[0] == 0);
    assert(v3[1] == 1);
    assert(v3[2] == 2);
    assert(v3[3] == 3);

    assert(v4[0] == 4);
    assert(v4[1] == 5);
    assert(v4[2] == 6);
    assert(v4[3] == 7);
  }

  {
    // test assign second to first
    std::vector<int> v1(4);
    std::vector<int> v2(4);

    std::iota(v1.begin(), v1.end(), 0);
    std::iota(v2.begin(), v2.end(), 4);

    auto z1 = zip(v1,v2);

    for(int i = 0; i < z.size(); ++i)
    {
      assign_second_to_first(z1[i]);
    }

    assert(v1[0] == 4);
    assert(v1[1] == 5);
    assert(v1[2] == 6);
    assert(v1[3] == 7);

    assert(v2[0] == 4);
    assert(v2[1] == 5);
    assert(v2[2] == 6);
    assert(v2[3] == 7);
  }
}

int main()
{
  test();

  std::cout << "OK" << std::endl;

  return 0;
}

<|endoftext|>


<commit_before><commit_msg>Simplify<commit_after><|endoftext|>


<commit_before>// This is free-ware
// Example code for using the Cytron Technologies HC-SR4 Ultrasonic distance sensor with Spark Core

volatile int StartMeasurement = D1;
volatile int EchoPin = D2;

int measurePulse(int pin);
int attemptDistanceMeasurementOnce(void);
int attemptDistanceUntilSuccess(void);

void setup() {
    pinMode(StartMeasurement, OUTPUT);
    pinMode(EchoPin, INPUT);
    Serial.begin(9600);
}

void loop() {
    int duration = 0;
    float distanceInches = 0.0;

    // get a distance measurement (might have to retry a few times -- hardware has been inconsistent)
    duration = attemptDistanceUntilSuccess();

    // use a USB terminal app to view the distance measurements
    // (I downloaded "serailTools" for Mac for free on appStore)
    Serial.print("Duration: ");
    Serial.println(duration);

    // totally empirical conversion (Note: dependent on processor speed of Spark Core!  Yuck!)
    distanceInches = (duration - 5.0) / 56.0;

    Serial.print("Distance in inches: ");
    Serial.println(distanceInches);
    Serial.println(" ");

    // make a new measurement about 4 times per second
    delay(250);
}

int attemptDistanceUntilSuccess()
{
    int duration;
    int attempts = 1;

    while(attempts < 10)
    {
        duration = attemptDistanceMeasurementOnce();
        if(duration > 0 && duration < 60000)
        {
            break;
        }

        // wait a short time before attempting again -- the primary failure mode is very slow - 187 ms echo pulse
        delay(200);
    }

    return duration;

}

int attemptDistanceMeasurementOnce(void)
{
    int duration;

    // pulse the sensor to make it get a distance reading
    digitalWrite(StartMeasurement, HIGH);
    delay(5);
    digitalWrite(StartMeasurement, LOW);

    // see how long the echo pulse emitted by distance sensor is
    duration = measurePulse(EchoPin);

    return duration;
}

// NOTE: This is ugly -- spark team has not provided micro second timing yet and need sub-millisecond timing...
// at 1130 feet per second, 1 inch is only about 74 microseconds, so obviously millisecond resolution
// won't work for typical distances for which the sensor is used
// so we are using this skanky processor speed dependent counter loop to deterimine the length of the echo pulse
// this should be improved once Spark team provides a sub-millisecond timer
int measurePulse(int pin)
{
    int duration = 0;

        // wait for the initial rising of the echo pin
        // give up if no pulse arrives
    while(digitalRead(EchoPin) == LOW && duration < 100000)
    {
        duration += 1;
    }

    // count cycles until the pin goes low
    // give up if something goes wrong (duration keeps going forever)
    duration = 0;
    while(digitalRead(EchoPin) == HIGH && duration < 100000)
    {
        duration += 1;
    }

    return duration;
}
<commit_msg>change to interrupt approach<commit_after>// This is free-ware
// Example code for using the Cytron Technologies HC-SR4 Ultrasonic distance sensor with Spark Core

volatile int StartMeasurement = D1;
volatile int EchoPin = D2;

volatile unsigned long startPulse = 0;
volatile unsigned long endPulse = 0;

int attemptDistanceMeasurementOnce(void);
int attemptDistanceUntilSuccess(void);
void ultrasonicISR(void);

void setup() {
    pinMode(StartMeasurement, OUTPUT);
    pinMode(EchoPin, INPUT);
    Serial.begin(9600);
}

void loop() {
    int duration = 0;
    float distanceInches = 0.0;

    // get a distance measurement (might have to retry a few times -- hardware has been inconsistent)
    duration = attemptDistanceUntilSuccess();

    // use a USB terminal app to view the distance measurements
    // (I downloaded "serailTools" for Mac for free on appStore)
    Serial.print("Duration in microseconds: ");
    Serial.println(duration);

    // totally empirical conversion (Note: dependent on processor speed of Spark Core!  Yuck!)
    distanceInches = duration / 148.0;

    Serial.print("Distance in inches: ");
    Serial.println(distanceInches);
    Serial.println(" ");

    // make a new measurement about 4 times per second
    delay(250);
}

int attemptDistanceUntilSuccess()
{
    int duration;
    int attempts = 1;

    while(attempts < 10)
    {
        duration = attemptDistanceMeasurementOnce();
        if(duration > 0 && duration < 60000)
        {
            break;
        }

        // wait a short time before attempting again -- the primary failure mode is very slow - 187 ms echo pulse
        delay(200);
    }

    return duration;

}

int attemptDistanceMeasurementOnce(void)
{
    int duration;

    endPulse = startPulse = 0;
    attachInterrupt(EchoPin, ultrasonicISR, CHANGE);

    // pulse the sensor to make it get a distance reading
    digitalWrite(StartMeasurement, HIGH);
    delay(5);
    digitalWrite(StartMeasurement, LOW);

    // wait while we ostensibly get both up and down edges of pulse (and interrupts)
    // the interrupt service routine sets our start and end "times" in microseconds
    delay(20);
    duration = endPulse - startPulse;

    detachInterrupt(EchoPin);

    return duration;
}

void ultrasonicISR(void)
{
    if(digitalRead(EchoPin) == HIGH)
        startPulse = micros();
    else
        endPulse = micros();

}

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2008-2018 the MRtrix3 contributors.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, you can obtain one at http://mozilla.org/MPL/2.0/
 *
 * MRtrix3 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *
 * For more details, see http://www.mrtrix.org/
 */

#include "fixel/matrix.h"

#include "thread_queue.h"
#include "file/ofstream.h"
#include "file/path.h"
#include "file/utils.h"
#include "fixel/helpers.h"

#include "dwi/tractography/mapping/loader.h"
#include "dwi/tractography/mapping/mapper.h"
#include "dwi/tractography/mapping/voxel.h"
#include "dwi/tractography/streamline.h"

namespace MR
{
  namespace Fixel
  {
    namespace Matrix
    {

      void InitFixel::add (const vector<index_type>& indices)
      {
        if ((*this).empty()) {
          (*this).reserve (indices.size());
          for (auto i : indices)
            (*this).emplace_back (InitElement (i));
          track_count = 1;
          return;
        }

        ssize_t self_index = 0, in_index = 0;

        // For anything in indices that doesn't yet appear in *this,
        //   add to this list; once completed, extend *this by the appropriate
        //   amount, and insert these into the appropriate locations
        // Need to continue making use of the existing allocated memory
        // Break into two passes:
        // - On first pass, increment those elements that already exist, and count the number of
        //   fixels that are not yet part of the set (but don't store them)
        // - Extend the length of the vector by as much as is required to fit the new elements
        // - On second pass, from back to front, move elements from previous back of vector to new back,
        //   inserting new elements at appropriate locations to retain sortedness of list
        const ssize_t old_size = (*this).size();
        const ssize_t in_count = indices.size();
        size_t intersection = 0;
        while (self_index < old_size && in_index < in_count) {
          if ((*this)[self_index].index() == indices[in_index]) {
            ++(*this)[self_index];
            ++self_index;
            ++in_index;
            ++intersection;
          } else if ((*this)[self_index].index() > indices[in_index]) {
            ++in_index;
          } else {
            ++self_index;
          }
        }

        self_index = old_size - 1;
        in_index = indices.size() - 1;

        // It's possible that a resize() call may always result in requesting
        //   a re-assignment of memory that exactly matches the size, which may in turn
        //   lead to memory bloat due to inability to return the old memory
        // If this occurs, iteratively calling push_back() may instead engage the
        //   memory-reservation-doubling behaviour
        while ((*this).size() < old_size + indices.size() - intersection)
          (*this).push_back (InitElement());
        ssize_t out_index = (*this).size() - 1;

        // For each output vector location, need to determine whether it should come from copying an existing entry,
        //   or creating a new one
        while (out_index > self_index && self_index >= 0 && in_index >= 0) {
          if ((*this)[self_index].index() == indices[in_index]) {
            (*this)[out_index] = (*this)[self_index];
            --self_index;
            --in_index;
          } else if ((*this)[self_index].index() > indices[in_index]) {
            (*this)[out_index] = (*this)[self_index];
            --self_index;
          } else {
            (*this)[out_index] = InitElement (indices[in_index]);
            --in_index;
          }
          --out_index;
        }
        if (self_index < 0) {
          while (in_index >= 0 && out_index >= 0)
            (*this)[out_index--] = InitElement (indices[in_index--]);
        }

        // Track total number of streamlines intersecting this fixel,
        //   independently of the extent of fixel-fixel connectivity
        ++track_count;
      }

      init_matrix_type generate (
          const std::string& track_filename,
          Image<index_type>& index_image,
          Image<bool>& fixel_mask,
          const float angular_threshold)
      {

        class TrackProcessor { MEMALIGN(TrackProcessor)

          public:
            TrackProcessor (const DWI::Tractography::Mapping::TrackMapperBase& mapper,
                            Image<index_type>& fixel_indexer,
                            Image<default_type>& fixel_directions,
                            Image<bool>& fixel_mask,
                            const default_type angular_threshold) :
                mapper               (mapper),
                fixel_indexer        (fixel_indexer) ,
                fixel_directions     (fixel_directions),
                fixel_mask           (fixel_mask),
                angular_threshold_dp (std::cos (angular_threshold * (Math::pi/180.0))) { }

            bool operator() (const DWI::Tractography::Streamline<>& tck,
                             vector<index_type>& out) const
            {
              using direction_type = Eigen::Vector3;
              using SetVoxelDir = DWI::Tractography::Mapping::SetVoxelDir;

              SetVoxelDir in;
              mapper (tck, in);

              // For each voxel tract tangent, assign to a fixel
              out.clear();
              out.reserve (in.size());
              for (const auto& i : in) {
                assign_pos_of (i).to (fixel_indexer);
                fixel_indexer.index(3) = 0;
                const index_type num_fixels = fixel_indexer.value();
                if (num_fixels > 0) {
                  fixel_indexer.index(3) = 1;
                  const index_type first_index = fixel_indexer.value();
                  const index_type last_index = first_index + num_fixels;
                  // Note: Streamlines can still be assigned to a fixel that is outside the mask;
                  //   however this will not be permitted to contribute to the matrix
                  index_type closest_fixel_index = num_fixels;
                  default_type largest_dp = 0.0;
                  const direction_type dir (i.get_dir().normalized());
                  for (index_type j = first_index; j < last_index; ++j) {
                    fixel_directions.index (0) = j;
                    const default_type dp = abs (dir.dot (direction_type (fixel_directions.row (1))));
                    if (dp > largest_dp) {
                      largest_dp = dp;
                      fixel_mask.index(0) = j;
                      if (fixel_mask.value())
                        closest_fixel_index = j;
                    }
                  }
                  if (closest_fixel_index != num_fixels && largest_dp > angular_threshold_dp)
                    out.push_back (closest_fixel_index);
                }
              }

              // Fixel indices must be sorted prior to providing to InitMatrixFixel::add()
              std::sort (out.begin(), out.end());
              return true;
            }

          private:
            const DWI::Tractography::Mapping::TrackMapperBase& mapper;
            mutable Image<index_type> fixel_indexer;
            mutable Image<default_type> fixel_directions;
            mutable Image<bool> fixel_mask;
            const default_type angular_threshold_dp;
        };

        auto directions_image = Fixel::find_directions_header (Path::dirname (index_image.name())).template get_image<default_type>().with_direct_io ({+2,+1});
        DWI::Tractography::Properties properties;
        DWI::Tractography::Reader<float> track_file (track_filename, properties);
        const uint32_t num_tracks = properties["count"].empty() ? 0 : to<uint32_t>(properties["count"]);
        DWI::Tractography::Mapping::TrackLoader loader (track_file, num_tracks, "computing fixel-fixel connectivity matrix");
        DWI::Tractography::Mapping::TrackMapperBase mapper (index_image);
        mapper.set_upsample_ratio (DWI::Tractography::Mapping::determine_upsample_ratio (index_image, properties, 0.333f));
        mapper.set_use_precise_mapping (true);
        TrackProcessor track_processor (mapper, index_image, directions_image, fixel_mask, angular_threshold);
        init_matrix_type connectivity_matrix (Fixel::get_number_of_fixels (index_image));
        Thread::run_queue (loader,
                           Thread::batch (DWI::Tractography::Streamline<float>()),
                           track_processor,
                           Thread::batch (vector<index_type>()),
                           // Inline lambda function for receiving streamline fixel visitations and
                           //   updating the connectivity matrix
                           [&] (const vector<index_type>& fixels)
                           {
                             try {
                               for (auto f : fixels)
                                 connectivity_matrix[f].add (fixels);
                               return true;
                             } catch (...) {
                               throw Exception ("Error assigning memory for CFE connectivity matrix");
                               return false;
                             }
                           });
        return connectivity_matrix;
      }

      // TODO Could receive a map<string, string>
      void normalise_and_write (init_matrix_type& matrix,
                                const connectivity_value_type threshold,
                                const std::string& path)
      {

        if (Path::exists (path)) {
          if (!Path::is_dir (path)) {
            if (App::overwrite_files) {
              File::unlink (path);
            } else {
              throw Exception ("Cannot create fixel-fixel connectivity matrix \"" + path + "\": Already exists as file");
            }
          }
        } else {
          File::mkdir (path);
        }

        Header index_header;
        index_header.ndim() = 4;
        index_header.size(0) = matrix.size();
        index_header.size(1) = 1;
        index_header.size(2) = 1;
        index_header.size(3) = 2;
        index_header.stride(0) = 2;
        index_header.stride(1) = 3;
        index_header.stride(2) = 4;
        index_header.stride(3) = 1;
        index_header.spacing(0) = index_header.spacing(1) = index_header.spacing(2) = 1.0;
        index_header.transform() = transform_type::Identity();
        index_header.keyval()["nfixels"] = str(matrix.size());
        index_header.datatype() = DataType::from<uint64_t>();
        Image<uint64_t> index_image = Image<uint64_t>::create (Path::join (path, "index.mif"), index_header);

        // Can't use function write_mrtrix_header() as the file offset of the
        //   first entry of the "dim" field needs to be known
        //   (and enough space needs to be left to fill in a large number upon completion)
        File::OFStream fixel_stream (Path::join (path, "fixels.mif"), std::ios_base::out | std::ios_base::in);
        File::OFStream value_stream (Path::join (path, "values.mif"), std::ios_base::out | std::ios_base::in);

        const std::string leadin = "mrtrix image\ndim: ";
        // Need enough space for the largest possible 64-bit unsigned integer,
        //   plus ",1,1" for the two dummy axes
        const size_t dim_padding = std::log10 (std::numeric_limits<size_t>::max()) + 4;

        Eigen::IOFormat fmt(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", "\ntransform: ", "", "", "\ntransform: ", "");

        for (size_t stream_index = 0; stream_index != 2; ++stream_index) {
          File::OFStream& stream (stream_index ? value_stream : fixel_stream);
          stream << leadin;
          for (size_t i = 0; i != dim_padding; ++i)
            stream << " ";
          stream << "\nvox: 1,1,1\nlayout: 0,1,2\ndatatype: ";
          if (stream_index)
            stream << DataType::from<connectivity_value_type>().specifier();
          else
            stream << DataType::from<index_type>().specifier();
          stream << transform_type::Identity().matrix().topLeftCorner(3,4).format(fmt);
          // TODO Insert command_history string
          stream << "\nscaling: 0,1\nnfixels: " + str(matrix.size()) + "\nfile: ";
          int64_t offset = stream.tellp() + int64_t(18);
          offset += ((4 - (offset % 4)) % 4);
          stream << ". " << offset << "\nEND\n";
          while (stream.tellp() < offset)
            stream << "\0";
        }

        size_t data_count = 0;

        for (size_t fixel_index = 0; fixel_index != matrix.size(); ++fixel_index) {
          index_image.index (0) = fixel_index;

          // Here, the connectivity matrix needs to be modified to reflect the
          //   fact that fixel indices in the template fixel image may not
          //   correspond to columns in the statistical analysis
          index_type num_fixels = 0;
          const connectivity_value_type normalisation_factor = 1.0 / connectivity_value_type (matrix[fixel_index].count());
          for (auto& it : matrix[fixel_index]) {
            const connectivity_value_type connectivity = normalisation_factor * it.value();
            if (connectivity >= threshold) {
              const index_type i = it.index();
              fixel_stream.write (reinterpret_cast<const char*>(&i), sizeof (index_type));
              value_stream.write (reinterpret_cast<const char*>(&connectivity), sizeof (connectivity_value_type));
              ++num_fixels;
            }
          }

          index_image.index (3) = 0; index_image.value() = uint64_t(num_fixels);
          index_image.index (3) = 1; index_image.value() = num_fixels ? data_count : uint64_t(0);
          data_count += num_fixels;

          // Force deallocation of memory used for this fixel in the generated matrix
          InitFixel().swap (matrix[fixel_index]);
        }

        // Update headers to reflect the number of fixel-fixel connections
        std::string dim_string = str(data_count) + ",1,1";
        while (dim_string.size() < dim_padding)
          dim_string += " ";
        for (size_t stream_index = 0; stream_index != 2; ++stream_index) {
          File::OFStream& stream (stream_index ? value_stream : fixel_stream);
          stream.seekp (leadin.size());
          stream << dim_string;
        }

      }

    }
  }
}
<commit_msg>fixelconnectivity: Second fixes to new matrix format output<commit_after>/*
 * Copyright (c) 2008-2018 the MRtrix3 contributors.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, you can obtain one at http://mozilla.org/MPL/2.0/
 *
 * MRtrix3 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *
 * For more details, see http://www.mrtrix.org/
 */

#include "fixel/matrix.h"

#include "thread_queue.h"
#include "file/ofstream.h"
#include "file/path.h"
#include "file/utils.h"
#include "fixel/helpers.h"

#include "dwi/tractography/mapping/loader.h"
#include "dwi/tractography/mapping/mapper.h"
#include "dwi/tractography/mapping/voxel.h"
#include "dwi/tractography/streamline.h"

namespace MR
{
  namespace Fixel
  {
    namespace Matrix
    {

      void InitFixel::add (const vector<index_type>& indices)
      {
        if ((*this).empty()) {
          (*this).reserve (indices.size());
          for (auto i : indices)
            (*this).emplace_back (InitElement (i));
          track_count = 1;
          return;
        }

        ssize_t self_index = 0, in_index = 0;

        // For anything in indices that doesn't yet appear in *this,
        //   add to this list; once completed, extend *this by the appropriate
        //   amount, and insert these into the appropriate locations
        // Need to continue making use of the existing allocated memory
        // Break into two passes:
        // - On first pass, increment those elements that already exist, and count the number of
        //   fixels that are not yet part of the set (but don't store them)
        // - Extend the length of the vector by as much as is required to fit the new elements
        // - On second pass, from back to front, move elements from previous back of vector to new back,
        //   inserting new elements at appropriate locations to retain sortedness of list
        const ssize_t old_size = (*this).size();
        const ssize_t in_count = indices.size();
        size_t intersection = 0;
        while (self_index < old_size && in_index < in_count) {
          if ((*this)[self_index].index() == indices[in_index]) {
            ++(*this)[self_index];
            ++self_index;
            ++in_index;
            ++intersection;
          } else if ((*this)[self_index].index() > indices[in_index]) {
            ++in_index;
          } else {
            ++self_index;
          }
        }

        self_index = old_size - 1;
        in_index = indices.size() - 1;

        // It's possible that a resize() call may always result in requesting
        //   a re-assignment of memory that exactly matches the size, which may in turn
        //   lead to memory bloat due to inability to return the old memory
        // If this occurs, iteratively calling push_back() may instead engage the
        //   memory-reservation-doubling behaviour
        while ((*this).size() < old_size + indices.size() - intersection)
          (*this).push_back (InitElement());
        ssize_t out_index = (*this).size() - 1;

        // For each output vector location, need to determine whether it should come from copying an existing entry,
        //   or creating a new one
        while (out_index > self_index && self_index >= 0 && in_index >= 0) {
          if ((*this)[self_index].index() == indices[in_index]) {
            (*this)[out_index] = (*this)[self_index];
            --self_index;
            --in_index;
          } else if ((*this)[self_index].index() > indices[in_index]) {
            (*this)[out_index] = (*this)[self_index];
            --self_index;
          } else {
            (*this)[out_index] = InitElement (indices[in_index]);
            --in_index;
          }
          --out_index;
        }
        if (self_index < 0) {
          while (in_index >= 0 && out_index >= 0)
            (*this)[out_index--] = InitElement (indices[in_index--]);
        }

        // Track total number of streamlines intersecting this fixel,
        //   independently of the extent of fixel-fixel connectivity
        ++track_count;
      }

      init_matrix_type generate (
          const std::string& track_filename,
          Image<index_type>& index_image,
          Image<bool>& fixel_mask,
          const float angular_threshold)
      {

        class TrackProcessor { MEMALIGN(TrackProcessor)

          public:
            TrackProcessor (const DWI::Tractography::Mapping::TrackMapperBase& mapper,
                            Image<index_type>& fixel_indexer,
                            Image<default_type>& fixel_directions,
                            Image<bool>& fixel_mask,
                            const default_type angular_threshold) :
                mapper               (mapper),
                fixel_indexer        (fixel_indexer) ,
                fixel_directions     (fixel_directions),
                fixel_mask           (fixel_mask),
                angular_threshold_dp (std::cos (angular_threshold * (Math::pi/180.0))) { }

            bool operator() (const DWI::Tractography::Streamline<>& tck,
                             vector<index_type>& out) const
            {
              using direction_type = Eigen::Vector3;
              using SetVoxelDir = DWI::Tractography::Mapping::SetVoxelDir;

              SetVoxelDir in;
              mapper (tck, in);

              // For each voxel tract tangent, assign to a fixel
              out.clear();
              out.reserve (in.size());
              for (const auto& i : in) {
                assign_pos_of (i).to (fixel_indexer);
                fixel_indexer.index(3) = 0;
                const index_type num_fixels = fixel_indexer.value();
                if (num_fixels > 0) {
                  fixel_indexer.index(3) = 1;
                  const index_type first_index = fixel_indexer.value();
                  const index_type last_index = first_index + num_fixels;
                  // Note: Streamlines can still be assigned to a fixel that is outside the mask;
                  //   however this will not be permitted to contribute to the matrix
                  index_type closest_fixel_index = num_fixels;
                  default_type largest_dp = 0.0;
                  const direction_type dir (i.get_dir().normalized());
                  for (index_type j = first_index; j < last_index; ++j) {
                    fixel_directions.index (0) = j;
                    const default_type dp = abs (dir.dot (direction_type (fixel_directions.row (1))));
                    if (dp > largest_dp) {
                      largest_dp = dp;
                      fixel_mask.index(0) = j;
                      if (fixel_mask.value())
                        closest_fixel_index = j;
                    }
                  }
                  if (closest_fixel_index != num_fixels && largest_dp > angular_threshold_dp)
                    out.push_back (closest_fixel_index);
                }
              }

              // Fixel indices must be sorted prior to providing to InitMatrixFixel::add()
              std::sort (out.begin(), out.end());
              return true;
            }

          private:
            const DWI::Tractography::Mapping::TrackMapperBase& mapper;
            mutable Image<index_type> fixel_indexer;
            mutable Image<default_type> fixel_directions;
            mutable Image<bool> fixel_mask;
            const default_type angular_threshold_dp;
        };

        auto directions_image = Fixel::find_directions_header (Path::dirname (index_image.name())).template get_image<default_type>().with_direct_io ({+2,+1});
        DWI::Tractography::Properties properties;
        DWI::Tractography::Reader<float> track_file (track_filename, properties);
        const uint32_t num_tracks = properties["count"].empty() ? 0 : to<uint32_t>(properties["count"]);
        DWI::Tractography::Mapping::TrackLoader loader (track_file, num_tracks, "computing fixel-fixel connectivity matrix");
        DWI::Tractography::Mapping::TrackMapperBase mapper (index_image);
        mapper.set_upsample_ratio (DWI::Tractography::Mapping::determine_upsample_ratio (index_image, properties, 0.333f));
        mapper.set_use_precise_mapping (true);
        TrackProcessor track_processor (mapper, index_image, directions_image, fixel_mask, angular_threshold);
        init_matrix_type connectivity_matrix (Fixel::get_number_of_fixels (index_image));
        Thread::run_queue (loader,
                           Thread::batch (DWI::Tractography::Streamline<float>()),
                           track_processor,
                           Thread::batch (vector<index_type>()),
                           // Inline lambda function for receiving streamline fixel visitations and
                           //   updating the connectivity matrix
                           [&] (const vector<index_type>& fixels)
                           {
                             try {
                               for (auto f : fixels)
                                 connectivity_matrix[f].add (fixels);
                               return true;
                             } catch (...) {
                               throw Exception ("Error assigning memory for CFE connectivity matrix");
                               return false;
                             }
                           });
        return connectivity_matrix;
      }

      // TODO Could receive a map<string, string>
      void normalise_and_write (init_matrix_type& matrix,
                                const connectivity_value_type threshold,
                                const std::string& path)
      {

        if (Path::exists (path)) {
          if (!Path::is_dir (path)) {
            if (App::overwrite_files) {
              File::unlink (path);
            } else {
              throw Exception ("Cannot create fixel-fixel connectivity matrix \"" + path + "\": Already exists as file");
            }
          }
        } else {
          File::mkdir (path);
        }

        Header index_header;
        index_header.ndim() = 4;
        index_header.size(0) = matrix.size();
        index_header.size(1) = 1;
        index_header.size(2) = 1;
        index_header.size(3) = 2;
        index_header.stride(0) = 2;
        index_header.stride(1) = 3;
        index_header.stride(2) = 4;
        index_header.stride(3) = 1;
        index_header.spacing(0) = index_header.spacing(1) = index_header.spacing(2) = 1.0;
        index_header.transform() = transform_type::Identity();
        index_header.keyval()["nfixels"] = str(matrix.size());
        index_header.datatype() = DataType::from<uint64_t>();
        Image<uint64_t> index_image = Image<uint64_t>::create (Path::join (path, "index.mif"), index_header);

        // Can't use function write_mrtrix_header() as the file offset of the
        //   first entry of the "dim" field needs to be known
        //   (and enough space needs to be left to fill in a large number upon completion)
        File::OFStream fixel_stream (Path::join (path, "fixels.mif"), std::ios_base::out | std::ios_base::binary);
        File::OFStream value_stream (Path::join (path, "values.mif"), std::ios_base::out | std::ios_base::binary);

        const std::string leadin = "mrtrix image\ndim: ";
        // Need enough space for the largest possible 64-bit unsigned integer,
        //   plus ",1,1" for the two dummy axes
        const size_t dim_padding = std::log10 (std::numeric_limits<size_t>::max()) + 4;

        Eigen::IOFormat fmt(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", "\ntransform: ", "", "", "\ntransform: ", "");

        for (size_t stream_index = 0; stream_index != 2; ++stream_index) {
          File::OFStream& stream (stream_index ? value_stream : fixel_stream);
          stream << leadin;
          for (size_t i = 0; i != dim_padding; ++i)
            stream << " ";
          stream << "\nvox: 1,1,1\nlayout: 0,1,2\ndatatype: ";
          if (stream_index)
            stream << DataType::from<connectivity_value_type>().specifier();
          else
            stream << DataType::from<index_type>().specifier();
          stream << transform_type::Identity().matrix().topLeftCorner(3,4).format(fmt);
          // TODO Insert command_history string
          stream << "\nscaling: 0,1\nnfixels: " + str(matrix.size()) + "\nfile: ";
          int64_t offset = stream.tellp() + int64_t(18);
          offset += ((4 - (offset % 4)) % 4);
          stream << ". " << offset << "\nEND\n";
          const char null = 0;
          while (stream.tellp() < offset)
            stream.write (&null, sizeof (char));
        }

        size_t data_count = 0;

        for (size_t fixel_index = 0; fixel_index != matrix.size(); ++fixel_index) {
          index_image.index (0) = fixel_index;

          // Here, the connectivity matrix needs to be modified to reflect the
          //   fact that fixel indices in the template fixel image may not
          //   correspond to columns in the statistical analysis
          index_type num_fixels = 0;
          const connectivity_value_type normalisation_factor = 1.0 / connectivity_value_type (matrix[fixel_index].count());
          for (auto& it : matrix[fixel_index]) {
            const connectivity_value_type connectivity = normalisation_factor * it.value();
            if (connectivity >= threshold) {
              const index_type i = it.index();
              fixel_stream.write (reinterpret_cast<const char*>(&i), sizeof (index_type));
              value_stream.write (reinterpret_cast<const char*>(&connectivity), sizeof (connectivity_value_type));
              ++num_fixels;
            }
          }

          index_image.index (3) = 0; index_image.value() = uint64_t(num_fixels);
          index_image.index (3) = 1; index_image.value() = num_fixels ? data_count : uint64_t(0);
          data_count += num_fixels;

          // Force deallocation of memory used for this fixel in the generated matrix
          InitFixel().swap (matrix[fixel_index]);
        }

        // Update headers to reflect the number of fixel-fixel connections
        std::string dim_string = str(data_count) + ",1,1";
        while (dim_string.size() < dim_padding)
          dim_string += " ";
        for (size_t stream_index = 0; stream_index != 2; ++stream_index) {
          File::OFStream& stream (stream_index ? value_stream : fixel_stream);
          stream.seekp (leadin.size());
          stream << dim_string;
        }

      }

    }
  }
}
<|endoftext|>