text:
////////////////////////////////////////////////////////////////////////////
//	Created 	: 21.11.2008
//	Author		: Dmitriy Iassenev
//	Copyright (C) GSC Game World - 2009
////////////////////////////////////////////////////////////////////////////

#include "pch.h"
#include "engine_world.h"
#include "rpc.h"
#include <xray/input/world.h>
#include <xray/editor/world/world.h>
#include <xray/render/base/engine_renderer.h>
#include <xray/core/core.h>
#include <boost/bind.hpp>

#include <xray/os_preinclude.h>
#undef NOUSER
#undef NOMSG
#undef NOWINSTYLES
#undef NOWINMESSAGES
#undef NOSYSMETRICS
#undef NOSHOWWINDOW
#undef NOWINOFFSETS
#define _WIN32_WINNT				0x0500
#include <xray/os_include.h>

#include <objbase.h>				// for COINIT_MULTITHREADED

#pragma comment( lib, "delayimp.lib" )

using xray::engine::engine_world;

static pcstr s_window_id					= XRAY_ENGINE_ID " DX9 Renderer Window";
static pcstr s_window_class_id				= XRAY_ENGINE_ID " DX9 Renderer Window Class ID";

static WNDCLASSEX s_window_class;

static xray::engine::engine_world*			s_world	= 0;

static LRESULT APIENTRY message_processor	( HWND window_handle, UINT message_id, WPARAM w_param, LPARAM l_param )
{
	switch ( message_id ) {
		case WM_DESTROY: {
			PostQuitMessage	( 0 );

			return			( 0 );
		}
		case WM_ACTIVATE: {

			u16 fActive						= LOWORD(w_param);
			BOOL fMinimized					= (BOOL) HIWORD(w_param);
			bool bActive					= ((fActive!=WA_INACTIVE) && (!fMinimized)) ? true : false;

			if (bActive != s_world->app_is_active())
			{
				if (bActive)
				{
					s_world->on_app_activate( );
					while (	ShowCursor( FALSE ) >= 0 );
				}else
				{
					s_world->on_app_deactivate( );
					while (	ShowCursor( TRUE ) < 0 );
				}
			}
			//if ( (w_param == WA_ACTIVE) || (w_param == WA_CLICKACTIVE) ) {
			//	if ( !s_world->editor_world() )
			//		while (	ShowCursor( FALSE ) >= 0 );

			//	s_world->on_app_activate( );
			//
			//	break;
			//}

			//ASSERT			( w_param == WA_INACTIVE );
			//if ( !s_world->editor_world() )
			//	while (	ShowCursor( TRUE ) < 0 );

			break;
		}
	}

	return					( DefWindowProc( window_handle, message_id, w_param, l_param ) );
}

static void create_window			( HWND& result )
{
	WNDCLASSEX const temp	=
	{
		sizeof( WNDCLASSEX ),
		CS_CLASSDC,
		&message_processor,
		0L,
		0L,
		GetModuleHandle( 0 ),
		NULL,
		NULL,
		NULL,
		NULL,
		s_window_class_id,
		NULL
	};

	s_window_class			= temp;
	RegisterClassEx			( &s_window_class );

	u32 const screen_size_x	= GetSystemMetrics( SM_CXSCREEN );
	u32 const screen_size_y	= GetSystemMetrics( SM_CYSCREEN );

	DWORD const	window_style = WS_OVERLAPPED;// | WS_CAPTION;

	u32 window_size_x		= 0;
	u32 window_size_y		= 0;

	u32 const window_sizes_x []	= { 1024, 800, 640 };
	u32 const window_sizes_y []	= { 768, 600, 480 };
	for ( u32 i=0; i<xray::array_size(window_sizes_x); ++i ) {
		if ( window_sizes_x[i] < screen_size_x &&
			 window_sizes_y[i] < screen_size_y )
		{
			window_size_x	= window_sizes_x[i];
			window_size_y	= window_sizes_y[i];
			break;
		}
	}

	R_ASSERT				(window_size_x);

	RECT		window_size = { 0, 0, window_size_x, window_size_y };
	AdjustWindowRect		(&window_size, window_style, false);

	result					=
		CreateWindow (
			s_window_class_id,
			s_window_id,
			window_style,
			( screen_size_x - window_size.right ) / 2,
			( screen_size_y - window_size.bottom ) / 2,
			window_size.right - window_size.left,
			window_size.bottom - window_size.top,
			GetDesktopWindow( ),
			0,
			s_window_class.hInstance,
			0
		);

	R_ASSERT				( result );
}

void engine_world::initialize_core	( )
{
	//if ( true )//command_line_editor( ) )
	//	CoInitializeEx		( 0, COINIT_APARTMENTTHREADED );
	//else
	//	CoInitializeEx		( 0, COINIT_MULTITHREADED );

	pcstr debug_thread_id	= 0;
	if ( threading::g_debug_single_thread )
		debug_thread_id		= "main";
	else if ( threading::core_count() == 1 ) {
		if ( command_line_editor() )
			debug_thread_id	= "editor + logic + render";
		else
			debug_thread_id	= "logic + render";
	}
	else
		debug_thread_id		= "render";

	if ( !command_line_editor() )
		core::initialize	( debug_thread_id, core::create_log, core::perform_debug_initialization );
	else
		core::initialize	( debug_thread_id, core::create_log, core::delay_debug_initialization );
}

void engine_world::create_render	( )
{
	s_world					= this;

	rpc::wait				( rpc::editor );

	if ( !m_editor ) {
		create_window		( m_window_handle );
		m_main_window_handle= m_window_handle;
	}

	m_render_world		= render::create_world( *this, m_window_handle );

// 	if ( !s_dx10 )
// 		m_render_world		= render_dx9::create_world( *this, m_window_handle );
// 	else
// 		m_render_world		= render::dx10::create_world( *this, m_window_handle );

	ASSERT					( m_render_world );
}

void engine_world::destroy_render	( )
{
	render::destroy_world( m_render_world );

	ShowWindow				( m_window_handle, SW_HIDE );

	ASSERT					( s_world );
	s_world					= 0;
}

void engine_world::show_window		( )
{
	if ( m_editor ) {
		SetForegroundWindow	( m_editor->main_handle( ) );
		return;
	}

	ShowWindow				( m_window_handle, SW_SHOW );
	SetForegroundWindow		( m_window_handle );
//	on_activate				( );
}

bool engine_world::process_messages( )
{
	ASSERT					( !m_editor );

	MSG						message;
	while ( PeekMessage( &message, 0, 0, 0, PM_REMOVE ) ) {
		TranslateMessage	( &message );
		DispatchMessage		( &message );

		if ( message.message == WM_QUIT )
			return			( false );
	}

	return					( true );
}

void engine_world::run				( )
{
	if ( m_early_destruction_started )
		return;

	m_timer.start			( );

	if ( m_editor && (threading::core_count( ) == 1) ) {
		m_editor->run		( );

		if ( !m_destruction_started )
			exit			( 0 );

		return;
	}

	for (;;) {
		if ( !m_editor && !process_messages() ) {
			if ( !m_destruction_started )
				exit		( 0 );

			return;
		}

		if ( m_destruction_started )
			return;

		tick				( );
	}
}

pcstr engine_world::get_resource_path		( ) const
{
	return								"../../resources";
}

pcstr engine_world::get_mounts_path		( ) const
{
	return								"../../mounts";
}

pcstr engine_world::get_underscore_G_path	( ) const
{
	return								"../../resources/scripts";
}

void engine_world::enable_game						( bool value )
{
	bool bdraw_editor	= !value;
	bool bdraw_game		= value;
	render_world().set_editor_render_mode(bdraw_editor, bdraw_game);
	enable_game_impl	( value );
}

void engine_world::post_initialize	( )
{
	if ( command_line_editor() )
		rpc::run			( rpc::editor, boost::bind( &engine_world::initialize_editor_thread_ids, this), rpc::continue_process_loop, rpc::wait_for_completion );

	rpc::run				( rpc::editor, boost::bind( &engine_world::show_window, this), rpc::break_process_loop, rpc::wait_for_completion );
}


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

#include <cstdio>
#include <csignal>
#include <iostream>
#include <sstream>
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>
#include <sys/socket.h>
#include <netinet/in.h>
#include "Hart.hpp"

#ifdef __MINGW64__
#define SIGTRAP 5
#endif

static
int
putDebugChar(char c, int fd)
{
  return write(fd, &c, sizeof(uint8_t));
}

static
uint8_t
getDebugChar(int fd)
{
  uint8_t res;
  if (read(fd, &res, sizeof(res)) == sizeof(res))
    return res;
  return uint8_t(-1); // TODO throw exception?
}

static
int
hexCharToInt(unsigned char c)
{
  if (c >= 'a' and c <= 'f')
    return 10 + c - 'a';
  if (c >= 'A' and c <= 'F')
    return 10 + c - 'A';
  if (c >= '0' and c <= '9')
    return c - '0';
  return -1;
}

static
bool
hexCharToInt(char c, unsigned& value)
{
  if (c >= 'a' and c <= 'f')
    {
      value = 10 + c - 'a';
      return true;
    }

  if (c >= 'A' and c <= 'F')
    {
      value = 10 + c - 'A';
      return true;
    }

  if (c >= '0' and c <= '9')
    {
      value = c - '0';
      return true;
    }

  return false;
}

/// Convert an 8-bit value to 2 headecimal characters.
static
inline
void
byteToHexChars(uint8_t& byte, uint8_t& highDigit, uint8_t& lowDigit)
{
  static char hexDigits[] = "0123456789abcdef";
  highDigit = hexDigits[(byte >> 4)];
  lowDigit = hexDigits[byte & 0xf];
}

static
bool
getStringComponents(const std::string& str, char delim,
		    std::string& comp1, std::string& comp2)
{
  auto sepIx = str.find(delim);
  if (sepIx == std::string::npos)
    return false;

  comp1 = str.substr(0, sepIx);
  comp2 = str.substr(sepIx + 1);
  return true;
}

static
bool
getStringComponents(const std::string& str, char delim1, char delim2,
		    std::string& comp1, std::string& comp2, std::string& comp3)
{
  auto delim1Ix = str.find(delim1);
  if (delim1Ix == std::string::npos)
    return false;

  comp1 = str.substr(0, delim1Ix);

  auto delim2Ix = str.find(delim2, delim1Ix + 1);
  if (delim1Ix == std::string::npos)
    return false;

  comp2 = str.substr(delim1Ix + 1, delim2Ix - delim1Ix - 1);
  comp3 = str.substr(delim2Ix + 1);
  return true;
}

// Receive a packet from gdb. Request a retransmit from gdb if packet
// checksum is incorrect. Return succesfully received packet.
static
void
receivePacketFromGdb(int fd, std::string& packet)
{
  unsigned char ch = ' '; // Anything besides $ will do.

  char buffer[1024];
  uint8_t sum = 0;  // checksum

  ssize_t count = read(fd, buffer, sizeof(buffer));
  if (count < 0)
    return;

  // Packet starts with a '$'
  ssize_t ix = 0;
  while (ix < count and buffer[ix] != '$')
    ++ix;

  ++ix; // Skip '$'

  while (ix < count and buffer[ix] != '#')
    {
      char cc = buffer[ix++];
      sum += cc;
      packet.push_back(cc);
    }

  ++ix;  // Skip '#'

  if (ix + 1 < count)
    {
      ch = buffer[ix++];
      uint8_t pacSum = static_cast<uint8_t>(hexCharToInt(ch) << 4); // Packet checksum
      ch = buffer[ix++];
      pacSum = static_cast<uint8_t>(pacSum + hexCharToInt(ch));

      if (sum != pacSum)
        {
          std::cerr << "Bad checksum form gdb: "
                    << (boost::format("%02x v %02x") % unsigned(sum) %
                        unsigned(pacSum))
                    << '\n';
          putDebugChar('-', fd); // Signal failed reception.
        }
      else
        putDebugChar('+', fd);  // Signal successul reception.
    }
}

// Send given data string as a gdb remote packet. Resend until a
// positive ack is received.
//
// Format of packet:  $<data>#<checksum>
//
// TODO: quote special characters.
static void
sendPacketToGdb(const std::string& data, int fd)
{
  const char hexDigit[] = "0123456789abcdef";

  unsigned char checksum = 0;
  for (unsigned char c : data)
    checksum = static_cast<uint8_t>(checksum + c);

  std::string packet;
  packet.reserve(8 + data.size());

  packet.push_back('$');
  packet += data;
  packet.push_back('#');
  packet.push_back(hexDigit[checksum >> 4]);
  packet.push_back(hexDigit[checksum & 0xf]);

  while (true)
    {
      write(fd, packet.data(), packet.size());
      char c = getDebugChar(fd);
      if (c == '+')
	return;
    }
}

/// Return hexadecimal representation of given integer register value.
template <typename T>
std::string
littleEndianIntToHex(T val)
{
  std::ostringstream oss;

  for (size_t i = 0; i < sizeof(T); ++i)
    {
      unsigned byte = val & 0xff;
      val = val >> 8;
      oss << (boost::format("%02x") % byte);
    }

  return oss.str();
}

/// Convert given little-endian hexadecimal string to an integer value
/// of type T. Return true on success and false if the string does not
/// contain a hexadecimal string or if it is too large for type T.
template <typename T>
bool
littleEndianHexToInt(const std::string& str, T& value)
{
  size_t byteCount = 0;

  value = 0;

  for (size_t i = 0; i < str.size(); i += 2)
    {
      char c = str.at(i);
      unsigned byte =  0;
      if (not hexCharToInt(c, byte))
	return false;

      if (i + 1 < str.size())
	{
	  byte <<= 4;

	  char c2 = str.at(i+1);
	  unsigned x = 0;
	  if (not hexCharToInt(c2, x))
	    return false;
	  byte |= x;
	}
      value |= byte << (byteCount*8);
      byteCount++;
    }

  return byteCount <= sizeof(T);
}

/// Convert given hexadecimal string to an integer value of type
/// T. Return true on success and false if the string does not contain
/// a hexadecimal string or if it is too large for type T.
template <typename T>
bool
hexToInt(const std::string& str, T& value)
{
  value = 0;

  if (str.empty())
    return false;

  const char* data = str.c_str();
  char* end = nullptr;
  uint64_t v = strtoull(data, &end, 16);
  if (*end)
    return false;

  value = static_cast<T>(v);
  if (v != value)
    return false; // Overflow

  return true;
}

// GBD uses indices 0-31 for integer registers, 32 for pc, 33-64 for
// floating-point registers, 65 and higher for CSRs.
static const unsigned fpRegOffset = 33, pcOffset = 32, csrOffset = 65;

template <typename URV>
void
handlePeekRegisterForGdb(WdRiscv::Hart<URV>& hart, unsigned regNum,
			 std::ostream& stream)
{
  URV value = 0; bool ok = true, fp = false;
  if (regNum < pcOffset)
    ok = hart.peekIntReg(regNum, value);
  else if (regNum == pcOffset)
    value = hart.peekPc();
  else if (regNum >= fpRegOffset and regNum < csrOffset)
    {
      fp = true;
      if (hart.isRvf() or hart.isRvd())
	{
	  unsigned fpReg = regNum - fpRegOffset;
	  uint64_t val64 = 0;
	  hart.peekUnboxedFpReg(fpReg, val64);
          stream << littleEndianIntToHex(val64);
	}
      else
	stream << littleEndianIntToHex(0L); // Old versions of gdb cannot handle error "E03";
    }
  else
    {
      URV csr = regNum - csrOffset;
      // Undocumented: gdb uses 0x1000 to obtain privilege mode.
      if (csr == 0x1000)
        value = URV(hart.privilegeMode());
      else
        hart.peekCsr(WdRiscv::CsrNumber(csr), value);
    }

  if (ok)
    {
      if (not fp)
	stream << littleEndianIntToHex(value);
    }
  else
    stream << "E04";
}

template <typename URV>
void
getGdbTargetXml(WdRiscv::Hart<URV>& hart, std::string& xml)
{
  xml.clear();

  xml += R"zzz(<?xml version="1.0"?>
<!DOCTYPE target SYSTEM "gdb-target.dtd">
<target version="1.0">
  <feature name="org.gnu.gdb.riscv.cpu">
)zzz";

  // Integer registers.
  std::string width = std::to_string(8*sizeof(URV));

  for (unsigned ix = 0; ix < hart.intRegCount(); ++ix)
    {
      std::string name = hart.intRegName(ix);
      std::string num = std::to_string(ix);
      xml += "    <reg name=\"" + name + "\" bitsize=\"" + width + "\" regnum=\"";
      xml += num + "\" save-restore=\"yes\" type=\"int\" group=\"general\"/>\n";
    }

  std::string pcNum = std::to_string(pcOffset);
  xml += "    <reg name=\"pc\" bitsize=\"" + width + "\" regnum=\"";
  xml += pcNum + "\" save-restore=\"yes\" type=\"int\" group=\"general\"/>\n";

  xml += "  </feature>\n";

  // FP registers.
  if (hart.isRvf())
    {
      xml += "  <feature name=\"org.gnu.gdb.riscv.fpu\">\n";

      for (unsigned ix = 0; ix < hart.fpRegCount(); ++ix)
        {
          std::string name = hart.fpRegName(ix);
          std::string num = std::to_string(ix + fpRegOffset);
          xml += "    <reg name=\"" + name + "\" bitsize=\"64\" regnum=\"";
          xml += num + "\" save-restore=\"yes\" type=\"float\" group=\"fp\"/>\n";
        }

      xml += "  </feature>\n";
    }

  // CSRs
  std::vector<WdRiscv::CsrNumber> csrNumbers;
  hart.getImplementedCsrs(csrNumbers);

  xml += "  <feature name=\"org.gnu.gdb.riscv.csr\">\n";
  for (auto csrn : csrNumbers)
    {
      URV val = 0;
      std::string name;
      if (not hart.peekCsr(csrn, val, name))
        continue;

      std::string num = std::to_string(unsigned(csrn) + csrOffset);

      xml += "    <reg name=\"" + name + "\" bitsize=\"" + width + "\" regnum=\"";
      xml += num + "\" save-restore=\"no\" type=\"int\" group=\"csr\"/>\n";
    }
  xml += "  </feature>\n";

  // Virtual CSRs (privilege mode pseudo-csr)
  // CSR 0x1000 is used by gdb to obtain privilege mode.
  std::string num = std::to_string(0x1000 + csrOffset);
  xml += "  <feature name=\"org.gnu.gdb.riscv.virtual\">\n";
  xml += "    <reg name=\"priv\" bitsize=\"" + width + "\" regnum=\"";
  xml += num + "\" save-restore=\"no\" type=\"int\" group=\"general\"/>\n";
  xml += "  </feature>\n";

  xml += "</target>";
}

// XML describing this RISCV processor to gdb.
static std::string targetXml;
static std::mutex xmlMutex;

template <typename URV>
void
processXferQuery(const std::string& packet, WdRiscv::Hart<URV>& hart,
                 std::ostream& reply)
{
  if (not boost::starts_with(packet, "qXfer:features:read:target.xml:"))
    {
      reply << "";
      return;
    }

  // Request is of the form: qXfer:features:read:target.xml:<offset>,<length>
  // Recover offset and length substring.
  auto ix = packet.find_last_of(':');
  auto ol = packet.substr(ix + 1);  // offset and length
  unsigned offset = 0, length = 0;
  if (sscanf(ol.c_str(), "%x,%x", &offset, &length) != 2)
    {
      std::cerr << "Malformed qXfer request: " << packet << '\n';
      reply << "";
      return;
    }

  {
    std::lock_guard<std::mutex> lock(xmlMutex);

    // Fill xml string on first call to this function.
    if (targetXml.empty())
      getGdbTargetXml(hart, targetXml);
  }

  auto part = targetXml.substr(offset, length);
  if (offset + length < targetXml.size())
    reply << 'm' << part;
  else
    reply << 'l' << part;
}

// Called after a stop (caused by an exception). Notify GDB of the
// stop.  Return the signal number corresponding to the exception.
template <typename URV>
unsigned
notifyGdbAfterStop(WdRiscv::Hart<URV>& hart, int fd)
{
  // Construct a reply of the form T xx n1:r1;n2:r2;... where xx is
  // the trap cause and ni is a resource (e.g. register number) and ri
  // is the resource data (e.g. content of register).
  std::ostringstream reply;

  unsigned signalNum = SIGTRAP;
  URV cause = 0;
  if (hart.peekCsr(WdRiscv::CsrNumber::MCAUSE, cause))
    {
      if (cause == URV(WdRiscv::ExceptionCause::BREAKP))
	signalNum = SIGTRAP;
      // FIX:  implement other causes.
    }

  reply << "T" << (boost::format("%02x") % signalNum);

  URV spVal = 0;
  unsigned spNum = WdRiscv::RegSp;
  hart.peekIntReg(spNum, spVal);
  reply << (boost::format("%02x") % spNum) << ':'
	<< littleEndianIntToHex(spVal) << ';';
  sendPacketToGdb(reply.str(), fd);

  return signalNum;
}

template <typename URV>
void
handleExceptionForGdb(WdRiscv::Hart<URV>& hart, int fd)
{
  // The trap handler is expected to set the PC to point to the instruction
  // after the one with the exception if necessary/possible.
  unsigned signalNum = notifyGdbAfterStop(hart, fd);

  bool gotQuit = false;

  std::string buffer;
  buffer.reserve(128);
  std::ostringstream reply(buffer);

  std::string packet;
  packet.reserve(128);

  while (1)
    {
      reply.str("");
      reply.clear();

      packet.clear();
      receivePacketFromGdb(fd, packet);
      if (packet.empty())
	continue;

      switch (packet.at(0))
	{
	case '?':
	  // Return signal number
	  reply << "S" << (boost::format("%02x") % signalNum);
	  break;

	case 'g':  // return the value of the CPU registers
	  {
	    for (unsigned i = 0; i < hart.intRegCount(); ++i)
	      {
		URV val = 0;
		hart.peekIntReg(i, val);
		reply << littleEndianIntToHex(val);
	      }
            reply << littleEndianIntToHex(hart.peekPc());
	  }
	  break;

	case 'G':  // set the value of the CPU registers - return OK
	  {
	    std::string status = "OK";
	    size_t len = packet.length();
	    if (len + 1 < hart.intRegCount() * sizeof(URV) * 2)
	      status = "E01";
	    else
	      {
		const char* ptr = packet.c_str() + 1;
		for (unsigned i = 0; i < hart.intRegCount(); ++i)
		  {
		    std::string buffer;
		    for (unsigned i = 0; i < 2*sizeof(URV); ++i)
		      buffer += *ptr++;
		    URV val = 0;
		    if (littleEndianHexToInt(buffer, val))
		      hart.pokeIntReg(i, val);
		    else
		      {
			status  = "E01";
			break;
		      }
		  }
	      }

	    reply << status;
	  }
	  break;

	case 'H':   // Hc<thread> or Hg<thread>
	  {
	    if (packet.length() < 2)
	      reply << "E01";
	    else
	      {
		unsigned threadId = 0;
		if (packet[1] != 'c' or packet[1] != 'g')
		  reply << "E01";
		else if (not hexToInt(packet.substr(2), threadId))
		  reply << "E01";
		else if (threadId != 0)
		  reply << "E01";  // Multi-thread not supported yet.
		else
		  reply << "OK";
	      }
	  }
	  break;

	case 'm': // mAA..AA,LLLL  Read LLLL bytes at address AA..AA
	  {
	    std::string addrStr, lenStr;
	    if (not getStringComponents(packet.substr(1), ',', addrStr, lenStr))
	      reply << "E01";
	    else
	      {
		URV addr = 0, len = 0;
		if (not hexToInt(addrStr, addr) or not hexToInt(lenStr, len))
		  reply << "E02";
		else
		  {
		    for (URV ix = 0; ix < len; ++ix)
		      {
			uint8_t byte = 0, high = 0, low = 0;
			hart.peekMemory(addr++, byte);
                        byteToHexChars(byte, high, low);
                        reply.write((char*) &high, 1);
                        reply.write((char*) &low, 1);
		      }
		  }
	      }
	  }
	  break;

	case 'M': // MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK
	  {
	    std::string addrStr, lenStr, data;
	    if (not getStringComponents(packet.substr(1), ',', ':', addrStr,
					lenStr, data))
	      reply << "E01";
	    else
	      {
		URV addr = 0, len = 0;
		if (not hexToInt(addrStr, addr) or not hexToInt(lenStr, len))
		  reply << "E02";
		else
		  {
		    if (data.size() < len*2)
		      reply << "E03";
		    else
		      {
			for (URV ix = 0; ix < len; ++ix)
			  {
			    int bb = hexCharToInt(data.at(2*ix));
			    bb = (bb << 4) | hexCharToInt(data.at(2*ix+1));
                            uint8_t val = bb;
			    hart.pokeMemory(addr, val);
                            addr++;
			  }
			reply << "OK";
		      }
		  }
	      }
	  }
	  break;

	case 'c':  // cAA..AA    Continue at address AA..AA(optional)
	  {
            if (not hart.isStarted())
              {
                std::cerr << "Hart not yet started (need to write mhartstart)\n";
                reply << "E01";
                break;
              }

	    if (packet.size() == 1)
	      return;

	    URV newPc = 0;
	    if (hexToInt(packet.substr(1), newPc))
	      {
		hart.pokePc(newPc);
		return;
	      }

	    reply << "E01";
	  }
	  break;

	case 'p':  // pn    Read value of register n
	  {
	    std::string regNumStr = packet.substr(1);
	    unsigned regNum = 0;

	    if (not hexToInt(regNumStr, regNum))
	      reply << "E01";
	    else
	      handlePeekRegisterForGdb(hart, regNum, reply);
	  }
	  break;

	case 'P':   // Pn=v   Set register n to v
	  {
	    auto eqIx = packet.find('=');
	    if (eqIx == std::string::npos or eqIx == 1)
	      reply << "E01";
	    else
	      {
		std::string regNumStr = packet.substr(1, eqIx - 1);
		std::string valueStr = packet.substr(eqIx + 1);
		unsigned regNum = 0;
		URV value = 0;
		if (not hexToInt(regNumStr, regNum))
		  reply << "E02";
		else if (not littleEndianHexToInt(valueStr, value))
		  reply << "E03";
		else
		  {
                    bool ok = true;
		    if (regNum < pcOffset)
		      ok = hart.pokeIntReg(regNum, value);
		    else if (regNum == pcOffset)
		      hart.pokePc(value);
		    else if (regNum >= fpRegOffset and regNum < csrOffset)
		      ok = hart.pokeFpReg(regNum - fpRegOffset, value);
                    else if (regNum >= csrOffset)
                      ok = hart.pokeCsr(WdRiscv::CsrNumber(regNum - csrOffset), value);
		    reply << (ok? "OK" : "E04");
		  }
	      }
	  }
	  break;

	case 's':
          if (not hart.isStarted())
            {
              std::cerr << "Hart not yet started (need to write mhartstart)\n";
              reply << "E01";
              break;
            }

	  hart.singleStep(nullptr);
	  notifyGdbAfterStop(hart, fd);
	  continue;
	  break;

	case 'k':  // kill
	  reply << "OK";
	  gotQuit = true;
	  break;

	case 'q':
	  if (packet == "qC")
	    reply << "QC 0";
	  else if (packet == "qAttached")
	    reply << "0";
	  else if (packet == "qOffsets")
	    reply << "Text=0;Data=0;Bss=0";
	  else if (packet == "qSymbol::")
	    reply << "OK";
	  else if (packet == "qfThreadInfo")
	    reply << "m0";
	  else if (packet == "qsThreadInfo")
	    reply << "l";
	  else if (packet == "qTStatus")
	    reply << "T0;tnotrun:0";
          else if (boost::starts_with(packet, "qSupported"))
            reply << "qXfer:features:read+";
          else if (boost::starts_with(packet, "qXfer"))
            processXferQuery(packet, hart, reply);
	  else
	    {
	      std::cerr << "Unhandled gdb request: " << packet << '\n';
	      reply << ""; // Unsupported: Empty response.
	    }
	  break;

	case 'v':
	  if (packet == "vMustReplyEmpty")
	    reply << "";
	  else if (packet.find("vKill;") == 0)
	    {
	      reply << "OK";
	      gotQuit = true;
	    }
	  else if (packet == "vCont?")
	    {
	      reply << "";
	    }
	  else
	    {
	      std::cerr << "Unhandled gdb request: " << packet << '\n';
	      reply << ""; // Unsupported: Empty response.
	    }
	  break;

	default:
	  std::cerr << "Unhandled gdb request: " << packet << '\n';
	  reply << "";   // Unsupported comand: Empty response.
	}

      // Reply to the request
      sendPacketToGdb(reply.str(), fd);

      if (gotQuit)
	exit(0);
    }
}

template void handleExceptionForGdb<uint32_t>(WdRiscv::Hart<uint32_t>&, int);
template void handleExceptionForGdb<uint64_t>(WdRiscv::Hart<uint64_t>&, int);

ext: cpp

max_stars_repo_name: Daichou/SweRV-ISS

max_issues_repo_name: Daichou/SweRV-ISS

max_forks_repo_name: Daichou/SweRV-ISS


text:
// Copyright David Stone 2020.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <tm/clients/pokemon_lab/read_team_file.hpp>

#include <tm/move/move.hpp>

#include <tm/pokemon/pokemon.hpp>
#include <tm/pokemon/species.hpp>

#include <tm/stat/ev.hpp>
#include <tm/stat/iv.hpp>
#include <tm/stat/iv_and_ev.hpp>

#include <tm/string_conversions/ability.hpp>
#include <tm/string_conversions/gender.hpp>
#include <tm/string_conversions/item.hpp>
#include <tm/string_conversions/move.hpp>
#include <tm/string_conversions/nature.hpp>
#include <tm/string_conversions/species.hpp>

#include <tm/constant_generation.hpp>
#include <tm/generation.hpp>
#include <tm/team.hpp>

#include <containers/algorithms/concatenate.hpp>
#include <containers/array/array.hpp>
#include <containers/flat_map.hpp>
#include <containers/lookup.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include <filesystem>
#include <string>
#include <string_view>
#include <stdexcept>

namespace technicalmachine::pl {
namespace {
using namespace std::string_view_literals;

constexpr auto parse_species(std::string_view const str) {
	using Storage = containers::array<containers::map_value_type<std::string_view, Species>, 16>;
	constexpr auto converter = containers::basic_flat_map<Storage>(
		containers::assume_sorted_unique,
		Storage{{
			{ "Deoxys", Species::Deoxys_Normal },
			{ "Deoxys-e", Species::Deoxys_Speed },
			{ "Deoxys-f", Species::Deoxys_Attack },
			{ "Deoxys-l", Species::Deoxys_Defense },
			{ "Giratina", Species::Giratina_Altered },
			{ "Giratina-o", Species::Giratina_Origin },
			{ "Rotom-c", Species::Rotom_Mow },
			{ "Rotom-f", Species::Rotom_Frost },
			{ "Rotom-h", Species::Rotom_Heat },
			{ "Rotom-s", Species::Rotom_Fan },
			{ "Rotom-w", Species::Rotom_Wash },
			{ "Shaymin", Species::Shaymin_Land },
			{ "Shaymin-s", Species::Shaymin_Sky },
			{ "Wormadam", Species::Wormadam_Plant },
			{ "Wormadam-g", Species::Wormadam_Sandy },
			{ "Wormadam-s", Species::Wormadam_Trash }
		}}
	);
	auto const result = containers::lookup(converter, str);
	return result ? *result : from_string<Species>(str);
}

auto parse_moves(Generation const generation, boost::property_tree::ptree const & pt) {
	auto moves = RegularMoves();
	for (boost::property_tree::ptree::value_type const & value : pt) {
		auto const name = from_string<Moves>(value.second.get_value<std::string>());
		auto const pp_ups = value.second.get<PP::pp_ups_type>("<xmlattr>.pp-up");
		moves.push_back(Move(generation, name, pp_ups));
	}
	return moves;
}

template<Generation generation>
auto parse_stats(boost::property_tree::ptree const & pt) {
	using StatType = std::conditional_t<generation <= Generation::two, DVAndEV, IVAndEV>;
	auto hp = bounded::optional<StatType>();
	auto atk = bounded::optional<StatType>();
	auto def = bounded::optional<StatType>();
	auto spa = bounded::optional<StatType>();
	auto spd = bounded::optional<StatType>();
	auto spe = bounded::optional<StatType>();

	for (auto const & value : pt.get_child("stats")) {
		auto const & stats = value.second;
		auto const & stat_name = stats.get<std::string>("<xmlattr>.name");
		using IVType = std::conditional_t<generation <= Generation::two, DV, IV>;
		auto const iv_and_ev = StatType{
			IVType(stats.get<typename IVType::value_type>("<xmlattr>.iv")),
			EV(stats.get<EV::value_type>("<xmlattr>.ev"))
		};
		if (stat_name == "HP") {
			hp = iv_and_ev;
		} else if (stat_name == "Atk") {
			atk = iv_and_ev;
		} else if (stat_name == "Def") {
			def = iv_and_ev;
		} else if (stat_name == "SpAtk") {
			spa = iv_and_ev;
		} else if (stat_name == "SpDef") {
			spd = iv_and_ev;
		} else if (stat_name == "Spd") {
			spe = iv_and_ev;
		} else {
			throw std::runtime_error(containers::concatenate<std::string>(std::string_view("Invalid stat name "), stat_name));
		}
	}
	if (!hp or !atk or !def or !spa or !spd or !spe) {
		throw std::runtime_error("IV or EV is missing");
	}

	if constexpr (generation <= Generation::two) {
		// I'm not sure if Pokemon Lab ever supported older generations, but if
		// it did this is probably what the file would look like?
		if (*spa != *spd) {
			throw std::runtime_error("Mismatched Special DV / EV");
		}
		auto const dvs = DVs(atk->dv, def->dv, spe->dv, spa->dv);
		if (dvs.hp() != hp->dv) {
			throw std::runtime_error("Invalid HP DV");
		}
		return CombinedStats<generation>{
			Nature::Hardy,
			dvs,
			OldGenEVs(hp->ev, atk->ev, def->ev, spe->ev, spa->ev)
		};
	} else {
		return CombinedStats<generation>{
			from_string<Nature>(pt.get<std::string>("nature")),
			IVs(hp->iv, atk->iv, def->iv, spa->iv, spd->iv, spe->iv),
			EVs(hp->ev, atk->ev, def->ev, spa->ev, spd->ev, spe->ev)
		};
	}
}

template<Generation generation>
auto parse_pokemon(boost::property_tree::ptree const & pt) {
	// auto const given_nickname = pt.get<std::string>("nickname");
	// auto const nickname = nickname_temp.empty() ? species_str : given_nickname;
	return Pokemon<generation>(
		parse_species(pt.get<std::string>("<xmlattr>.species")),
		Level(pt.get<Level::value_type>("level")),
		Gender(from_string<Gender>(pt.get<std::string>("gender"))),
		from_string<Item>(pt.get<std::string>("item")),
		Ability(from_string<Ability>(pt.get<std::string>("ability"))),
		parse_stats<generation>(pt),
		parse_moves(generation, pt.get_child("moveset")),
		Happiness(pt.get<Happiness::value_type>("happiness"))
	);
}

auto number_of_pokemon(boost::property_tree::ptree const & pt) -> TeamSize {
	auto pokemon_count = TeamSize(0_bi);
	for (auto const & value : pt) {
		if (value.first == "pokemon") {
			if (pokemon_count == numeric_traits::max_value<TeamSize>) {
				throw std::runtime_error("Attempted to add too many Pokemon");
			}
			++pokemon_count;
		}
	}
	return pokemon_count;
}

template<Generation generation>
auto parse_team(boost::property_tree::ptree const & all_pokemon) {
	constexpr bool is_me = true;
	auto const team_size = number_of_pokemon(all_pokemon);
	auto team = Team<generation>(team_size, is_me);
	for (auto const & value : all_pokemon) {
		if (value.first == "pokemon") {
			team.add_pokemon(parse_pokemon<generation>(value.second));
		}
	}
	team.all_pokemon().set_index(0_bi);
	return team;
}

} // namespace

auto read_team_file(std::filesystem::path const & team_file) -> GenerationGeneric<Team> {
	try {
		boost::property_tree::ptree pt;
		read_xml(team_file.string(), pt);

		auto const all_pokemon = pt.get_child("shoddybattle");
		using GenerationInteger = bounded::integer<1, 7>;
		// The original format did not include a generation. Require users to add
		// this field.
		auto const parsed_generation = static_cast<Generation>(all_pokemon.get<GenerationInteger>("<xmlattr>.generation"));
		return constant_generation(parsed_generation, [&]<Generation generation>(constant_gen_t<generation>) {
			return GenerationGeneric<Team>(parse_team<generation>(all_pokemon));
		});
	} catch (std::exception const & ex) {
		throw std::runtime_error(containers::concatenate<std::string>("Failed to parse Pokemon Lab team file \""sv, team_file.string(), "\" -- "sv, std::string_view(ex.what())));
	}
}

} // namespace technicalmachine::pl

max_stars_repo_name: davidstone/technical-machine

max_issues_repo_name: davidstone/technical-machine

max_forks_repo_name: davidstone/technical-machine


text:
#include <boost/test/unit_test.hpp>
#include <joint_control_base/MotionConstraint.hpp>
#include <joint_control_base/ConstrainedJointsCmd.hpp>

using namespace std;

BOOST_AUTO_TEST_CASE(motion_constraint){
    joint_control_base::MotionConstraint constraint;
    BOOST_CHECK(constraint.hasMaxPosition() == false);
    BOOST_CHECK(constraint.hasMinPosition() == false);
    BOOST_CHECK(constraint.hasMaxVelocity() == false);
    BOOST_CHECK(constraint.hasMaxAcceleration() == false);
    BOOST_CHECK(constraint.hasMaxJerk() == false);

    BOOST_CHECK_THROW(constraint.validatePositionLimits(), std::invalid_argument);
    BOOST_CHECK_THROW(constraint.validateAccelerationLimit(), std::invalid_argument);
    BOOST_CHECK_THROW(constraint.validateJerkLimit(), std::invalid_argument);
    BOOST_CHECK_THROW(constraint.validateVelocityLimit(), std::invalid_argument);

    joint_control_base::MotionConstraint default_constraint;
    default_constraint.max.position = (double)rand()/RAND_MAX;
    default_constraint.min.position = (double)rand()/RAND_MAX;
    default_constraint.max.speed= (double)rand()/RAND_MAX;
    default_constraint.max.acceleration = (double)rand()/RAND_MAX;
    default_constraint.max_jerk = (double)rand()/RAND_MAX;
    constraint.applyDefaultIfUnset(default_constraint);

    BOOST_CHECK(constraint.max.position == default_constraint.max.position);
    BOOST_CHECK(constraint.min.position == default_constraint.min.position);
    BOOST_CHECK(constraint.max.speed == default_constraint.max.speed);
    BOOST_CHECK(constraint.max.acceleration == default_constraint.max.acceleration);
    BOOST_CHECK(constraint.max_jerk == default_constraint.max_jerk);
}

BOOST_AUTO_TEST_CASE(constrained_joints_cmd){
    joint_control_base::ConstrainedJointsCmd cmd;
    BOOST_CHECK(cmd.empty() == true);

    cmd.names.push_back(("joint_1"));
    BOOST_CHECK_THROW(cmd.validate(),std::invalid_argument);
    cmd.elements.push_back(base::JointState());
    BOOST_CHECK_NO_THROW(cmd.validate());

    joint_control_base::MotionConstraint constraint;
    constraint.max.position = (double)rand()/RAND_MAX;
    constraint.min.position = (double)rand()/RAND_MAX;
    constraint.max.speed= (double)rand()/RAND_MAX;
    constraint.max.acceleration = (double)rand()/RAND_MAX;
    constraint.max_jerk = (double)rand()/RAND_MAX;
    cmd.motion_constraints.push_back(constraint);
    BOOST_CHECK_NO_THROW(cmd.validate());

    cmd.motion_constraints.push_back(constraint);
    BOOST_CHECK_THROW(cmd.validate(), std::invalid_argument);
}


text: #include <boost/spirit/repository/include/qi_keywords.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
//------------------------------------------------------------------------------
/*
    This file is part of rippled: https://github.com/casinocoin/casinocoind
    Copyright (c) 2018 CasinoCoin Foundation

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

//==============================================================================
/*
    2018-05-26  jrojek          Created
*/
//==============================================================================

#include <BeastConfig.h>
#include <casinocoin/app/ledger/OpenLedger.h>
#include <casinocoin/app/main/Application.h>
#include <casinocoin/app/misc/NetworkOPs.h>
#include <casinocoin/overlay/Overlay.h>
#include <casinocoin/overlay/impl/PeerImp.h>
#include <casinocoin/overlay/Message.h>
#include <casinocoin/overlay/predicates.h>
#include <casinocoin/app/misc/CRN.h>
#include <boost/format.hpp>
#include <boost/regex.hpp>
#include <algorithm>
#include <mutex>

namespace casinocoin {

const CRN::EligibilityMap CRN::eligibilityMapNone = CRN::EligibilityMap();

CRN::CRN(const PublicKey &pubKey,
         const std::string &domain,
         const std::string &domainSignature,
         const uint16_t wsPort,
         NetworkOPs &networkOps,
         const LedgerIndex &startupSeq,
         beast::Journal j,
         Config &conf,
         LedgerMaster& ledgerMaster)
    : id_(pubKey, domain, domainSignature, wsPort, j, conf, ledgerMaster)
    , performance_(networkOps, startupSeq, id_, j)
    , j_(j)
{

}

CRN::CRN(Config &conf,
         NetworkOPs& networkOps,
         LedgerIndex const& startupSeq,
         beast::Journal j,
         LedgerMaster& ledgerMaster)
    : id_(conf, j, ledgerMaster)
    , performance_(networkOps, startupSeq, id_, j)
    , j_(j)
{

}

Json::Value CRN::json() const
{
    return id_.json();
}

CRNId const& CRN::id() const
{
    return id_;
}

bool CRN::activated() const
{
    bool crnActivated = false;
    JLOG(j_.info()) << "CRN Activated: " << crnActivated;
    return crnActivated;
}

STPerformanceReport::pointer
CRN::prepareReport(
    LedgerIndex const& lastClosedLedgerSeq,
    Application &app)
{
    return performance_.prepareReport(lastClosedLedgerSeq, app);
}

void CRN::broadcast(STPerformanceReport::ref report, Application &app)
{
    performance_.broadcast(report, app);
}

std::unique_ptr<CRN> make_CRN(Config &conf,
                              NetworkOPs &networkOps,
                              const LedgerIndex &startupSeq,
                              beast::Journal j,
                              LedgerMaster& ledgerMaster)
{
    return std::make_unique<CRN>(conf,
                                 networkOps,
                                 startupSeq,
                                 j,
                                 ledgerMaster);
}

std::unique_ptr<CRN> make_CRN(PublicKey const& pubKey,
                              std::string const& domain,
                              std::string const& domainSignature,
                              uint16_t const& wsPort,
                              NetworkOPs &networkOps,
                              LedgerIndex const& startupSeq,
                              beast::Journal j,
                              Config &conf,
                              LedgerMaster& ledgerMaster)
{
    return std::make_unique<CRN>(pubKey,
                                 domain,
                                 domainSignature,
                                 wsPort,
                                 networkOps,
                                 startupSeq,
                                 j,
                                 conf,
                                 ledgerMaster);
}

}

max_stars_repo_name: rrozek/casinocoind

max_issues_repo_name: rrozek/casinocoind

max_forks_repo_name: rrozek/casinocoind


text:
// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>

#include <asp/Core/FileUtils.h>

namespace asp{

  using namespace vw;

  bool is_latest_timestamp(std::string              const& test_file,
                           std::vector<std::string> const& other_files) {
    if (!boost::filesystem::exists(test_file))
      return false;
    std::time_t test_time = boost::filesystem::last_write_time(test_file);
    for (size_t i=0; i<other_files.size(); ++i) {
      if (other_files[i] == "") // Ignore blank files that were passed in.
        continue;
      if (!boost::filesystem::exists(other_files[i]))
        return false;
      std::time_t t = boost::filesystem::last_write_time(other_files[i]);
      if (test_time < t)
        return false;
    }
    return true;
  }

  bool is_latest_timestamp(std::string const& test_file, std::string const& f1) {
    std::vector<std::string> vec(1);
    vec[0] = f1;
    return is_latest_timestamp(test_file, vec);
  }
  bool is_latest_timestamp(std::string const& test_file,
                           std::string const& f1, std::string const& f2) {
    std::vector<std::string> vec(2);
    vec[0] = f1;  vec[1] = f2;
    return is_latest_timestamp(test_file, vec);
  }
  bool is_latest_timestamp(std::string const& test_file,
                           std::string const& f1, std::string const& f2,
                           std::string const& f3, std::string const& f4) {
    std::vector<std::string> vec(4);
    vec[0] = f1;  vec[1] = f2;
    vec[2] = f2;  vec[3] = f4;
    return is_latest_timestamp(test_file, vec);
  }

  void read_1d_points(std::string const& file, std::vector<double> & points){

    std::ifstream ifs(file.c_str());
    if (!ifs.good())
      vw_throw( ArgumentErr() << "Could not open file for reading: " << file << "\n" );

    double a;
    points.clear();
    while (ifs >> a)
      points.push_back(a);
  }

  void read_2d_points(std::string const& file, std::vector<Vector2> & points){

    std::ifstream ifs(file.c_str());
    if (!ifs.good())
      vw_throw( ArgumentErr() << "Could not open file for reading: " << file << "\n" );

    double a, b;
    points.clear();
    while (ifs >> a >> b){
      Vector2 p(a, b);
      points.push_back(p);
    }
  }

  void read_3d_points(std::string const& file,
                      std::vector<Vector3> & points){

    std::ifstream ifs(file.c_str());
    if (!ifs.good())
      vw_throw( ArgumentErr() << "Could not open file for reading: " << file << "\n" );

    double a, b, c;
    points.clear();
    while (ifs >> a >> b >> c){
      Vector3 p(a, b, c);
      points.push_back(p);
    }
  }

}

ext: cc

max_stars_repo_name: AndrewAnnex/StereoPipeline

max_issues_repo_name: AndrewAnnex/StereoPipeline

max_forks_repo_name: AndrewAnnex/StereoPipeline


text:
// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// Generalized Pose-and-Scale Estimation using 4-Point Congruence Constraints
//
// Victor Fragoso and Sudipta Sinha.
// In Proc. of the IEEE International Conf. on 3D Vision (3DV) 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <gp4pc/pinhole_camera.h>

#include <cmath>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

namespace msft {

PinholeCamera::PinholeCamera(const double focal_length,
                             const Eigen::Vector2d& principal_point,
                             const Eigen::Quaterniond& world_to_camera_rotation,
                             const Eigen::Vector3d& world_to_camera_translation)
    : rotation_(world_to_camera_rotation.normalized()),
      translation_(world_to_camera_translation) {
  intrinsics_mat_ <<
      focal_length, 0.0, principal_point.x(),
      0.0, focal_length, principal_point.y(),
      0.0, 0.0, 1.0;
}

PinholeCamera::PinholeCamera(const double focal_length,
                             const Eigen::Vector2d& principal_point,
                             const Eigen::Vector4d& world_to_camera_rotation,
                             const Eigen::Vector3d& world_to_camera_translation)
    : PinholeCamera(focal_length,
                    principal_point,
                    Eigen::Quaterniond(world_to_camera_rotation[0],
                                       world_to_camera_rotation[1],
                                       world_to_camera_rotation[2],
                                       world_to_camera_rotation[3]),
                    world_to_camera_translation) {
  CHECK(std::abs(world_to_camera_rotation.norm() - 1.0) <
        std::numeric_limits<double>::epsilon())
      << "Quaternion has not unit norm";
}

double PinholeCamera::ProjectPoint(const Eigen::Vector3d& point3d,
                                   Eigen::Vector2d* point2d) const {
  const Eigen::Vector3d point_in_camera = rotation_ * point3d + translation_;
  *CHECK_NOTNULL(point2d) = (intrinsics_mat_ * point_in_camera).hnormalized();
  return point_in_camera.z();
}

Eigen::Vector3d
PinholeCamera::PixelToUnitRay(const Eigen::Vector2d& pixel) const {
  const Eigen::Vector3d ray =
      rotation_.conjugate() *
      (intrinsics_mat_.inverse() * pixel.homogeneous()).normalized();
  return ray;
}

Eigen::Vector3d PinholeCamera::GetPosition() const {
  const Eigen::Vector3d camera_position = rotation_.conjugate() * -translation_;
  return camera_position;
}

}  // namespace msft


text:
#include "stdafx.h"
#include "html_processing.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE( test_html_processing )

BOOST_AUTO_TEST_CASE( test_init )
{
	html_processing processing ;
	BOOST_CHECK ( ! processing.is_underline_specified() ) ;
	BOOST_CHECK ( ! processing.is_italic_specified() ) ;
	BOOST_CHECK ( ! processing.is_bold_specified() ) ;
	BOOST_CHECK ( ! processing.is_font_size_specified() ) ;
	BOOST_CHECK ( ! processing.is_fore_color_specified() ) ;
}

BOOST_AUTO_TEST_CASE( test_html_size_to_point_size )
{
	BOOST_CHECK_EQUAL(8u, html_size_to_point_size(1u)) ;
	BOOST_CHECK_EQUAL(10u, html_size_to_point_size(2u)) ;
	BOOST_CHECK_EQUAL(12u, html_size_to_point_size(3u)) ;
	BOOST_CHECK_EQUAL(14u, html_size_to_point_size(4u)) ;
	BOOST_CHECK_EQUAL(18u, html_size_to_point_size(5u)) ;
	BOOST_CHECK_EQUAL(24u, html_size_to_point_size(6u)) ;
	BOOST_CHECK_EQUAL(36u, html_size_to_point_size(7u)) ;
}

BOOST_AUTO_TEST_CASE( test_html_size_to_point_size_out_of_bounds )
{
	BOOST_CHECK_EQUAL(8u, html_size_to_point_size(0u)) ;
	BOOST_CHECK_EQUAL(36u, html_size_to_point_size(8u)) ;
}

BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_8 )
{
	BOOST_CHECK_EQUAL(1u, point_size_to_html_size(8u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_10 )
{
	BOOST_CHECK_EQUAL(2u, point_size_to_html_size(10u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_11 )
{
	BOOST_CHECK_EQUAL(2u, point_size_to_html_size(11u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_12 )
{
	BOOST_CHECK_EQUAL(3u, point_size_to_html_size(12u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_14 )
{
	BOOST_CHECK_EQUAL(4u, point_size_to_html_size(14u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_18 )
{
	BOOST_CHECK_EQUAL(5u, point_size_to_html_size(18u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_19 )
{
	BOOST_CHECK_EQUAL(5u, point_size_to_html_size(19u)) ;
}

BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_24 )
{
	BOOST_CHECK_EQUAL(6u, point_size_to_html_size(24u)) ;
}
BOOST_AUTO_TEST_CASE( test_point_size_to_html_size_36 )
{
	BOOST_CHECK_EQUAL(7u, point_size_to_html_size(36u)) ;
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: ultimatezen/felix

max_issues_repo_name: ultimatezen/felix

max_forks_repo_name: ultimatezen/felix


text:
/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "flint/ls.h"

#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <string>

#include <boost/uuid/nil_generator.hpp>
#include <boost/uuid/uuid.hpp>

#include "bc/index.h"
#include "flint/dps.h"
#include "flint/key.h"
#include "flint/utf8path.h"
#include "isdf/reader.h"
#include "lo/layout.h"

namespace flint {
namespace ls {

namespace {

class Handler {
public:
	explicit Handler(std::map<int, key::Data> &m)
		: m_(m)
	{}

	void GetDescription(std::uint32_t i, std::uint32_t num_bytes, const char *d) {
		if (i == 0) // skip the first column
			return;
		key::Data kd;
		if (num_bytes < 38) {
			kd.uuid = boost::uuids::nil_uuid();
			kd.name = std::string(d, num_bytes);
		} else {
			// TODO: check returned value
			(void)key::Data::FromString(std::string(d, num_bytes), &kd);
		}
		m_.emplace(i, kd);
	}

private:
	std::map<int, key::Data> &m_;
};

}

std::unique_ptr<Configuration> CreateConfiguration(const char *dps_path, const Layout &layout)
{
	std::unique_ptr<Configuration> config;
	boost::filesystem::path path = GetPathFromUtf8(dps_path);
	std::map<int, key::Data> m_dps;
	{
		boost::filesystem::ifstream ifs(path, std::ios::in|std::ios::binary);
		if (!ifs)
			return config;
		isdf::Reader reader;
		if (!reader.ReadHeader(&ifs))
			return config;
		if (!reader.SkipComment(&ifs))
			return config;
		Handler handler(m_dps);
		if (!reader.ReadDescriptions(handler, &ifs))
			return config;
		ifs.close();
		config.reset(new Configuration);
		config->data_offset = reader.GetDataOffset();
		config->row_size = reader.num_objs() * sizeof(double);
	}
	std::map<key::Data, size_t> m_data;
	for (const auto &p : m_dps)
		m_data.emplace(p.second, 0);
	if (!layout.SelectByKeyData(&m_data)) {
		config.reset();
		return config;
	}
	for (const auto &p : m_dps) {
		auto it = m_data.find(p.second);
		if (it == m_data.end()) // ignore missing data points
			continue;
		if (it->second == 0) // found in data points, but not found in the layout
			continue;
		auto r = config->indice.emplace(p.first, it->second);
		assert(r.second);
	}
	auto path_s = path.string();
	boost::interprocess::file_mapping fm(path_s.c_str(), boost::interprocess::read_only);
	config->fm = std::move(fm);
	config->bound = std::numeric_limits<double>::max();
	return config;
}

Accumulator::Accumulator(Configuration &config, double &sum)
	: config_(config)
	, cursor_(config.fm, config.data_offset, config.row_size)
	, sum_(sum)
{}

Accumulator::State Accumulator::operator()(const double *data)
{
	double *p;
	switch (cursor_(data[kIndexTime], &p)) {
	case dps::Cursor::Position::kGt:
		return State::kLeq;
	case dps::Cursor::Position::kLeq:
		{
			// access the mapped region sequentially
			for (const auto &idx : config_.indice)
				sum_ += std::pow(p[idx.first] - data[idx.second], 2);
			std::lock_guard<std::mutex> g(config_.mutex);
			if (config_.bound < sum_)
				return State::kGt;
			return State::kLeq;
		}
	default:
		{
			UpdateBound();
			return State::kDone;
		}
	}
}

void Accumulator::UpdateBound()
{
	std::lock_guard<std::mutex> g(config_.mutex);
	if (sum_ < config_.bound)
		config_.bound = sum_;
}

}
}

max_stars_repo_name: Abhisheknishant/Flint

max_issues_repo_name: Abhisheknishant/Flint

max_forks_repo_name: Abhisheknishant/Flint


text:
#ifndef SODA_CIRCULAR_BUFFER_HDR
#define SODA_CIRCULAR_BUFFER_HDR

#include <cstring>
#include <iostream>
#include <mutex>
#include <boost/format.hpp>

namespace SoDa {

  /**
   * @class CircularBuffer
   *
   * @brief A circular buffer for vectors of samples (as in,
   * audio samples waiting to be consumed by a callback).
   *
   */
  template<typename T> class CircularBuffer {
  public:
    CircularBuffer(size_t elements) {
      buffer_elements = elements;
      buffer = new T[buffer_elements];
      head_pointer = buffer;
      tail_pointer = buffer;
      num_written = 0;
      num_read = 0;
    }

    /**
     * @brief clear the buffer by reseting the pointers.
     */
    void clear() {
      std::lock_guard<std::mutex> lck(pointer_mutex);
      head_pointer = buffer;
      tail_pointer = buffer;
      num_written = num_read = 0;
    }

    size_t put(const T * in, size_t len) {
      // several interesting cases:
      //
      // 1. room left between head pointer and end of the buffer:
      //    copy the entire buffer, bump head pointer by len;
      // 2. insufficient room between head pointer and end of buffer:
      //    copy the first part (pre_len) up to the end of the buffer
      //    copy the second part to the start of the buffer.
      //
      // Then independently, the head pointer could pass the tail
      // pointer.  We know that it has if len + numElements is greater
      // than the buffer size. If the head pointer passes the tail
      // pointer, then we set the tail pointer equal to the head
      // pointer plus one (wrapped).
      //
      bool lock_head_tail = (numElements() + len) > buffer_elements;
      size_t nput = 0;
      while(len > 0) {
	// need to do this in a loop, because we might actually
	// have a really really large buffer.
	// we're only going to copy up to the end of the buffer.
	size_t this_len = min(len, buffer + buffer_elements - head_pointer);

	// move the samples.
	memcpy(head_pointer, in, sizeof(T) * this_len);
	{
	  std::lock_guard<std::mutex> lck(pointer_mutex);
	  head_pointer = wrapBufferPointer(head_pointer, this_len);
	}

	// we moved some samples, update the input pointer
	in = in + this_len;

	// how much is left?
	len = len - this_len;
	// how much did we write?
	nput = nput + this_len;
      }

      if(lock_head_tail) {
	std::lock_guard<std::mutex> lck(pointer_mutex);
	tail_pointer = head_pointer;
      }

      num_written += nput;
      return nput;
    }

    size_t get(T * out, size_t len) {
      // several interesting cases, just like "put":
      //
      // 1. room left between tail pointer and end of the buffer:
      //    copy the entire buffer, bump head pointer by len;
      // 2. insufficient room between tail pointer and end of buffer:
      //    copy the first part (pre_len) up to the end of the buffer
      //    copy the second part to the start of the buffer.
      //
      // If the length is greater than the number of elements in
      // the buffer, we return a shorter buffer.
      //

      if (len > numElements()) {
	len = numElements();
      }

      size_t pre_len = min(len, buffer + buffer_elements - tail_pointer);
      size_t post_len = len - pre_len;

      if(pre_len > 0) {
	memcpy(out, tail_pointer, sizeof(T) * pre_len);
	{
	  std::lock_guard<std::mutex> lck(pointer_mutex);
	  tail_pointer = wrapBufferPointer(tail_pointer, pre_len);
	}
      }
      if(post_len > 0) {
	memcpy(out + pre_len, tail_pointer, sizeof(T) * post_len);
	{
	  std::lock_guard<std::mutex> lck(pointer_mutex);
	  tail_pointer = wrapBufferPointer(tail_pointer, post_len);
	}
      }

      num_read += len;
      return len;
    }

    size_t put(const std::vector<T> & in) {
      return put(in.data(), in.size());
    }

    size_t get(std::vector<T> & out) {
      return get(out.data(), out.size());
    }

    size_t numElements() {
      size_t ret = num_written - num_read;
      if(ret > buffer_elements) {
	// we have over-run the buffer;
	ret = buffer_elements;
	// so we need to correct the number_read
	// to "reset" and ignore the over-written elements.
	num_read = num_written - buffer_elements;
      }
      return ret;
    }

    void dump(std::ostream & os) {
      os << boost::format("buffer       = %p\n") % buffer;
      os << boost::format("head_pointer = %p\n") % head_pointer;
      os << boost::format("tail_pointer = %p\n") % tail_pointer;
      os << boost::format("buffer_elements    = %d\n") % buffer_elements;
      os << boost::format("available elements = %d\n") % numElements();
      os << boost::format("num_written        = %d\n") % num_written;
      os << boost::format("num_read           = %d\n") % num_read;
      for(int i = 0; i < buffer_elements; i++) {
	char tp, hp;
	tp = (tail_pointer == (buffer + i)) ? 'T' : ' ';
	hp = (head_pointer == (buffer + i)) ? 'H' : ' ';
	os << boost::format("%c%c %4d: ") % tp % hp % i;
	os << buffer[i] << std::endl;
      }
    }

    template <typename T2> void dumpBuf(std::ostream & os) {
      T2 * tptr = (T2*) buffer;
      for(int i = 0; i < (buffer_elements * sizeof(T)) / sizeof(T2); i++) {
	os << i << " " << tptr[i] << std::endl;
      }
    }
  private:
    size_t min(size_t a, size_t b) {
      return (a < b) ? a : b;
    }

    T * wrapBufferPointer(T * ptr, size_t len) {
      T * ret = ptr + len;
      size_t offset = ret - buffer;
      if(offset >= buffer_elements) {
	// the pointer has wrapped.
	ret = buffer + (offset - buffer_elements);
      }

      return ret;
    }

    T * buffer;
    size_t buffer_elements;
    T * head_pointer; ///< points to the next element to be written
    T * tail_pointer; ///< points to the next element to be read
    size_t num_written, num_read;

    // locks for tail and head pointer
    std::mutex pointer_mutex;
  };
}
#endif

ext: hxx


text:
#pragma once
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include "BaseNeuron.hpp"
#include "RecurrentNeuron.hpp"

namespace snn::internal
{
    class GatedRecurrentUnit final
    {
    private:
        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

        friend class RecurrentNeuron;

        std::vector<float> errors;

        int numberOfInputs{};

        float previousOutput = 0;
        float recurrentError = 0;
        float updateGateOutput = 0;
        float outputGateOutput = 0;

        RecurrentNeuron resetGate;
        RecurrentNeuron updateGate;
        RecurrentNeuron outputGate;

        void reset();

    public:
        GatedRecurrentUnit() = default; // use restricted to Boost library only
        GatedRecurrentUnit(NeuronModel model, std::shared_ptr<NeuralNetworkOptimizer> optimizer);
        GatedRecurrentUnit(const GatedRecurrentUnit& recurrentNeuron) = default;
        ~GatedRecurrentUnit() = default;

        [[nodiscard]] float output(const std::vector<float>& inputs, bool reset);
        [[nodiscard]] std::vector<float>& backOutput(float error);
        void train(float error);

        [[nodiscard]] std::vector<float> getWeights() const;
        [[nodiscard]] int getNumberOfParameters() const;
        [[nodiscard]] int getNumberOfInputs() const;

        [[nodiscard]] int isValid() const;

        NeuralNetworkOptimizer* getOptimizer() const;
        void setOptimizer(std::shared_ptr<NeuralNetworkOptimizer> newOptimizer);

        bool operator==(const GatedRecurrentUnit& neuron) const;
        bool operator!=(const GatedRecurrentUnit& neuron) const;
    };

    template <class Archive>
    void GatedRecurrentUnit::serialize(Archive& ar, [[maybe_unused]] const unsigned version)
    {
        ar & this->numberOfInputs;
        ar & this->previousOutput;
        ar & this->recurrentError;
        ar & this->updateGateOutput;
        ar & this->outputGateOutput;
        ar & this->resetGate;
        ar & this->updateGate;
        ar & this->outputGate;
    }
}


text:
/*
 * Copyright (C) 2007-2013 Istituto Italiano di Tecnologia ADVR & iCub Facility
 * Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro and Alessandro Settimi
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 */

#ifndef GAZEBOYARP_CONFIGURATION_HELPERS_HH
#define GAZEBOYARP_CONFIGURATION_HELPERS_HH

#include <sdf/sdf_config.h>

#if SDF_MAJOR_VERSION >= 3

#include <sdf/Element.hh>

#else

#include <boost/shared_ptr.hpp>

namespace sdf {
   class Element;
   typedef boost::shared_ptr<Element> ElementPtr;
}

#endif

#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/physics/PhysicsTypes.hh>

namespace yarp {
    namespace os {
        class Property;
    }
}

namespace GazeboYarpPlugins {

/**
 * Split a string in a vector of string given a (single char) delimiter
 */
std::vector<std::string> splitString(const std::string &s, const std::string &delim);

/**
 * Load the configuration for a given model plugin,
 * and save the configuration in the plugin_parameters output object.
 * This involves calling addGazeboEnviromentalVariablesModel and then loading
 * the yarp configuration file specified in the sdf with the yarpConfigurationFile tag.
 */
bool loadConfigModelPlugin(gazebo::physics::ModelPtr _parent,
                           sdf::ElementPtr _sdf,
                           yarp::os::Property & plugin_parameters);

/**
 * Add some Gazebo specific "enviromental variables" that it is possible
 * to expand in .ini configuration file loaded using the ${variable} syntax,
 * for writing configuration files where port names and other parameters
 * depend on Gazebo names.
 *
 * This function add some model related "enviromental variables",
 * copying their values from some ModelPtr methods :
 *
 * |  Yarp parameter name       | ModelPtr method    |
 * |:--------------------------:|:------------------:|
 * | gazeboYarpPluginsRobotName | model->GetName()   |
 *
 * @return true if all went well, false otherwise
 */
bool addGazeboEnviromentalVariablesModel(gazebo::physics::ModelPtr _parent,
                                         sdf::ElementPtr _sdf,
                                         yarp::os::Property & plugin_parameters);

/**
 * Load the configuration for a given sensor plugin,
 * and save the configuration in the plugin_parameters output object.
 * This involves calling addGazeboEnviromentalVariablesSensor and then loading
 * the yarp configuration file specified in the sdf with the yarpConfigurationFile tag.
 */
bool loadConfigSensorPlugin(gazebo::sensors::SensorPtr _sensor,
                            sdf::ElementPtr _sdf,
                            yarp::os::Property & plugin_parameters);

/**
 * Add some Gazebo specific "enviromental variables" that it is possible
 * to expand in .ini configuration file loaded using the ${variable} syntax,
 * for writing configuration files where port names and other parameters
 * depend on Gazebo names.
 *
 * This function add some Sensor related "enviromental variables",
 * copying their values from some SensorPtr methods :
 *
 * |  Yarp parameter name        | SensorPtr method    |
 * |:---------------------------:|:-------------------:|
 * | gazeboYarpPluginsSensorName | sensor->GetName()   |
 * | gazeboYarpPluginsRobotName  | Model name as extracted from sensor->GetScopedName()  |
 *
 * @return true if all went well, false otherwise
 */
bool addGazeboEnviromentalVariablesSensor(gazebo::sensors::SensorPtr _sensor,
                                          sdf::ElementPtr _sdf,
                                          yarp::os::Property & plugin_parameters);

}

#endif  // GAZEBOYARP_CONFIGURATION_HELPERS_HH

ext: hh


text:
// Copyright 2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_fill_profile

#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <cassert>
#include <iostream>
#include <sstream>
#include <tuple>

int main() {
  using namespace boost::histogram;

  // make a profile, it computes the mean of the samples in each histogram cell
  auto h = make_profile(axis::integer<>(0, 3));

  // mean is computed from the values marked with the sample() helper function
  h(0, sample(1)); // sample goes to cell 0
  h(0, sample(2)); // sample goes to cell 0
  h(1, sample(3)); // sample goes to cell 1
  h(sample(4), 1); // sample goes to cell 1; sample can be first or last argument

  // fills from tuples are also supported, 5 and 6 go to cell 2
  auto a = std::make_tuple(2, sample(5));
  auto b = std::make_tuple(sample(6), 2);
  h(a);
  h(b);

  // builtin accumulators have methods to access their state
  std::ostringstream os;
  for (auto&& x : indexed(h)) {
    // use `.` to access methods of accessor, like `index()`
    // use `->` to access methods of accumulator
    const auto i = x.index();
    const auto n = x->count();     // how many samples are in this bin
    const auto vl = x->value();    // mean value
    const auto vr = x->variance(); // estimated variance of the mean value
    os << boost::format("index %i count %i value %.1f variance %.1f\n") % i % n % vl % vr;
  }

  std::cout << os.str() << std::flush;

  assert(os.str() == "index 0 count 2 value 1.5 variance 0.5\n"
                     "index 1 count 2 value 3.5 variance 0.5\n"
                     "index 2 count 2 value 5.5 variance 0.5\n");
}

//]

max_stars_repo_name: cpp-pm/boost

max_issues_repo_name: cpp-pm/boost

max_forks_repo_name: cpp-pm/boost


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

#include "fw/math.h"

#include <boost/test/auto_unit_test.hpp>

using namespace moteus;

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE(log2f_approx_test, * boost::unit_test::tolerance(1e-3f)) {
  const float test_values[] = {
    1.0,
    0.5,
    0.25,
    1.01,
    1.02,
    2.0,
    1.5,
    3.0,
    4.0,
    20.0,
    31.0,
    32.1,
  };
  for (const auto value : test_values) {
    BOOST_TEST_CONTEXT(value) {
      BOOST_TEST(std::abs(std::log2(value) - log2f_approx(value)) < 2e-3);
      // This has a relative tolerance instead of an absolute one.
      BOOST_TEST(pow2f_approx(log2f(value)) == value);
    }
  }

  BOOST_TEST(pow2f_approx(0.0024f) == 1.00166f);
  BOOST_TEST(pow2f_approx(log2f_approx(0.01)) == 0.01,
             boost::test_tools::tolerance(3e-2));
}

BOOST_AUTO_TEST_CASE(WrapZeroToTwoPiTest) {
  for (float v = -50.0f; v <= 50.0f; v += 0.001f) {
    BOOST_TEST_CONTEXT(v) {
      auto result = WrapZeroToTwoPi(v);
      const auto oracle_neg_pos = std::atan2(std::sin(v), std::cos(v));
      const auto corrected =
          (oracle_neg_pos > 0.0f) ?
          oracle_neg_pos :
          (2.0f * M_PI + oracle_neg_pos);
      BOOST_TEST(std::abs(corrected - result) < 0.001f);
    }
  }
}


text:
/**
 *  $Id: I3CORSIKAReader.cxx 179231 2020-03-05 00:53:07Z nwhitehorn $
 *
 *  Copyright (C) 2012
 *  The IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 */

#include <icetray/open.h>
#include <icetray/I3Frame.h>
#include <icetray/I3Module.h>
#include <dataio/I3FileStager.h>

#include <dataclasses/physics/I3MCTreeUtils.h>
#include <dataclasses/physics/I3Particle.h>
#include <dataclasses/I3Double.h>
#include <simclasses/I3CorsikaInfo.h>
#include <simclasses/I3CorsikaShowerInfo.h>
#include <phys-services/I3RandomService.h>

#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/assign.hpp>
#include <boost/python.hpp>
#include <boost/scoped_ptr.hpp>

#include <cmath>

#include "corsika-reader/I3CORSIKAReaderUtils.h"

typedef I3Map<I3ParticleID, double> I3ParticleIDMap;
typedef boost::shared_ptr<I3ParticleIDMap> I3ParticleIDMapPtr;

class I3CORSIKAReader : public I3Module
{
  public:
	I3CORSIKAReader(const I3Context& context);
	void Configure();
	void Process();

  private:
	SET_LOGGER("I3CorsikaReader");

	int FillFrameFromCORSIKA(I3FramePtr frame);
	int ReadNextBlock(float corsika_block[274], char type[5]);
	void ProcessEventHeader(float *corsika_block, I3FramePtr frame,
	    I3MCTreePtr mctree, I3Particle &primary, double core_displace[2]);
	void ProcessParticleBlock(float *corsika_block, I3FramePtr frame,
	    std::vector<I3Particle> &daughters,
	    I3MCTreePtr mctree, I3ParticleIDMapPtr weights,
	    const I3Particle &primary, double core_displace[2]);

        I3CorsikaInfoPtr info_;
	I3RandomServicePtr rng_;
	I3FileStagerPtr file_stager_;
	I3::dataio::shared_filehandle current_filename_;
	boost::iostreams::filtering_istream i3_ifs_;
	boost::iostreams::filtering_istream corsika_ifs_;
	std::vector<std::string> filenames_;
	std::vector<std::string> files_;
	std::vector<std::string>::const_iterator filenames_iter_;
	std::vector<I3Particle::ParticleType> particles_to_write_;
	std::string weightdictname_;
	int nevents_;
	int legacy_oversampling_;
	bool runComplete_;
	bool checkOnly_;
	unsigned lastEventID_;
	unsigned eventID_;
	unsigned run_number_;
	I3MapStringDouble weightMap_;

	double obs_level_;
	bool padded_;
	bool curved_, curved_obs_;
	int sub_blocks_;

	bool thinned_;
	bool ehistory_;

	// Book-keeping for extended history
	I3Particle mother_, grandmother_;
	int ancestors_;

};

I3_MODULE(I3CORSIKAReader);

I3CORSIKAReader::I3CORSIKAReader(const I3Context& context)
  : I3Module(context), info_(new I3CorsikaInfo)
{

	AddParameter("FilenameList", "Paths to CORSIKA DAT files", filenames_);
	AddParameter("Prefix", "Path to I3 file with frames to prefix to "
	    "the stream (e.g. GCD information)", "");
	AddParameter("NEvents", "Number of CORSIKA showers per input file, "
	    "required for older CORSIKA versions that do not write this into "
	    "the run number. If value is wrong, will fail (with a useful "
	    "message) at run time. Negative values disable weight checking. ",
	    0);
	AddParameter("LegacyOverSampling",
	    "(deprecated) number of times to oversample showers ", 1);
	particles_to_write_.push_back(I3Particle::MuMinus);
	particles_to_write_.push_back(I3Particle::MuPlus);
	particles_to_write_.push_back(I3Particle::NuE);
	particles_to_write_.push_back(I3Particle::NuEBar);
	particles_to_write_.push_back(I3Particle::NuMu);
	particles_to_write_.push_back(I3Particle::NuMuBar);
	particles_to_write_.push_back(I3Particle::NuTau);
	particles_to_write_.push_back(I3Particle::NuTauBar);
	AddParameter("ParticlesToWrite", "Types of particle to include in the "
	    "output MC tree. If blank, include all particles present in the "
	    "shower at ground level (this can be very large)",
	    particles_to_write_);
	AddParameter("WeightMapName", "Name of weights map to store in frame",
	    "CorsikaWeightMap");
	AddParameter("CheckIntegrity", "Check that each CORSIKA file is "
	    "complete and contains the expected number of showers, but emit no "
	    "frames.", false);
	AddParameter("Thinned", "Read thinned CORSIKA files", false);
	AddParameter("EHistory", "Read EHISTORY CORSIKA files."
             "For proper rotation of Direction/Position"
             "This Parameter must be passed.", false);

	AddOutBox("OutBox");
}

void I3CORSIKAReader::Configure()
{
	std::string filename;

	file_stager_ = context_.Get<I3FileStagerPtr>();
	if (!file_stager_)
		file_stager_ = I3TrivialFileStager::create();

	GetParameter("LegacyOverSampling", legacy_oversampling_);
	GetParameter("FilenameList", filenames_);

	GetParameter("Prefix", filename);
	if (filename.length() > 0)
		file_stager_->WillReadLater(filename);

	// Oversampling by reading input files multiple times
	for ( int f_i = 0; f_i < legacy_oversampling_ ; f_i++)
	{
	    for ( filenames_iter_ = filenames_.begin(); filenames_iter_ != filenames_.end(); filenames_iter_++)
		files_.push_back(*filenames_iter_);
	}
	filenames_iter_ = files_.begin();
	for ( ; filenames_iter_ != files_.end(); filenames_iter_++)
		file_stager_->WillReadLater(*filenames_iter_);
	filenames_iter_ = files_.begin();
	GetParameter("NEvents", nevents_);
	if (filename.length() > 0) {
		log_info("Prefixing with filename %s", filename.c_str());
		current_filename_ = file_stager_->GetReadablePath(filename);
		I3::dataio::open(i3_ifs_, *current_filename_);
	}
	GetParameter("ParticlesToWrite", particles_to_write_);
	GetParameter("WeightMapName", weightdictname_ );
	GetParameter("CheckIntegrity", checkOnly_);
	GetParameter("Thinned", thinned_);
	GetParameter("EHistory", ehistory_);

	rng_ = context_.Get<I3RandomServicePtr>();
	if (rng_ == NULL)
		log_fatal("No I3RandomService in context!");

	ancestors_ = 0;
	runComplete_ = true;
	lastEventID_ = 0;
	eventID_ = 0;
}

void I3CORSIKAReader::Process()
{
	log_trace("%s: %s", GetName().c_str(), __PRETTY_FUNCTION__);

	I3FramePtr frame(new I3Frame(I3Frame::DAQ));
	if (!i3_ifs_.empty()) {
		// Get frame from prefix file
		frame->load(i3_ifs_);
		if (i3_ifs_.peek() == EOF)
			i3_ifs_.reset();
		PushFrame(frame);
		return;
	}

	if (FillFrameFromCORSIKA(frame) != 0)
		return;

        if (info_){
          //create one S-Frame at the begining of the stream
          log_info_stream("Instering I3CorsikaInfo into S-Frame:\n" << *info_);
          I3FramePtr sframe(new I3Frame(I3Frame::Simulation));
          sframe->Put(info_);
          PushFrame(sframe);

          //delete the info object so we don't write to it again
          info_.reset();
        }

	PushFrame(frame);
}

void
I3CORSIKAReader::ProcessEventHeader(float *corsika_block, I3FramePtr frame,
    I3MCTreePtr mctree, I3Particle &primary, double core_displace[2])
{
	curved_obs_ = (corsika_block[168] != 0);
	curved_ = (corsika_block[79] != 0);

	primary = I3Particle();
	primary.SetShape(I3Particle::Primary);
	primary.SetLocationType(I3Particle::Anywhere);

	primary.SetPdgEncoding(I3CORSIKAReaderUtils::CorsikaToPDG(corsika_block[3]));
	primary.SetShape(I3Particle::Primary);
	I3Direction dir(corsika_block[8], corsika_block[9],
	    -corsika_block[10]);

	if (ehistory_){
		log_debug("EHISTORY: Rotating Primary Direction by 119 deg "
		    "(std ARRANG value).");
		// ARRANG keyword conflicts with EHISTORY option.
		// Hence rotation of Primary is being done here.
		// There is no need to apply rotation matrix on Primary
		// position because it is derived using Primary Dir

		// Rotate by -1*(2 pi - 119 degrees)
		dir.RotateZ(-1.0*(2.0*I3Constants::pi - 2.076942)*I3Units::rad);
	} else {
		dir.RotateZ(-corsika_block[93]*I3Units::rad);
	}

	primary.SetDir(dir);
	primary.SetEnergy(corsika_block[4]*I3Units::GeV);

	log_debug_stream(primary.GetEnergy() << " GeV " << primary.GetTypeString());

	// Time 0 is when the particle intersects the observation level
	double time_to_intersection;
	double detection_altitude;
	double entry_altitude = 0;
	double interaction_height = corsika_block[7]*I3Units::cm;
	double entry_height = corsika_block[158]*I3Units::cm;
	double thetaMin = corsika_block[81]*I3Units::deg;
	double thetaMax = corsika_block[82]*I3Units::deg;
        double min_energy = corsika_block[59]*I3Units::GeV;
        double max_energy = corsika_block[60]*I3Units::GeV;
        double power_law_index = corsika_block[58];
        double n_events = nevents_*legacy_oversampling_;

	if (interaction_height < 0) {
		// TMARGIN option set, times relative to entrance into the
		// atmosphere
		interaction_height = -interaction_height;
	}

        if (info_){
          info_->run_id = run_number_;
          info_->n_events = n_events;
          info_->primary_type=primary.GetType();
          info_->min_zenith = thetaMin;
          info_->max_zenith = thetaMax;
          info_->min_energy = min_energy;
          info_->max_energy = max_energy;
          info_->power_law_index=power_law_index;
        }

	I3CorsikaShowerInfoPtr shower_info(new I3CorsikaShowerInfo());
	shower_info->curvedObs = curved_obs_;
	shower_info->curved = curved_;
	shower_info->crsRunID = run_number_;
	shower_info->crsEventID = eventID_;

	shower_info->entryHeight = entry_height;
	shower_info->firstIntHeight = interaction_height;
	shower_info->obsLevelHeight = obs_level_;
	frame->Put("ShowerInfo", shower_info);

	double zspeed = primary.GetSpeed()*cos(primary.GetZenith() / I3Units::radian);
	double time_to_interaction =
	    (corsika_block[158]*I3Units::cm - interaction_height)/zspeed;
	primary.SetLength(primary.GetSpeed()*time_to_interaction);

	I3MCTreeUtils::AddPrimary(*mctree, primary);

	if (weightMap_.size() == 0) {
		weightMap_["EnergyPrimaryMin"] = corsika_block[59]*I3Units::GeV;
		weightMap_["EnergyPrimaryMax"] = corsika_block[60]*I3Units::GeV;
		weightMap_["PrimarySpectralIndex"] = corsika_block[58];
		weightMap_["NEvents"] = nevents_*legacy_oversampling_;
		// In VOLUMEDET mode, the angular density of injected showers is
		// uniform, but the projected area of the target cylinder is not,
		// so diagonal showers have to be spread out over a larger area than
		// vertical or horizontal showers, effectively reducing the simulated
		// livetime at middling zenith angles. In VOLUMECORR mode, however,
		// the angular density of injected showers is already proportional to
		// the target's projected area, so the average shower sees the same
		// projected area regardless of zenith angle.
	}
	I3MapStringDoublePtr weightdict(new I3MapStringDouble(weightMap_));

	(*weightdict)["ThetaMin"] = thetaMin;
	(*weightdict)["ThetaMax"] = thetaMax;
	(*weightdict)["PrimaryEnergy"] = primary.GetEnergy();
	(*weightdict)["PrimaryType"] = (double)primary.GetType();
	(*weightdict)["OverSampling"] = (double)legacy_oversampling_;
        (*weightdict)["GeneratorID"] = (double)eventID_;

	frame->Put(weightdictname_, weightdict);
}

inline bool
IsEquivalent(double p1, double p2)
{
	return (p1 == p2 || (std::isnan(p1) && std::isnan(p2)));
}

inline bool
IsEquivalent(const I3Position &p1, const I3Position &p2)
{
	return (IsEquivalent(p1.GetX(),p2.GetX()) &&
	    IsEquivalent(p1.GetY(),p2.GetY()) &&
	    IsEquivalent(p1.GetZ(),p2.GetZ()));
}

inline bool
IsEquivalent(const I3Particle &p1, const I3Particle &p2)
{
	return (p1.GetID() == p2.GetID()) ||
	   (p1.GetType() == p2.GetType() &&
	    p1.GetEnergy() == p2.GetEnergy() &&
	    p1.GetDir() == p2.GetDir() &&
	    IsEquivalent(p1.GetPos(), p2.GetPos())
	);
}

I3MCTree::iterator
EnsureNode(I3MCTreePtr mctree, I3MCTree::iterator root, I3Particle &child)
{
	// Check if a particle is a daughter. If not, add it.
	// Use IsEquivalent for testing particle equality because CORSIKA
	if (IsEquivalent(*root, child))
		return root;
	I3MCTree::sibling_iterator i = mctree->end_sibling(), p_iter = mctree->end_sibling();
	for (i = mctree->children(root); i != mctree->end_sibling(); i++)
		if (IsEquivalent(*i, child)) {
			p_iter = i;
			break;
		}
	if (p_iter != mctree->end_sibling())
		return p_iter;
	else
		return mctree->append_child(root, child);
}

void
I3CORSIKAReader::ProcessParticleBlock(float *corsika_block, I3FramePtr frame,
    std::vector<I3Particle> &daughters,
    I3MCTreePtr mctree, I3ParticleIDMapPtr weights, const I3Particle &primary,
    double core_displace[2])
{
	for (int i = 0; i < 39; i++) {
		int start = (7+thinned_)*i;
		int id = corsika_block[start + 1]/1000;
		if (id == 0)
			break;

		// Skip extra muon info
		if (id == 75 || id == 76)
			continue;

		// Extended-history entries have negative particle IDs
		// http://digbib.ubka.uni-karlsruhe.de/volltexte/documents/2545953
		if (id < 0)
			ancestors_++;

		I3Particle part;
		if (id > 0) {
			part.SetShape(I3Particle::StartingTrack);
			part.SetPdgEncoding(I3CORSIKAReaderUtils::CorsikaToPDG(id));
			part.SetLocationType(I3Particle::InIce);
		} else {
			part.SetShape(I3Particle::Null);
			part.SetPdgEncoding(I3CORSIKAReaderUtils::CorsikaToPDG(-id));
			part.SetLocationType(I3Particle::Anywhere);
		}

		I3Direction dir(corsika_block[start + 2],
		    corsika_block[start + 3],
		    -corsika_block[start + 4]);

		if (ehistory_){
			// ARRANG keyword conflicts with EHISTORY option.
			// Hence rotation of Primary is being done in the
			// corsika-reader.
			dir.RotateZ(-1.0*(2.0*I3Constants::pi - 2.076942)*
			    I3Units::rad);
		}

		part.SetDir(dir);

		I3Position temp_pos(corsika_block[start+5]*I3Units::cm,
		corsika_block[start+6]*I3Units::cm, obs_level_);
		if (ehistory_) {
			// ARRANG keyword conflicts with EHISTORY option.
			// Hence rotation of Primary is being done by hand here.
			temp_pos.RotateZ(-1.0*(2.0*I3Constants::pi - 2.076942)*
			    I3Units::rad);
		}

		if (curved_obs_) {
			double theta, phi, D;
			// Normal particles are recorded at the observation
			// level, extended-history entries at production level
			D = I3CORSIKAReaderUtils::EarthRadius + I3Constants::OriginElev +
			    (id > 0 ?
			    obs_level_ : corsika_block[start+7]*I3Units::cm);
			theta = hypot(
			  temp_pos.GetX() + core_displace[0],
			  temp_pos.GetY() + core_displace[1]
			  )/D;
			phi = atan2(temp_pos.GetY() + core_displace[1],
			  temp_pos.GetX() +core_displace[0]);
			// Only the mother particle has a position; the
			// grandmother only has a column depth.
			if (id > 0 || ancestors_ == 1)
				part.SetPos(
				    D*sin(theta)*cos(phi),
				    D*sin(theta)*sin(phi),
				    D*cos(theta) - I3CORSIKAReaderUtils::EarthRadius -
				    I3Constants::OriginElev
				);
		} else if (id > 0 || ancestors_ == 1) {
			part.SetPos(temp_pos.GetX() + core_displace[0],
			  temp_pos.GetY() + core_displace[1],
			  obs_level_);
		}
		// Normal entries have an explicit time, but extended-history
		// entries do not. Assume speed-of-light propagation from the
		// primary vertex.
		if (id > 0)
			part.SetTime(corsika_block[start + 7]*I3Units::ns);
		else
			part.SetTime(
			    (part.GetPos()-primary.GetPos()).Magnitude()/
			    I3Constants::c);

		// Calculate energy assuming E=cp
		part.SetEnergy(I3Units::GeV*sqrt(
		    pow(corsika_block[start + 2], 2) +
		    pow(corsika_block[start + 3], 2) +
		    pow(corsika_block[start + 4], 2)));

		if (id < 0) {
			// De-duplicate references to the primary that ignore
			// the rest mass in the energy calculation
			if (std::abs(part.GetEnergy()-primary.GetEnergy()) < 1)
				part = primary;
			if (ancestors_ == 1)
				mother_ = part;
			else if (ancestors_ == 2)
				grandmother_ = part;
			continue;
		}

		// Check against list
		if (particles_to_write_.size() > 0 &&
		    std::find(particles_to_write_.begin(),
		    particles_to_write_.end(), part.GetType()) ==
		    particles_to_write_.end()) {
			ancestors_ = 0;
			continue;
		}

		// Record thinning weight if it exists and is non-trivial
		if (thinned_ && (corsika_block[start + 8] != 1.f))
			(*weights)[part.GetID()] = corsika_block[start + 8];

		// No extended history: defer tree insertion for speed.
		if (ancestors_ == 0) {
			daughters.push_back(part);
		} else if (ancestors_ == 2) {
			// Write extended history if we have it

			I3MCTree::iterator node = mctree->begin();
			node = EnsureNode(mctree, node, grandmother_);
			node = EnsureNode(mctree, node, mother_);
			mctree->append_child(node, part);
		} else {
			log_fatal_stream("This particle claims to have " <<
			    ancestors_ << " ancestors. This can't be.");
		}

		ancestors_ = 0;
	}
}

int
I3CORSIKAReader::FillFrameFromCORSIKA(I3FramePtr frame)
{
	BOOST_STATIC_ASSERT(sizeof(float) == 4);
	float corsika_block[313];
	char type[5] = "";

	// NB: In the normal mode of operation, CORSIKA gives us just the particles
	// at ground level. Since the cost of each call to I3MCTree::append_child()
	// is proportional to the number of particles already in the tree, it is
	// much faster (500x for 100k particles) to keep an intermediate vector of
	// ground-level particles and attach them all to the tree at once with
	// I3MCTree::append_children(). In extended-history mode each ground-level
	// particle record includes its parent and grandparent particles. Since
	// a parent may have multiple daughters the entries must be de-duplicated
	// as we go anyhow, and we fall back to inserting the entries one by one.
	I3MCTreePtr mctree(new I3MCTree);
	std::vector<I3Particle> daughters;
	I3ParticleIDMapPtr weights(new I3ParticleIDMap);
	I3Particle primary;
	double core_displacement[2];

	while (1) {
		if (ReadNextBlock(corsika_block, type) != 0)
			return 1;

		if (strcmp(type, "RUNH") == 0) {
			runComplete_ = false;
			run_number_ = corsika_block[2];
			if (corsika_block[93] > 0)
				nevents_ = corsika_block[93];
			if (corsika_block[5] > 1)
				log_fatal("Only one observation level "
				    "supported. This file has %d.\n",
				    (int)corsika_block[5]);
			obs_level_ = corsika_block[6]*I3Units::cm -
			    I3Constants::OriginElev;
			weightMap_.clear();
		} else if (strcmp(type, "RUNE") == 0) {
			runComplete_ = true;
			if (nevents_ >= 0 && corsika_block[3] != nevents_)
				log_fatal("Number of events in this file (%d) "
				    "does not match configured number (%d). As "
				    "a result, all weight entries written to "
				    "disk were lies. Please correct and try "
				    "again.", int(corsika_block[3]), nevents_);
		} else if (checkOnly_)  {
			if (strcmp(type, "EVTH") == 0)
				lastEventID_ = corsika_block[2];
			return 1;
		} else if (strcmp(type, "EVTH") == 0) {
			eventID_ = corsika_block[2];
			ProcessEventHeader(corsika_block, frame, mctree,
			    primary, core_displacement);
		} else if (strcmp(type, "EVTE") == 0) {
			if (weights->size() > 0)
				frame->Put("CorsikaParticleWeights", weights);
			// Add a block of ground-level particles if we have it
			mctree->append_children(primary, daughters);
			frame->Put("I3MCTree", mctree);
			return 0;
		} else if (strcmp(type, "LONG") == 0) {
			continue;
		} else if (strcmp(type, "RUNE") == 0) {
			runComplete_ = true;
			if (corsika_block[3] != nevents_)
				log_fatal("Number of events in this file (%d) "
				    "does not match configured number (%d). As "
				    "a result, all weight entries written to "
				    "disk were lies. Please correct and try "
				    "again.", int(corsika_block[3]), nevents_);
			return 1;
		} else {
			ProcessParticleBlock(corsika_block, frame, daughters, mctree,
			    weights, primary, core_displacement);
		}
	}
}

int
I3CORSIKAReader::ReadNextBlock(float corsika_block[313], char type[5])
{
	unsigned offset = 0;
	if (corsika_ifs_.peek() == EOF) {
		if (!runComplete_ && filenames_iter_ != files_.begin())
			log_fatal("%s is truncated (event %u did not end)!",
			    (filenames_iter_-1)->c_str(), lastEventID_);
		if (filenames_iter_ == files_.end()) {
			RequestSuspension();
			return EOF;
		}
		if (!corsika_ifs_.empty())
			corsika_ifs_.pop();
		corsika_ifs_.reset();
		assert(corsika_ifs_.empty());

		current_filename_ = file_stager_->GetReadablePath(*filenames_iter_);
		filenames_iter_++;
		I3::dataio::open(corsika_ifs_, *current_filename_);
		// in an unpadded file, the first 4 bytes are "RUNH"
		corsika_ifs_.read((char *)&corsika_block[1], 4);
		padded_ = (memcmp((char *)&corsika_block[1], "RUNH", 4) != 0);
		// we already have the first 4 bytes of the block
		if (!padded_)
			offset = 1;
		sub_blocks_ = 0;

		log_info_stream("Opened "<<(thinned_ ? "thinned " : "")<<"CORSIKA file "<<(*current_filename_));
	}

	// NOTE: CORSIKA files have no defined endianness, which is delightful,
	// so just assume that the endianness of this system matches the
	// endianness of the system that produced the file.
	corsika_ifs_.read((char *)(&corsika_block[1+offset]),
	    ((thinned_ ? 312 : 273)-offset)*sizeof(float));
	memcpy(type, &corsika_block[1], 4);
	type[4] = 0;
	if (++sub_blocks_ % 21 == 0 && padded_) {
		char junk[8];
		corsika_ifs_.read((char *)&junk, 8);
	}

	return 0;
}

ext: cxx