<commit_before>//
//  AliEveHLTZMQeventManager
//
//  blame: Mikolaj Krzewicki, mikolaj.krzewicki@cern.ch
//
//

#include "AliEveHLTZMQeventManager.h"

#include "AliEveConfigManager.h"
#include "AliCDBManager.h"
#include "AliGRPPreprocessor.h"
#include <TEnv.h>
#include <TInterpreter.h>
#include <iostream>

#include "AliHLTComponent.h"
#include "AliHLTMessage.h"

#include "AliOnlineReconstructionUtil.h"

#ifdef ZMQ
#include "zmq.h"
#endif

using namespace std;

AliEveHLTZMQeventManager::AliEveHLTZMQeventManager(Int_t ev, bool storageManager) :
    AliEveEventManager("HLT",ev,false),
    fEventListenerThreadHLT(0),
    fCurrentRun(-1),
    fZMQContext(NULL),
    fZMQeventQueue(NULL),
    fHLTPublisherAddress("tcp://localhost:60201")
{
#ifdef ZMQ
  fIsOpen=kTRUE;
  //get the address of the HLT proxy from the environment
  if (gSystem->Getenv("HLT_ZMQ_proxy"))
    fHLTPublisherAddress=gSystem->Getenv("HLT_ZMQ_proxy");
  //single ZMQ context for inter thread comm. etc.
  fZMQContext = zmq_ctx_new();
  //single ZMQ socket for gathering the events form various listening threads
  //must be bound before threads can connect
  fZMQeventQueue = zmq_socket(fZMQContext, ZMQ_PULL);
  zmq_bind(fZMQeventQueue, "inproc://fCurrentEvent");

  cout<<"ZMQ FOUND. Starting subscriber threads."<<endl;
  fEventListenerThreadHLT = new TThread("fEventListenerThreadHLT",DispatchEventListenerHLT,(void*)this);
  fEventListenerThreadHLT->Run();
#endif

  AliEveEventManager::SetMaster(this);
}

AliEveHLTZMQeventManager::~AliEveHLTZMQeventManager()
{
#ifdef ZMQ
  if (fZMQeventQueue)
  {
    int lingerValue = 0;
    int rc = zmq_setsockopt(fZMQeventQueue, ZMQ_LINGER, &lingerValue, sizeof(int));
    if (rc<0) printf("error setting linger on fZMQeventQueue\n");
    rc = zmq_close(fZMQeventQueue);
    if (rc<0) printf("error closing socket fZMQeventQueue\n");
  }

  printf("trying to destroy fZMQContext\n");
  if (fZMQContext) zmq_ctx_destroy(fZMQContext);
  printf("destroyed fZMQContext\n");
#endif

  if(fEventListenerThreadHLT)
  {
    fEventListenerThreadHLT->Join();
    fEventListenerThreadHLT->Kill();
    delete fEventListenerThreadHLT;
    cout<<"HLT listener thread killed and deleted"<<endl;
  }

}

void AliEveHLTZMQeventManager::PullEventFromHLT()
{
#ifdef ZMQ
  int rc = 0;

  if (!fZMQContext) return;

  //connection to the HLT data
  void* listenerSocket = zmq_socket(fZMQContext, ZMQ_SUB);
  if (!listenerSocket) {return;}
  printf("connecting to ZMQ socket: %s\n", fHLTPublisherAddress.Data());
  rc = zmq_connect(listenerSocket, fHLTPublisherAddress.Data());
  rc = zmq_setsockopt (listenerSocket, ZMQ_SUBSCRIBE, NULL, 0);
  if (rc < 0) {return;}

  //internal publisher
  void* internalPublisher = zmq_socket(fZMQContext, ZMQ_PUSH);
  if (!internalPublisher) {return;}
  rc = zmq_connect(internalPublisher, "inproc://fCurrentEvent");
  if (rc < 0) {return;}

  //create a default selection of any data:
  char requestedTopic[kAliHLTComponentDataTypeTopicSize+1]; memset(&requestedTopic, 0, sizeof(requestedTopic));
  memset(&requestedTopic, '*', kAliHLTComponentDataTypeTopicSize);
  strncpy(requestedTopic, "ALIESDV0HLT**************", kAliHLTComponentDataTypeTopicSize);
  char requestedTopic1[kAliHLTComponentDataTypeTopicSize+1]; memset(&requestedTopic1, 0, sizeof(requestedTopic1));
  memset(&requestedTopic1, '*', kAliHLTComponentDataTypeTopicSize);
  strncpy(requestedTopic1, "FLATESD**************", kAliHLTComponentDataTypeTopicSize);

  Bool_t done=false;
  while (!done)
  {
    int more = 0;
    size_t more_size = sizeof more;
    do
    {
      //here the HLT data comes in
      //receive the topic
      char receivedTopic[kAliHLTComponentDataTypeTopicSize+1]; memset(&receivedTopic, 0, sizeof(receivedTopic));
      int receivedTopicSize = zmq_recv(listenerSocket, &receivedTopic, sizeof(receivedTopic), 0);
      if (receivedTopicSize < 0 && errno==ETERM) {printf("listenerSocket received ETERM, exiting main loop\n"); done=true; break;}

      //reive the message if there is any
      zmq_msg_t message;
      rc = zmq_msg_init(&message);
      rc = zmq_getsockopt(listenerSocket, ZMQ_RCVMORE, &more, &more_size);
      if (more)
      {
        rc = zmq_msg_recv(&message, listenerSocket, 0);
        if (rc < 0 && errno==ETERM) { zmq_msg_close(&message); done=true; break; }
      }
      rc = zmq_getsockopt(listenerSocket, ZMQ_RCVMORE, &more, &more_size);

      //after receiving a single part (topic+message) publish it internally
      //in general this will be an HLT ESD.
      //do check first if this is what we wanted
      bool isSelected = Topicncmp(requestedTopic, receivedTopic, kAliHLTComponentDataTypeTopicSize, receivedTopicSize) ||
                        Topicncmp(requestedTopic1, receivedTopic, kAliHLTComponentDataTypeTopicSize, receivedTopicSize);

      if (!isSelected)
      {
        //clean up the memory and continue
        //printf("  dropped %s\n",receivedTopic);
        zmq_msg_close(&message);
        continue;
      }

      //publish the data for further use
      rc = -1;
      //printf("pushing %s\n", receivedTopic);
      rc = zmq_send(internalPublisher, &receivedTopic, receivedTopicSize, ZMQ_SNDMORE);
      rc = zmq_msg_send(&message, internalPublisher, 0);
      if (rc < 0) zmq_msg_close(&message);

    } while (more != 0);
  }
  //cleanly exit the thread
  int lingerValue = 10;
  rc = zmq_setsockopt(listenerSocket, ZMQ_LINGER, &lingerValue, sizeof(lingerValue));
  if (rc<0) printf("error setting linger on listenerSocket, errno: %i\n", errno);
  rc = zmq_setsockopt(internalPublisher, ZMQ_LINGER, &lingerValue, sizeof(lingerValue));
  if (rc<0) printf("error setting linger on internalPublisher, errno: %i\n",errno);
  rc = zmq_close(listenerSocket);
  if (rc<0) printf("error closing listenerSocket, errno: %i\n", errno);
  rc = zmq_close(internalPublisher);
  if (rc<0) printf("error closing internalPublisher, errno: %i\n", errno);
#endif
}

void AliEveHLTZMQeventManager::InitOCDB(int runNo)
{
    AliCDBManager* cdb = AliCDBManager::Instance();
    if (runNo!=fCurrentRun){
      fCurrentRun=runNo;
      cdb->SetRun(runNo);
    }
}

void AliEveHLTZMQeventManager::GotoEvent(Int_t /*event*/)
{
    static const TEveException kEH("AliEveEventManager::GotoEvent ");

    if (fAutoLoadTimerRunning){throw (kEH + "Event auto-load timer is running.");}

    NextEvent();
    gEve->Redraw3D(kFALSE, kTRUE); // Enforce drop of all logicals.

    return;

}

void AliEveHLTZMQeventManager::NextEvent()
{
    static const TEveException kEH("AliEveEventManager::NextEvent ");
  //read event from queue
  if (fAutoLoadTimerRunning){throw (kEH + "Event auto-load timer is running.");}

#ifdef ZMQ
  //init some stuff
  int rc = 0;
  AliESDEvent* esdObject = NULL;
  char topic[kAliHLTComponentDataTypeTopicSize+1]; memset(topic, 0, sizeof(topic));
  memset(topic, 0, kAliHLTComponentDataTypeTopicSize);
  zmq_msg_t message;
  rc = zmq_msg_init(&message);
  int64_t more = 0;
  size_t more_size = sizeof(more);

  //try to receive a new topic+message from the queue
  int topicSize = zmq_recv(fZMQeventQueue, &topic, kAliHLTComponentDataTypeTopicSize, ZMQ_DONTWAIT);
  int errnoTopic = errno;
  rc = zmq_getsockopt(fZMQeventQueue, ZMQ_RCVMORE, &more, &more_size);
  if (more) rc = zmq_msg_recv(&message, fZMQeventQueue, ZMQ_DONTWAIT);

  //decode the message into an AliESDEvent,
  //for HLT messages AliHLTMessage needs to be used
  if( topicSize > 0 /*&& errnoTopic != EAGAIN*/ && zmq_msg_size(&message) > 0 )
  {
    if (Topicncmp(topic, "ALIESDV0HLT", topicSize, 11))
    {
      printf("trying to decode %s with AliHLTMessage\n", topic);
      AliHLTMessage msg(zmq_msg_data(&message), zmq_msg_size(&message));
      TClass* objclass = msg.GetClass();
      esdObject = static_cast<AliESDEvent*>(msg.ReadObject(AliESDEvent::Class()));
    }
    else if (Topicncmp(topic, "ALIESDV0POR", topicSize, 11))
    {
      printf("trying to decode %s with TBufferFile\n", topic);
      TBufferFile *mess = new TBufferFile(TBuffer::kRead,
          zmq_msg_size(&message)+sizeof(UInt_t),
          zmq_msg_data(&message));
      mess->InitMap();
      mess->ReadClass();// get first the class stored in message
      mess->SetBufferOffset(sizeof(UInt_t) + sizeof(kMESS_OBJECT));
      mess->ResetMap();
      esdObject = (AliESDEvent*)(mess->ReadObjectAny(AliESDEvent::Class()));
    }
    else
    {
      printf("Don't know what to do with %s\n",topic);
    }
  }
  if (esdObject)
  {
    printf("setting new event\n");
    esdObject->GetStdContent();
    DestroyElements();
    int runNumber = esdObject->GetRunNumber();
    InitOCDB(runNumber);

    //replace the ESD
    delete fESD;
    fESD = esdObject;
    fHasEvent=kTRUE;

    AfterNewEventLoaded();
  }
  else
  {
    cout<<"No new event is avaliable."<<endl;
    fHasEvent=kFALSE;
    NoEventLoaded();
  }
  zmq_msg_close(&message);
#endif
  gSystem->ProcessEvents();
}

<commit_msg>cleanup includes<commit_after>//
//  AliEveHLTZMQeventManager
//
//  blame: Mikolaj Krzewicki, mikolaj.krzewicki@cern.ch
//
//

#include "AliEveHLTZMQeventManager.h"

#include "AliEveConfigManager.h"
#include "AliCDBManager.h"
#include "AliGRPPreprocessor.h"
#include <TEnv.h>
#include <TInterpreter.h>
#include <iostream>

#include "AliHLTComponent.h"
#include "AliHLTMessage.h"

#ifdef ZMQ
#include "zmq.h"
#endif

using namespace std;

AliEveHLTZMQeventManager::AliEveHLTZMQeventManager(Int_t ev, bool storageManager) :
    AliEveEventManager("HLT",ev,false),
    fEventListenerThreadHLT(0),
    fCurrentRun(-1),
    fZMQContext(NULL),
    fZMQeventQueue(NULL),
    fHLTPublisherAddress("tcp://localhost:60201")
{
#ifdef ZMQ
  fIsOpen=kTRUE;
  //get the address of the HLT proxy from the environment
  if (gSystem->Getenv("HLT_ZMQ_proxy"))
    fHLTPublisherAddress=gSystem->Getenv("HLT_ZMQ_proxy");
  //single ZMQ context for inter thread comm. etc.
  fZMQContext = zmq_ctx_new();
  //single ZMQ socket for gathering the events form various listening threads
  //must be bound before threads can connect
  fZMQeventQueue = zmq_socket(fZMQContext, ZMQ_PULL);
  zmq_bind(fZMQeventQueue, "inproc://fCurrentEvent");

  cout<<"ZMQ FOUND. Starting subscriber threads."<<endl;
  fEventListenerThreadHLT = new TThread("fEventListenerThreadHLT",DispatchEventListenerHLT,(void*)this);
  fEventListenerThreadHLT->Run();
#endif

  AliEveEventManager::SetMaster(this);
}

AliEveHLTZMQeventManager::~AliEveHLTZMQeventManager()
{
#ifdef ZMQ
  if (fZMQeventQueue)
  {
    int lingerValue = 0;
    int rc = zmq_setsockopt(fZMQeventQueue, ZMQ_LINGER, &lingerValue, sizeof(int));
    if (rc<0) printf("error setting linger on fZMQeventQueue\n");
    rc = zmq_close(fZMQeventQueue);
    if (rc<0) printf("error closing socket fZMQeventQueue\n");
  }

  printf("trying to destroy fZMQContext\n");
  if (fZMQContext) zmq_ctx_destroy(fZMQContext);
  printf("destroyed fZMQContext\n");
#endif

  if(fEventListenerThreadHLT)
  {
    fEventListenerThreadHLT->Join();
    fEventListenerThreadHLT->Kill();
    delete fEventListenerThreadHLT;
    cout<<"HLT listener thread killed and deleted"<<endl;
  }

}

void AliEveHLTZMQeventManager::PullEventFromHLT()
{
#ifdef ZMQ
  int rc = 0;

  if (!fZMQContext) return;

  //connection to the HLT data
  void* listenerSocket = zmq_socket(fZMQContext, ZMQ_SUB);
  if (!listenerSocket) {return;}
  printf("connecting to ZMQ socket: %s\n", fHLTPublisherAddress.Data());
  rc = zmq_connect(listenerSocket, fHLTPublisherAddress.Data());
  rc = zmq_setsockopt (listenerSocket, ZMQ_SUBSCRIBE, NULL, 0);
  if (rc < 0) {return;}

  //internal publisher
  void* internalPublisher = zmq_socket(fZMQContext, ZMQ_PUSH);
  if (!internalPublisher) {return;}
  rc = zmq_connect(internalPublisher, "inproc://fCurrentEvent");
  if (rc < 0) {return;}

  //create a default selection of any data:
  char requestedTopic[kAliHLTComponentDataTypeTopicSize+1]; memset(&requestedTopic, 0, sizeof(requestedTopic));
  memset(&requestedTopic, '*', kAliHLTComponentDataTypeTopicSize);
  strncpy(requestedTopic, "ALIESDV0HLT**************", kAliHLTComponentDataTypeTopicSize);
  char requestedTopic1[kAliHLTComponentDataTypeTopicSize+1]; memset(&requestedTopic1, 0, sizeof(requestedTopic1));
  memset(&requestedTopic1, '*', kAliHLTComponentDataTypeTopicSize);
  strncpy(requestedTopic1, "FLATESD**************", kAliHLTComponentDataTypeTopicSize);

  Bool_t done=false;
  while (!done)
  {
    int more = 0;
    size_t more_size = sizeof more;
    do
    {
      //here the HLT data comes in
      //receive the topic
      char receivedTopic[kAliHLTComponentDataTypeTopicSize+1]; memset(&receivedTopic, 0, sizeof(receivedTopic));
      int receivedTopicSize = zmq_recv(listenerSocket, &receivedTopic, sizeof(receivedTopic), 0);
      if (receivedTopicSize < 0 && errno==ETERM) {printf("listenerSocket received ETERM, exiting main loop\n"); done=true; break;}

      //reive the message if there is any
      zmq_msg_t message;
      rc = zmq_msg_init(&message);
      rc = zmq_getsockopt(listenerSocket, ZMQ_RCVMORE, &more, &more_size);
      if (more)
      {
        rc = zmq_msg_recv(&message, listenerSocket, 0);
        if (rc < 0 && errno==ETERM) { zmq_msg_close(&message); done=true; break; }
      }
      rc = zmq_getsockopt(listenerSocket, ZMQ_RCVMORE, &more, &more_size);

      //after receiving a single part (topic+message) publish it internally
      //in general this will be an HLT ESD.
      //do check first if this is what we wanted
      bool isSelected = Topicncmp(requestedTopic, receivedTopic, kAliHLTComponentDataTypeTopicSize, receivedTopicSize) ||
                        Topicncmp(requestedTopic1, receivedTopic, kAliHLTComponentDataTypeTopicSize, receivedTopicSize);

      if (!isSelected)
      {
        //clean up the memory and continue
        //printf("  dropped %s\n",receivedTopic);
        zmq_msg_close(&message);
        continue;
      }

      //publish the data for further use
      rc = -1;
      //printf("pushing %s\n", receivedTopic);
      rc = zmq_send(internalPublisher, &receivedTopic, receivedTopicSize, ZMQ_SNDMORE);
      rc = zmq_msg_send(&message, internalPublisher, 0);
      if (rc < 0) zmq_msg_close(&message);

    } while (more != 0);
  }
  //cleanly exit the thread
  int lingerValue = 10;
  rc = zmq_setsockopt(listenerSocket, ZMQ_LINGER, &lingerValue, sizeof(lingerValue));
  if (rc<0) printf("error setting linger on listenerSocket, errno: %i\n", errno);
  rc = zmq_setsockopt(internalPublisher, ZMQ_LINGER, &lingerValue, sizeof(lingerValue));
  if (rc<0) printf("error setting linger on internalPublisher, errno: %i\n",errno);
  rc = zmq_close(listenerSocket);
  if (rc<0) printf("error closing listenerSocket, errno: %i\n", errno);
  rc = zmq_close(internalPublisher);
  if (rc<0) printf("error closing internalPublisher, errno: %i\n", errno);
#endif
}

void AliEveHLTZMQeventManager::InitOCDB(int runNo)
{
    AliCDBManager* cdb = AliCDBManager::Instance();
    if (runNo!=fCurrentRun){
      fCurrentRun=runNo;
      cdb->SetRun(runNo);
    }
}

void AliEveHLTZMQeventManager::GotoEvent(Int_t /*event*/)
{
    static const TEveException kEH("AliEveEventManager::GotoEvent ");

    if (fAutoLoadTimerRunning){throw (kEH + "Event auto-load timer is running.");}

    NextEvent();
    gEve->Redraw3D(kFALSE, kTRUE); // Enforce drop of all logicals.

    return;

}

void AliEveHLTZMQeventManager::NextEvent()
{
    static const TEveException kEH("AliEveEventManager::NextEvent ");
  //read event from queue
  if (fAutoLoadTimerRunning){throw (kEH + "Event auto-load timer is running.");}

#ifdef ZMQ
  //init some stuff
  int rc = 0;
  AliESDEvent* esdObject = NULL;
  char topic[kAliHLTComponentDataTypeTopicSize+1]; memset(topic, 0, sizeof(topic));
  memset(topic, 0, kAliHLTComponentDataTypeTopicSize);
  zmq_msg_t message;
  rc = zmq_msg_init(&message);
  int64_t more = 0;
  size_t more_size = sizeof(more);

  //try to receive a new topic+message from the queue
  int topicSize = zmq_recv(fZMQeventQueue, &topic, kAliHLTComponentDataTypeTopicSize, ZMQ_DONTWAIT);
  int errnoTopic = errno;
  rc = zmq_getsockopt(fZMQeventQueue, ZMQ_RCVMORE, &more, &more_size);
  if (more) rc = zmq_msg_recv(&message, fZMQeventQueue, ZMQ_DONTWAIT);

  //decode the message into an AliESDEvent,
  //for HLT messages AliHLTMessage needs to be used
  if( topicSize > 0 /*&& errnoTopic != EAGAIN*/ && zmq_msg_size(&message) > 0 )
  {
    if (Topicncmp(topic, "ALIESDV0HLT", topicSize, 11))
    {
      printf("trying to decode %s with AliHLTMessage\n", topic);
      AliHLTMessage msg(zmq_msg_data(&message), zmq_msg_size(&message));
      TClass* objclass = msg.GetClass();
      esdObject = static_cast<AliESDEvent*>(msg.ReadObject(AliESDEvent::Class()));
    }
    else if (Topicncmp(topic, "ALIESDV0POR", topicSize, 11))
    {
      printf("trying to decode %s with TBufferFile\n", topic);
      TBufferFile *mess = new TBufferFile(TBuffer::kRead,
          zmq_msg_size(&message)+sizeof(UInt_t),
          zmq_msg_data(&message));
      mess->InitMap();
      mess->ReadClass();// get first the class stored in message
      mess->SetBufferOffset(sizeof(UInt_t) + sizeof(kMESS_OBJECT));
      mess->ResetMap();
      esdObject = (AliESDEvent*)(mess->ReadObjectAny(AliESDEvent::Class()));
    }
    else
    {
      printf("Don't know what to do with %s\n",topic);
    }
  }
  if (esdObject)
  {
    printf("setting new event\n");
    esdObject->GetStdContent();
    DestroyElements();
    int runNumber = esdObject->GetRunNumber();
    InitOCDB(runNumber);

    //replace the ESD
    delete fESD;
    fESD = esdObject;
    fHasEvent=kTRUE;

    AfterNewEventLoaded();
  }
  else
  {
    cout<<"No new event is avaliable."<<endl;
    fHasEvent=kFALSE;
    NoEventLoaded();
  }
  zmq_msg_close(&message);
#endif
  gSystem->ProcessEvents();
}

<|endoftext|>


<commit_before>/* RoboticsLab, Copyright 2008-2010 SimLab Co., Ltd. All rights reserved.
*
* This library is commercial and cannot be redistributed, and/or modified
* WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
*/
#include "hybrid_automaton/include/hybrid_automaton_manager/AbstractHybridAutomatonManager.h"
#include "hybrid_automaton/include/hybrid_automaton_manager/AbstractHybridAutomatonManagerCmd.h"

#include "ros_bridge\include\msgs\String.h"
#include "ros_bridge\include\msgs\Transform.h"

#include <process.h>

unsigned __stdcall deserializeHybridAutomaton(void *udata)
{
	DeserializingThreadArguments* thread_args = static_cast<DeserializingThreadArguments*>(udata);
	HybridAutomaton* ha = NULL;
	try {
		ha = XMLDeserializer::createHybridAutomaton(thread_args->_string, thread_args->_sys, thread_args->_dT);
		//std::cout << "--------------- recieving hybrid automaton ------------------" << std::endl;
		//std::cout << ha->toStringXML() << std::endl;
	}
	catch(std::string e)
	{
		std::cerr << "[deserializeHybridAutomaton] ERROR while deserializing hybrid automaton!" << std::endl;
		std::cerr << e << std::endl;

		delete ha;
		delete thread_args;
		return 0;
	}

	WaitForSingleObject(*(thread_args->_deserialize_mutex), INFINITE);
	if(thread_args->_noQueue)
	{
		for(size_t i=0;i<thread_args->_deserialized_hybrid_automatons->size();i++)
		{
			delete thread_args->_deserialized_hybrid_automatons->at(i);
		}
		thread_args->_deserialized_hybrid_automatons->clear();
	}

	thread_args->_deserialized_hybrid_automatons->push_back(ha);
	ReleaseMutex(*(thread_args->_deserialize_mutex));

	delete thread_args;

	return 0;
}

AbstractHybridAutomatonManager::AbstractHybridAutomatonManager(rDC rdc)
:rControlAlgorithm(rdc)
, _sys(NULL)
, _blackboard(NULL)
, _activeMotionBehaviour(NULL)
, _dof(0)
, _servo_on(false)
, _hybrid_automaton(NULL)
, _qdot_filter(NULL)
, _noQueue(false)
{
	_deserialize_mutex = CreateMutex(0, FALSE, 0);
	if( !_deserialize_mutex ) {
		throw(std::string("[AbstractHybridAutomatonManager::HybridAutomatonManager] ERROR: Mutex was not created (_deserialize_mutex)!"));
	}
}

AbstractHybridAutomatonManager::~AbstractHybridAutomatonManager()
{
	if(_activeMotionBehaviour)
		delete _activeMotionBehaviour;

	if(_blackboard)
	{
		delete _blackboard;
		_blackboard = NULL;
	}

	if(_qdot_filter)
	{
		delete _qdot_filter;
	}

	FREE_SYSTEMS();
}

void AbstractHybridAutomatonManager::init(int mode)
{
	_sys = LOAD_SYSTEM(_path, _aml, _T0, _q0);
	assert(_sys);

	//Find the robot device. This device is used to read position and write torques
	//to the robot
	_robot = findDevice(_T("ROBOT"));
	RASSERT(_robot != INVALID_RHANDLE);

	rxDevice* ft_sensor = (rxDevice*)findDevice(_T("FT_SENSOR"));
	if (ft_sensor)
		_sys->addDevice(ft_sensor);

	_dof = _sys->jointDOF() + _sys->earthDOF() + _sys->constraintDOF();

	//Initialize variables
	_q.resize(_dof);
	_qOld.resize(_dof);
	_qdot.resize(_dof);
	_torque.resize(_dof);

	_q.zero();
	_qOld.zero();
	_torque.zero();
	_qdot.zero();

	_qdot_filter = new rxFilteredDerivative(this->_dT, 10.0*this->_dT, this->_sys->jdof());

	_activeMotionBehaviour = new MotionBehaviour(new Milestone(), new Milestone(), _sys);

}

void AbstractHybridAutomatonManager::activateBlackboard(std::string &rlab_host, int rlab_port, std::string &ros_host, int ros_port)
{
	_blackboard = RTBlackBoard::getInstance(rlab_host, rlab_port, ros_host, ros_port);
}

void AbstractHybridAutomatonManager::setHybridAutomaton(const std::string& _new_hybrid_automaton_str)
{
	DeserializingThreadArguments* thread_args = new DeserializingThreadArguments();
	thread_args->_sys = this->_sys;
	thread_args->_string = _new_hybrid_automaton_str;
	thread_args->_dT = this->_dT;
	thread_args->_noQueue = this->_noQueue;
	thread_args->_deserialize_mutex = &(this->_deserialize_mutex);
	thread_args->_deserialized_hybrid_automatons = &(this->_deserialized_hybrid_automatons);

	if (_beginthreadex(NULL, 0, deserializeHybridAutomaton, (void*)thread_args, 0, NULL) == 0)
	{
		std::cerr << "[AbstractHybridAutomatonManager::updateHybridAutomaton] Error creating thread to deserialize xml string!" << std::endl;
	}
}

void AbstractHybridAutomatonManager::setHybridAutomaton(HybridAutomaton* _new_hybrid_automaton)
{
	WaitForSingleObject(_deserialize_mutex, INFINITE);
	this->_deserialized_hybrid_automatons.push_back(_new_hybrid_automaton);
	ReleaseMutex(_deserialize_mutex);
}

void AbstractHybridAutomatonManager::update(const rTime& t)
{
	this->updateHybridAutomaton();

	this->updateMotionBehaviour(t);

	if (_activeMotionBehaviour)
	{
		Milestone* currentMilestone = (Milestone*)(_activeMotionBehaviour->getChild());
		currentMilestone->update(t);
	}

	rControlAlgorithm::update(t);

	this->updateBlackboard();
}

void AbstractHybridAutomatonManager::updateHybridAutomaton()
{
	if (_blackboard && _blackboard->isUpdated("update_hybrid_automaton"))
	{
		rlab::String* ha_xml = dynamic_cast<rlab::String*>(_blackboard->getData("update_hybrid_automaton"));
		try
		{
			DeserializingThreadArguments* thread_args = new DeserializingThreadArguments();
			thread_args->_sys = this->_sys;
			thread_args->_string = ha_xml->get();
			thread_args->_dT = this->_dT;
			thread_args->_noQueue= this->_noQueue;
			thread_args->_deserialize_mutex = &(this->_deserialize_mutex);
			thread_args->_deserialized_hybrid_automatons = &(this->_deserialized_hybrid_automatons);

			if (_beginthreadex(NULL, 0, deserializeHybridAutomaton, (void*)thread_args, 0, NULL) == 0)
			{
				std::cerr << "[AbstractHybridAutomatonManager::updateHybridAutomaton] Error creating thread to deserialize xml string!" << std::endl;
			}
		}
		catch(::std::string e)
		{
			std::cout << "[AbstractHybridAutomatonManager::updateHybridAutomaton] Error caught: " << std::endl;
			::std::cout << e << ::std::endl;
		}
	}
}

void AbstractHybridAutomatonManager::updateBlackboard()
{
	if(!_blackboard)
		return;

	_blackboard->setJointState("/joint_states", _sys->q(), _sys->qdot(), _torque);

	HTransform relative_transform;
	rxBody* end_effector = _sys->getUCSBody(_T("EE"), relative_transform);
	HTransform absolute_transform = end_effector->T() * relative_transform;
	_blackboard->setTransform("ee", absolute_transform, "base_link");

	_blackboard->step();
}

void AbstractHybridAutomatonManager::setNominalSystem(const TCHAR* path, const TCHAR* aml, const HTransform& T0, const dVector& q0)
{
	this->_path = path;
	this->_aml = aml;
	this->_T0 = T0;
	this->_q0 = q0;
}

void AbstractHybridAutomatonManager::setPeriod(const rTime& dT)
{
	this->_dT = dT;
}

void AbstractHybridAutomatonManager::_readDevices()
{
	_qOld = _q;

	int r = readDeviceValue(_robot, &_q[0], sizeof(double) * _q.size(), 0);
	assert (r == sizeof(double) * _q.size());

}

void AbstractHybridAutomatonManager::_writeDevices()
{
	int w = writeDeviceValue(_robot, &_torque[0], sizeof(double) * _torque.size());
	assert(w == sizeof(double)*_torque.size());
}

void AbstractHybridAutomatonManager::_reflect()
{
	_sys->q(_q);
	_sys->qdot(_qdot);
}

void AbstractHybridAutomatonManager::_compute(const double& t)
{
	if (_servo_on)
	{
		_torque = _activeMotionBehaviour->update(t);
	}
	else
	{
		_torque.zero();
	}

	setTorque(_torque);
}

void AbstractHybridAutomatonManager::_estimate()
{
	this->_qdot_filter->compute(this->_q, this->_qdot);
}

int AbstractHybridAutomatonManager::command(const short& cmd, const int& arg)
{
	switch (cmd)
	{
	case SERVO_ON:
		{
			_servo_on = !_servo_on;

			if(_servo_on)
				this->_qdot_filter->initialize(this->_q);

			std::cout << "[AbstractHybridAutomatonManager::command] Servo ON: " << _servo_on << std::endl;
		}
		break;

	case BLACKBOARD_ON:
		{
			std::map<int, std::string> domain_names;
			domain_names[URI_LOCAL] = "";
			domain_names[URI_BOTTOM_1]	= "130.149.238.178";
			domain_names[URI_BOTTOM_2]	= "130.149.238.179";
			domain_names[URI_BOTTOM_3]	= "130.149.238.180";
			domain_names[URI_LOHENGRIN] = "130.149.238.186";
			domain_names[URI_HASMA]		= "130.149.238.184";
			domain_names[URI_LEIBNIZ]	= "130.149.238.185";
            domain_names[URI_POSEIDON]	= "130.149.238.193";
			domain_names[URI_FIRSTMM]	= "130.149.238.220";
			domain_names[URI_SHOEFER]	= "130.149.238.182";
			domain_names[URI_TOP_1A]	= "130.149.238.188";
			domain_names[URI_TOP_1B]	= "130.149.238.189";
			domain_names[URI_ARIS]		= "130.149.238.194";
			domain_names[URI_ANGEL]		= "130.149.238.149";
			int bit_code = arg;

			int port = bit_code >> 16;
			bit_code &= ~(port << 16);

			int local_host = bit_code >> 8;
			bit_code &= ~(local_host << 8);

			int remote_host = bit_code;

			std::cout << "Enabled BlackBoard. Establishing connection between " << domain_names[local_host] << ":" << port << " and " << domain_names[remote_host] << ":" << port << "." << std::endl;

			if (local_host != URI_LOCAL && domain_names[local_host].empty() ||
				remote_host != URI_LOCAL && domain_names[remote_host].empty())
			{
				std::cout << "WARNING! Unknown host: " << local_host << " or " << remote_host << "! (Will only establish unconnected BlackBoard)" << std::endl;
			}

			activateBlackboard(domain_names[local_host], port, domain_names[remote_host], port);
		}
		break;

	default:
		break;
	}

	return 0;
}

void AbstractHybridAutomatonManager::datanames(vector<string_type>& names, int channel)
{
	switch (channel) {
		case PLOT_TORQUE: names.push_back(_T("Torque"));
		case PLOT_Q: names.push_back(_T("Q"));
		case PLOT_VELOCITY: names.push_back(_T("Velocity"));
		case PLOT_EEFRAME: names.push_back(_T("EEPosition"));
	}
}

void AbstractHybridAutomatonManager::collect(vector<double>& data, int channel)
{
	if (channel == PLOT_TORQUE)
	{
		for(int i = 0; i < _dof; ++i)
			data.push_back(_torque[i]);
	}
	else if (channel == PLOT_Q)
	{
		for(int i = 0; i < _dof; ++i)
			data.push_back(_sys->q()[i]);
	}
	else if (channel == PLOT_VELOCITY)
	{
		for(int i = 0; i < _dof; ++i)
			data.push_back(_sys->qdot()[i]);
	}
	else if (channel == PLOT_EEFRAME)
	{
		HTransform relative_transform;
		rxBody* end_effector = _sys->getUCSBody(_T("EE"), relative_transform);
		HTransform absolute_transform = end_effector->T() * relative_transform;
		for(int i = 0; i < 3; ++i)
			data.push_back(absolute_transform.r(i));
	}
}

void AbstractHybridAutomatonManager::onSetInterestFrame(const TCHAR* name, const HTransform& T)
{
}

/*
rControlAlgorithm* CreateControlAlgorithm(rDC& rdc)
{
	return new AbstractHybridAutomatonManager(rdc);
}
*/<commit_msg>Commented FREE_SYSTEMS out. Gave always a run-time exception.<commit_after>/* RoboticsLab, Copyright 2008-2010 SimLab Co., Ltd. All rights reserved.
*
* This library is commercial and cannot be redistributed, and/or modified
* WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
*/
#include "hybrid_automaton/include/hybrid_automaton_manager/AbstractHybridAutomatonManager.h"
#include "hybrid_automaton/include/hybrid_automaton_manager/AbstractHybridAutomatonManagerCmd.h"

#include "ros_bridge\include\msgs\String.h"
#include "ros_bridge\include\msgs\Transform.h"

#include <process.h>

unsigned __stdcall deserializeHybridAutomaton(void *udata)
{
	DeserializingThreadArguments* thread_args = static_cast<DeserializingThreadArguments*>(udata);
	HybridAutomaton* ha = NULL;
	try {
		ha = XMLDeserializer::createHybridAutomaton(thread_args->_string, thread_args->_sys, thread_args->_dT);
		//std::cout << "--------------- recieving hybrid automaton ------------------" << std::endl;
		//std::cout << ha->toStringXML() << std::endl;
	}
	catch(std::string e)
	{
		std::cerr << "[deserializeHybridAutomaton] ERROR while deserializing hybrid automaton!" << std::endl;
		std::cerr << e << std::endl;

		delete ha;
		delete thread_args;
		return 0;
	}

	WaitForSingleObject(*(thread_args->_deserialize_mutex), INFINITE);
	if(thread_args->_noQueue)
	{
		for(size_t i=0;i<thread_args->_deserialized_hybrid_automatons->size();i++)
		{
			delete thread_args->_deserialized_hybrid_automatons->at(i);
		}
		thread_args->_deserialized_hybrid_automatons->clear();
	}

	thread_args->_deserialized_hybrid_automatons->push_back(ha);
	ReleaseMutex(*(thread_args->_deserialize_mutex));

	delete thread_args;

	return 0;
}

AbstractHybridAutomatonManager::AbstractHybridAutomatonManager(rDC rdc)
:rControlAlgorithm(rdc)
, _sys(NULL)
, _blackboard(NULL)
, _activeMotionBehaviour(NULL)
, _dof(0)
, _servo_on(false)
, _hybrid_automaton(NULL)
, _qdot_filter(NULL)
, _noQueue(false)
{
	_deserialize_mutex = CreateMutex(0, FALSE, 0);
	if( !_deserialize_mutex ) {
		throw(std::string("[AbstractHybridAutomatonManager::HybridAutomatonManager] ERROR: Mutex was not created (_deserialize_mutex)!"));
	}
}

AbstractHybridAutomatonManager::~AbstractHybridAutomatonManager()
{
	if(_activeMotionBehaviour)
		delete _activeMotionBehaviour;

	if(_blackboard)
	{
		delete _blackboard;
		_blackboard = NULL;
	}

	if(_qdot_filter)
	{
		delete _qdot_filter;
	}

	/*
	FREE_SYSTEMS();
	*/
}

void AbstractHybridAutomatonManager::init(int mode)
{
	_sys = LOAD_SYSTEM(_path, _aml, _T0, _q0);
	assert(_sys);

	//Find the robot device. This device is used to read position and write torques
	//to the robot
	_robot = findDevice(_T("ROBOT"));
	RASSERT(_robot != INVALID_RHANDLE);

	rxDevice* ft_sensor = (rxDevice*)findDevice(_T("FT_SENSOR"));
	if (ft_sensor)
		_sys->addDevice(ft_sensor);

	_dof = _sys->jointDOF() + _sys->earthDOF() + _sys->constraintDOF();

	//Initialize variables
	_q.resize(_dof);
	_qOld.resize(_dof);
	_qdot.resize(_dof);
	_torque.resize(_dof);

	_q.zero();
	_qOld.zero();
	_torque.zero();
	_qdot.zero();

	_qdot_filter = new rxFilteredDerivative(this->_dT, 10.0*this->_dT, this->_sys->jdof());

	_activeMotionBehaviour = new MotionBehaviour(new Milestone(), new Milestone(), _sys);

}

void AbstractHybridAutomatonManager::activateBlackboard(std::string &rlab_host, int rlab_port, std::string &ros_host, int ros_port)
{
	_blackboard = RTBlackBoard::getInstance(rlab_host, rlab_port, ros_host, ros_port);
}

void AbstractHybridAutomatonManager::setHybridAutomaton(const std::string& _new_hybrid_automaton_str)
{
	DeserializingThreadArguments* thread_args = new DeserializingThreadArguments();
	thread_args->_sys = this->_sys;
	thread_args->_string = _new_hybrid_automaton_str;
	thread_args->_dT = this->_dT;
	thread_args->_noQueue = this->_noQueue;
	thread_args->_deserialize_mutex = &(this->_deserialize_mutex);
	thread_args->_deserialized_hybrid_automatons = &(this->_deserialized_hybrid_automatons);

	if (_beginthreadex(NULL, 0, deserializeHybridAutomaton, (void*)thread_args, 0, NULL) == 0)
	{
		std::cerr << "[AbstractHybridAutomatonManager::updateHybridAutomaton] Error creating thread to deserialize xml string!" << std::endl;
	}
}

void AbstractHybridAutomatonManager::setHybridAutomaton(HybridAutomaton* _new_hybrid_automaton)
{
	WaitForSingleObject(_deserialize_mutex, INFINITE);
	this->_deserialized_hybrid_automatons.push_back(_new_hybrid_automaton);
	ReleaseMutex(_deserialize_mutex);
}

void AbstractHybridAutomatonManager::update(const rTime& t)
{
	this->updateHybridAutomaton();

	this->updateMotionBehaviour(t);

	if (_activeMotionBehaviour)
	{
		Milestone* currentMilestone = (Milestone*)(_activeMotionBehaviour->getChild());
		currentMilestone->update(t);
	}

	rControlAlgorithm::update(t);

	this->updateBlackboard();
}

void AbstractHybridAutomatonManager::updateHybridAutomaton()
{
	if (_blackboard && _blackboard->isUpdated("update_hybrid_automaton"))
	{
		rlab::String* ha_xml = dynamic_cast<rlab::String*>(_blackboard->getData("update_hybrid_automaton"));
		try
		{
			DeserializingThreadArguments* thread_args = new DeserializingThreadArguments();
			thread_args->_sys = this->_sys;
			thread_args->_string = ha_xml->get();
			thread_args->_dT = this->_dT;
			thread_args->_noQueue= this->_noQueue;
			thread_args->_deserialize_mutex = &(this->_deserialize_mutex);
			thread_args->_deserialized_hybrid_automatons = &(this->_deserialized_hybrid_automatons);

			if (_beginthreadex(NULL, 0, deserializeHybridAutomaton, (void*)thread_args, 0, NULL) == 0)
			{
				std::cerr << "[AbstractHybridAutomatonManager::updateHybridAutomaton] Error creating thread to deserialize xml string!" << std::endl;
			}
		}
		catch(::std::string e)
		{
			std::cout << "[AbstractHybridAutomatonManager::updateHybridAutomaton] Error caught: " << std::endl;
			::std::cout << e << ::std::endl;
		}
	}
}

void AbstractHybridAutomatonManager::updateBlackboard()
{
	if(!_blackboard)
		return;

	_blackboard->setJointState("/joint_states", _sys->q(), _sys->qdot(), _torque);

	HTransform relative_transform;
	rxBody* end_effector = _sys->getUCSBody(_T("EE"), relative_transform);
	HTransform absolute_transform = end_effector->T() * relative_transform;
	_blackboard->setTransform("ee", absolute_transform, "base_link");

	_blackboard->step();
}

void AbstractHybridAutomatonManager::setNominalSystem(const TCHAR* path, const TCHAR* aml, const HTransform& T0, const dVector& q0)
{
	this->_path = path;
	this->_aml = aml;
	this->_T0 = T0;
	this->_q0 = q0;
}

void AbstractHybridAutomatonManager::setPeriod(const rTime& dT)
{
	this->_dT = dT;
}

void AbstractHybridAutomatonManager::_readDevices()
{
	_qOld = _q;

	int r = readDeviceValue(_robot, &_q[0], sizeof(double) * _q.size(), 0);
	assert (r == sizeof(double) * _q.size());

}

void AbstractHybridAutomatonManager::_writeDevices()
{
	int w = writeDeviceValue(_robot, &_torque[0], sizeof(double) * _torque.size());
	assert(w == sizeof(double)*_torque.size());
}

void AbstractHybridAutomatonManager::_reflect()
{
	_sys->q(_q);
	_sys->qdot(_qdot);
}

void AbstractHybridAutomatonManager::_compute(const double& t)
{
	if (_servo_on)
	{
		_torque = _activeMotionBehaviour->update(t);
	}
	else
	{
		_torque.zero();
	}

	setTorque(_torque);
}

void AbstractHybridAutomatonManager::_estimate()
{
	this->_qdot_filter->compute(this->_q, this->_qdot);
}

int AbstractHybridAutomatonManager::command(const short& cmd, const int& arg)
{
	switch (cmd)
	{
	case SERVO_ON:
		{
			_servo_on = !_servo_on;

			if(_servo_on)
				this->_qdot_filter->initialize(this->_q);

			std::cout << "[AbstractHybridAutomatonManager::command] Servo ON: " << _servo_on << std::endl;
		}
		break;

	case BLACKBOARD_ON:
		{
			std::map<int, std::string> domain_names;
			domain_names[URI_LOCAL] = "";
			domain_names[URI_BOTTOM_1]	= "130.149.238.178";
			domain_names[URI_BOTTOM_2]	= "130.149.238.179";
			domain_names[URI_BOTTOM_3]	= "130.149.238.180";
			domain_names[URI_LOHENGRIN] = "130.149.238.186";
			domain_names[URI_HASMA]		= "130.149.238.184";
			domain_names[URI_LEIBNIZ]	= "130.149.238.185";
            domain_names[URI_POSEIDON]	= "130.149.238.193";
			domain_names[URI_FIRSTMM]	= "130.149.238.220";
			domain_names[URI_SHOEFER]	= "130.149.238.182";
			domain_names[URI_TOP_1A]	= "130.149.238.188";
			domain_names[URI_TOP_1B]	= "130.149.238.189";
			domain_names[URI_ARIS]		= "130.149.238.194";
			domain_names[URI_ANGEL]		= "130.149.238.149";
			int bit_code = arg;

			int port = bit_code >> 16;
			bit_code &= ~(port << 16);

			int local_host = bit_code >> 8;
			bit_code &= ~(local_host << 8);

			int remote_host = bit_code;

			std::cout << "Enabled BlackBoard. Establishing connection between " << domain_names[local_host] << ":" << port << " and " << domain_names[remote_host] << ":" << port << "." << std::endl;

			if (local_host != URI_LOCAL && domain_names[local_host].empty() ||
				remote_host != URI_LOCAL && domain_names[remote_host].empty())
			{
				std::cout << "WARNING! Unknown host: " << local_host << " or " << remote_host << "! (Will only establish unconnected BlackBoard)" << std::endl;
			}

			activateBlackboard(domain_names[local_host], port, domain_names[remote_host], port);
		}
		break;

	default:
		break;
	}

	return 0;
}

void AbstractHybridAutomatonManager::datanames(vector<string_type>& names, int channel)
{
	switch (channel) {
		case PLOT_TORQUE: names.push_back(_T("Torque"));
		case PLOT_Q: names.push_back(_T("Q"));
		case PLOT_VELOCITY: names.push_back(_T("Velocity"));
		case PLOT_EEFRAME: names.push_back(_T("EEPosition"));
	}
}

void AbstractHybridAutomatonManager::collect(vector<double>& data, int channel)
{
	if (channel == PLOT_TORQUE)
	{
		for(int i = 0; i < _dof; ++i)
			data.push_back(_torque[i]);
	}
	else if (channel == PLOT_Q)
	{
		for(int i = 0; i < _dof; ++i)
			data.push_back(_sys->q()[i]);
	}
	else if (channel == PLOT_VELOCITY)
	{
		for(int i = 0; i < _dof; ++i)
			data.push_back(_sys->qdot()[i]);
	}
	else if (channel == PLOT_EEFRAME)
	{
		HTransform relative_transform;
		rxBody* end_effector = _sys->getUCSBody(_T("EE"), relative_transform);
		HTransform absolute_transform = end_effector->T() * relative_transform;
		for(int i = 0; i < 3; ++i)
			data.push_back(absolute_transform.r(i));
	}
}

void AbstractHybridAutomatonManager::onSetInterestFrame(const TCHAR* name, const HTransform& T)
{
}

/*
rControlAlgorithm* CreateControlAlgorithm(rDC& rdc)
{
	return new AbstractHybridAutomatonManager(rdc);
}
*/<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/customize/p9_xip_customize.H $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
#ifndef _P9_XIP_CUSTOMIZE_
#define _P9_XIP_CUSTOMIZE_

#include <fapi2.H>

typedef fapi2::ReturnCode (*p9_xip_customize_FP_t) (
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_proc_target,
    void*     io_image,
    uint32_t& io_imageSize,
    void*     io_ringSectionBuf,
    uint32_t& io_ringSectionBufSize,
    uint8_t   i_sysPhase,
    uint8_t   i_modeBuild,
    void*     io_ringBuf1,
    uint32_t  i_ringBufSize1,
    void*     io_ringBuf2,
    uint32_t  i_ringBufSize2,
    uint32_t& io_bootCoreMask);

extern "C"
{
/// @brief p9_xip_customize used to customize the SBE/CME/SGPE images with
///        mailbox attributes, VPD rings and other stuff.
///
/// @param[in] i_proc_target             => P9 proc chip target
/// @param[in/out] io_image              => Pointer to an in-memory image
///                                         HB_SBE:
///                                             In:  SBE image
///                                             Out: Seeprom image
///                                         RT_{CME,SGPE}:
///                                             In:  HW image
///                                             Out: HW image
/// @param[in/out] io_imageSize          => Size of image
///                                         HB_SBE:
///                                             In:  MAX_SEEPROM_IMAGE_SIZE
///                                             Out: Final size
///                                         RT_{CME,SGPE}:
///                                             In:  Size of HW image
///                                             Out: Unchanged
/// @param[in/out] io_ringSectionBuf     => Pointer to an in-memory ring section
///                                         buffer
///                                         HB_SBE:
///                                             Used as temporary image work buffer
///                                             In:  Undefined
///                                             Out: Undefined
///                                         RT_{CME,SGPE}:
///                                             In:  Undefined
///                                             Out: Updated .rings section
/// @param[in/out] io_ringSectionBufSize => In: Size of ring section buffer
///                                         HB_SBE:
///                                             In:  MAX_SEEPROM_IMAGE_SIZE
///                                             Out: Unchanged
///                                         RT_{CME,SGPE}:
///                                             In:  MAX_SEEPROM_IMAGE_SIZE
///                                             Out: Final size
/// @param[in] i_sysPhase                => ={HB_SBE, RT_CME, RT_SGPE}
/// @param[in] i_modeBuild               => ={IPL, REBUILD}
/// @param[in/out] io_ringBuf1           => Caller supplied in-memory buffer
///                                         for uncompressed VPD rings in
/// @param[in] i_ringBufSize1            => Max size of VPD ring buffer
///                                         (Should equal MAX_RING_BUF_SIZE)
/// @param[in/out] io_ringBuf2           => Caller supplied in-memory buffer
///                                         for uncompressed overlay rings
/// @param[in] i_ringBufSize2            => Max size of overlay ring buffer
///                                         (Should equal MAX_RING_BUF_SIZE)
/// @param[in/out] io_bootCoreMask       => In: Mask of the desired boot cores
///                                         Out: Actual boot cores
///                                         (Only used in HB_SBE sysPhase)
///                                         (Bits(8:31) = EC00:EC23)
///
/// @return FAPI_RC_SUCCESS if the customization was successful
///
    fapi2::ReturnCode p9_xip_customize (
        const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_proc_target,
        void*     io_image,
        uint32_t& io_imageSize,
        void*     io_ringSectionBuf,
        uint32_t& io_ringSectionBufSize,
        uint8_t   i_sysPhase,
        uint8_t   i_modeBuild,
        void*     io_ringBuf1,
        uint32_t  i_ringBufSize1,
        void*     io_ringBuf2,
        uint32_t  i_ringBufSize2,
        uint32_t& io_bootCoreMask);

}
#endif
<commit_msg>xip_customize: Four fixes for relieving Seeprom space constraint.<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/customize/p9_xip_customize.H $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
#ifndef _P9_XIP_CUSTOMIZE_
#define _P9_XIP_CUSTOMIZE_

#include <fapi2.H>

typedef fapi2::ReturnCode (*p9_xip_customize_FP_t) (
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_proc_target,
    void*     io_image,
    uint32_t& io_imageSize,
    void*     io_ringSectionBuf,
    uint32_t& io_ringSectionBufSize,
    uint8_t   i_sysPhase,
    uint8_t   i_modeBuild,
    void*     io_ringBuf1,
    uint32_t  i_ringBufSize1,
    void*     io_ringBuf2,
    uint32_t  i_ringBufSize2,
    uint32_t& io_bootCoreMask);

extern "C"
{
/// @brief p9_xip_customize used to customize the SBE/CME/SGPE images with
///        mailbox attributes, VPD rings and other stuff.
///
/// @param[in] i_proc_target             => P9 proc chip target
/// @param[in/out] io_image              => Pointer to an in-memory image
///                                         HB_SBE:
///                                             In:  SBE image
///                                             Out: Seeprom image
///                                         RT_{CME,SGPE}:
///                                             In:  HW image
///                                             Out: HW image
/// @param[in/out] io_imageSize          => Size of image
///                                         HB_SBE:
///                                             In:  >=MAX_SBE_SEEPROM_SIZE
///                                             Out: Final size
///                                         RT_{CME,SGPE}:
///                                             In:  Size of HW image
///                                             Out: Unchanged
/// @param[in/out] io_ringSectionBuf     => Pointer to an in-memory ring section
///                                         buffer
///                                         HB_SBE:
///                                             Used as temporary image work buffer
///                                             In:  Undefined
///                                             Out: Undefined
///                                         RT_{CME,SGPE}:
///                                             In:  Undefined
///                                             Out: Updated .rings section
/// @param[in/out] io_ringSectionBufSize => In: Size of ring section buffer
///                                         HB_SBE:
///                                             In:  Input value of io_imageSize
///                                             Out: Final size
///                                         RT_{CME,SGPE}:
///                                             In:  >=MAX_SBE_SEEPROM_SIZE
///                                             Out: Final size
/// @param[in] i_sysPhase                => ={HB_SBE, RT_CME, RT_SGPE}
/// @param[in] i_modeBuild               => ={IPL, REBUILD}
/// @param[in/out] io_ringBuf1           => Caller supplied in-memory buffer
///                                         for uncompressed VPD rings in
/// @param[in] i_ringBufSize1            => Max size of VPD ring buffer
///                                         (Should equal MAX_RING_BUF_SIZE)
/// @param[in/out] io_ringBuf2           => Caller supplied in-memory buffer
///                                         for uncompressed overlay rings
/// @param[in] i_ringBufSize2            => Max size of overlay ring buffer
///                                         (Should equal MAX_RING_BUF_SIZE)
/// @param[in/out] io_bootCoreMask       => In: Mask of the desired boot cores
///                                         Out: Actual boot cores
///                                         (Only used in HB_SBE sysPhase)
///                                         (Bits(8:31) = EC00:EC23)
///
/// @return FAPI_RC_SUCCESS if the customization was successful
///
    fapi2::ReturnCode p9_xip_customize (
        const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_proc_target,
        void*     io_image,
        uint32_t& io_imageSize,
        void*     io_ringSectionBuf,
        uint32_t& io_ringSectionBufSize,
        uint8_t   i_sysPhase,
        uint8_t   i_modeBuild,
        void*     io_ringBuf1,
        uint32_t  i_ringBufSize1,
        void*     io_ringBuf2,
        uint32_t  i_ringBufSize2,
        uint32_t& io_bootCoreMask);

}
#endif
<|endoftext|>


<commit_before>/*
*
* Copyright 2013 Telefonica Investigacion y Desarrollo, S.A.U
*
* This file is part of Orion Context Broker.
*
* Orion Context Broker is free software: you can redistribute it and/or
* modify it under the terms of the GNU Affero General Public License as
* published by the Free Software Foundation, either version 3 of the
* License, or (at your option) any later version.
*
* Orion Context Broker 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 Affero
* General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* along with Orion Context Broker. If not, see http://www.gnu.org/licenses/.
*
* For those usages not covered by this license please contact with
* fermin at tid dot es
*
* Author: Ken Zangelin
*/
#include <string>
#include <vector>

#include "logMsg/logMsg.h"
#include "logMsg/traceLevels.h"

#include "convenience/UpdateContextElementResponse.h"
#include "convenienceMap/mapPutIndividualContextEntityAttribute.h"
#include "ngsi/ParseData.h"
#include "rest/ConnectionInfo.h"
#include "serviceRoutines/putIndividualContextEntityAttribute.h"

/* ****************************************************************************
*
* putIndividualContextEntityAttribute -
*/
std::string putIndividualContextEntityAttribute(ConnectionInfo* ciP, int components, std::vector<std::string> compV, ParseData* parseDataP)
{
  std::string  answer;
  std::string  entityId      = compV[2];
  std::string  attributeName = compV[4];
  StatusCode   response;

  LM_T(LmtConvenience, ("CONVENIENCE: got a 'PUT' request for entityId '%s'", entityId.c_str()));
  ciP->httpStatusCode = mapPutIndividualContextEntityAttribute(entityId, attributeName, &parseDataP->upcar.res, &response);
  answer = response.render(ciP->outFormat, "");
  response.release();

  return answer;
}
<commit_msg>Change author<commit_after>/*
*
* Copyright 2013 Telefonica Investigacion y Desarrollo, S.A.U
*
* This file is part of Orion Context Broker.
*
* Orion Context Broker is free software: you can redistribute it and/or
* modify it under the terms of the GNU Affero General Public License as
* published by the Free Software Foundation, either version 3 of the
* License, or (at your option) any later version.
*
* Orion Context Broker 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 Affero
* General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* along with Orion Context Broker. If not, see http://www.gnu.org/licenses/.
*
* For those usages not covered by this license please contact with
* fermin at tid dot es
*
* Author: TID Developer
*/
#include <string>
#include <vector>

#include "logMsg/logMsg.h"
#include "logMsg/traceLevels.h"

#include "convenience/UpdateContextElementResponse.h"
#include "convenienceMap/mapPutIndividualContextEntityAttribute.h"
#include "ngsi/ParseData.h"
#include "rest/ConnectionInfo.h"
#include "serviceRoutines/putIndividualContextEntityAttribute.h"

/* ****************************************************************************
*
* putIndividualContextEntityAttribute -
*/
std::string putIndividualContextEntityAttribute(ConnectionInfo* ciP, int components, std::vector<std::string> compV, ParseData* parseDataP)
{
  std::string  answer;
  std::string  entityId      = compV[2];
  std::string  attributeName = compV[4];
  StatusCode   response;

  LM_T(LmtConvenience, ("CONVENIENCE: got a 'PUT' request for entityId '%s'", entityId.c_str()));
  ciP->httpStatusCode = mapPutIndividualContextEntityAttribute(entityId, attributeName, &parseDataP->upcar.res, &response);
  answer = response.render(ciP->outFormat, "");
  response.release();

  return answer;
}
<|endoftext|>


<commit_before>/*
	This file is part of cpp-ethereum.
	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.
	cpp-ethereum 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 General Public License for more details.
	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file ClientModel.cpp
 * @author Yann yann@ethdev.com
 * @author Arkadiy Paronyan arkadiy@ethdev.com
 * @date 2015
 * Ethereum IDE client.
 */

#include <QtConcurrent/QtConcurrent>
#include <QDebug>
#include <QQmlContext>
#include <QQmlApplicationEngine>
#include <libdevcore/CommonJS.h>
#include <libethereum/Transaction.h>
#include <libqwebthree/QWebThree.h>
#include "AppContext.h"
#include "DebuggingStateWrapper.h"
#include "QContractDefinition.h"
#include "QVariableDeclaration.h"
#include "ContractCallDataEncoder.h"
#include "CodeModel.h"
#include "ClientModel.h"
#include "QEther.h"
#include "Web3Server.h"
#include "ClientModel.h"

using namespace dev;
using namespace dev::eth;
using namespace dev::mix;

ClientModel::ClientModel(AppContext* _context):
	m_context(_context), m_running(false), m_qWebThree(nullptr)
{
	qRegisterMetaType<QBigInt*>("QBigInt*");
	qRegisterMetaType<QEther*>("QEther*");
	qRegisterMetaType<QVariableDefinition*>("QVariableDefinition*");
	qRegisterMetaType<QVariableDefinitionList*>("QVariableDefinitionList*");
	qRegisterMetaType<QList<QVariableDefinition*>>("QList<QVariableDefinition*>");
	qRegisterMetaType<QList<QVariableDeclaration*>>("QList<QVariableDeclaration*>");
	qRegisterMetaType<QVariableDeclaration*>("QVariableDeclaration*");
	qRegisterMetaType<AssemblyDebuggerData>("AssemblyDebuggerData");

	connect(this, &ClientModel::dataAvailable, this, &ClientModel::showDebugger, Qt::QueuedConnection);
	m_client.reset(new MixClient());

	m_qWebThree = new QWebThree(this);
	m_qWebThreeConnector.reset(new QWebThreeConnector());
	m_qWebThreeConnector->setQWeb(m_qWebThree);
	m_web3Server.reset(new Web3Server(*m_qWebThreeConnector.get(), std::vector<dev::KeyPair> { m_client->userAccount() }, m_client.get()));
	connect(m_qWebThree, &QWebThree::response, this, &ClientModel::apiResponse);

	_context->appEngine()->rootContext()->setContextProperty("clientModel", this);
}

ClientModel::~ClientModel()
{
}

void ClientModel::apiRequest(QString const& _message)
{
	(void)_message;
//	m_qWebThree->postMessage(_message);
}

QString ClientModel::contractAddress() const
{
	QString address = QString::fromStdString(dev::toJS(m_client->lastContractAddress()));
	return address;
}

void ClientModel::debugDeployment()
{
	executeSequence(std::vector<TransactionSettings>(), 10000000 * ether);
}

void ClientModel::debugState(QVariantMap _state)
{
	u256 balance = (qvariant_cast<QEther*>(_state.value("balance")))->toU256Wei();
	QVariantList transactions = _state.value("transactions").toList();

	std::vector<TransactionSettings> transactionSequence;
	TransactionSettings constructorTr;
	for (auto const& t: transactions)
	{
		QVariantMap transaction = t.toMap();
		QString functionId = transaction.value("functionId").toString();
		u256 gas = (qvariant_cast<QEther*>(transaction.value("gas")))->toU256Wei();
		u256 value = (qvariant_cast<QEther*>(transaction.value("value")))->toU256Wei();
		u256 gasPrice = (qvariant_cast<QEther*>(transaction.value("gasPrice")))->toU256Wei();
		QVariantMap params = transaction.value("parameters").toMap();
		TransactionSettings transactionSettings(functionId, value, gas, gasPrice);

		for (auto p = params.cbegin(); p != params.cend(); ++p)
		{
			QBigInt* param = qvariant_cast<QBigInt*>(p.value());
			transactionSettings.parameterValues.insert(std::make_pair(p.key(), boost::get<dev::u256>(param->internalValue())));
		}

		if (transaction.value("executeConstructor").toBool())
			constructorTr = transactionSettings;
		else
			transactionSequence.push_back(transactionSettings);
	}
	executeSequence(transactionSequence, balance, constructorTr);
}

void ClientModel::executeSequence(std::vector<TransactionSettings> const& _sequence, u256 _balance, TransactionSettings const& ctrTransaction)
{
	if (m_running)
		throw (std::logic_error("debugging already running"));
	auto compilerRes = m_context->codeModel()->code();
	std::shared_ptr<QContractDefinition> contractDef = compilerRes->sharedContract();
	m_running = true;

	emit runStarted();
	emit stateChanged();

	//run sequence
	QtConcurrent::run([=]()
	{
		try
		{
			bytes contractCode = compilerRes->bytes();
			std::vector<dev::bytes> transactonData;
			QFunctionDefinition* f = nullptr;
			ContractCallDataEncoder c;
			//encode data for all transactions
			for (auto const& t: _sequence)
			{
				f = nullptr;
				for (int tf = 0; tf < contractDef->functionsList().size(); tf++)
				{
					if (contractDef->functionsList().at(tf)->name() == t.functionId)
					{
						f = contractDef->functionsList().at(tf);
						break;
					}
				}
				if (!f)
					throw std::runtime_error("function " + t.functionId.toStdString() + " not found");

				c.encode(f);
				for (int p = 0; p < f->parametersList().size(); p++)
				{
					QVariableDeclaration* var = (QVariableDeclaration*)f->parametersList().at(p);
					u256 value = 0;
					auto v = t.parameterValues.find(var->name());
					if (v != t.parameterValues.cend())
						value = v->second;
					c.encode(var, value);
				}
				transactonData.emplace_back(c.encodedData());
			}

			//run contract creation first
			m_client->resetState(_balance);
			ExecutionResult debuggingContent = deployContract(contractCode, ctrTransaction);
			Address address = debuggingContent.contractAddress;
			for (unsigned i = 0; i < _sequence.size(); ++i)
				debuggingContent = callContract(address, transactonData.at(i), _sequence.at(i));

			QList<QVariableDefinition*> returnParameters;

			if (f)
				returnParameters = c.decode(f->returnParameters(), debuggingContent.returnValue);

			//we need to wrap states in a QObject before sending to QML.
			QList<QObject*> wStates;
			for (unsigned i = 0; i < debuggingContent.machineStates.size(); i++)
			{
				QPointer<DebuggingStateWrapper> s(new DebuggingStateWrapper(debuggingContent.executionCode, debuggingContent.executionData.toBytes()));
				s->setState(debuggingContent.machineStates[i]);
				wStates.append(s);
			}
			//collect states for last transaction
			AssemblyDebuggerData code = DebuggingStateWrapper::getHumanReadableCode(debuggingContent.executionCode);
			emit dataAvailable(returnParameters, wStates, code);
			emit runComplete();
		}
		catch(boost::exception const&)
		{
			emit runFailed(QString::fromStdString(boost::current_exception_diagnostic_information()));
		}

		catch(std::exception const& e)
		{
			emit runFailed(e.what());
		}
		m_running = false;
		emit stateChanged();
	});
}

void ClientModel::showDebugger(QList<QVariableDefinition*> const& _returnParam, QList<QObject*> const& _wStates, AssemblyDebuggerData const& _code)
{
	m_context->appEngine()->rootContext()->setContextProperty("debugStates", QVariant::fromValue(_wStates));
	m_context->appEngine()->rootContext()->setContextProperty("humanReadableExecutionCode", QVariant::fromValue(std::get<0>(_code)));
	m_context->appEngine()->rootContext()->setContextProperty("bytesCodeMapping", QVariant::fromValue(std::get<1>(_code)));
	m_context->appEngine()->rootContext()->setContextProperty("contractCallReturnParameters", QVariant::fromValue(new QVariableDefinitionList(_returnParam)));
	showDebuggerWindow();
}

void ClientModel::showDebugError(QString const& _error)
{
	//TODO: change that to a signal
	m_context->displayMessageDialog(tr("Debugger"), _error);
}

ExecutionResult ClientModel::deployContract(bytes const& _code, TransactionSettings const& _ctrTransaction)
{
	Address newAddress;
	if (!ctrTransaction.isEmpty())
		newAddress = m_client->transact(m_client->userAccount().secret(), _ctrTransaction.value, _code, _ctrTransaction.gas, _ctrTransaction.gasPrice);
	else
	{
		u256 gasPrice = 10000000000000;
		u256 gas = 125000;
		u256 amount = 100;
		newAddress = m_client->transact(m_client->userAccount().secret(), amount, _code, gas, gasPrice);
	}

	Address lastAddress = m_client->lastContractAddress();
	ExecutionResult r = m_client->lastExecutionResult();
	if (newAddress != lastAddress)
		contractAddressChanged();
	return r;
}

ExecutionResult ClientModel::callContract(Address const& _contract, bytes const& _data, TransactionSettings const& _tr)
{
	m_client->transact(m_client->userAccount().secret(), _tr.value, _contract, _data, _tr.gas, _tr.gasPrice);
	ExecutionResult r = m_client->lastExecutionResult();
	r.contractAddress = _contract;
	return r;
}
<commit_msg> - Coding Standards<commit_after>/*
	This file is part of cpp-ethereum.
	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.
	cpp-ethereum 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 General Public License for more details.
	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file ClientModel.cpp
 * @author Yann yann@ethdev.com
 * @author Arkadiy Paronyan arkadiy@ethdev.com
 * @date 2015
 * Ethereum IDE client.
 */

#include <QtConcurrent/QtConcurrent>
#include <QDebug>
#include <QQmlContext>
#include <QQmlApplicationEngine>
#include <libdevcore/CommonJS.h>
#include <libethereum/Transaction.h>
#include <libqwebthree/QWebThree.h>
#include "AppContext.h"
#include "DebuggingStateWrapper.h"
#include "QContractDefinition.h"
#include "QVariableDeclaration.h"
#include "ContractCallDataEncoder.h"
#include "CodeModel.h"
#include "ClientModel.h"
#include "QEther.h"
#include "Web3Server.h"
#include "ClientModel.h"

using namespace dev;
using namespace dev::eth;
using namespace dev::mix;

ClientModel::ClientModel(AppContext* _context):
	m_context(_context), m_running(false), m_qWebThree(nullptr)
{
	qRegisterMetaType<QBigInt*>("QBigInt*");
	qRegisterMetaType<QEther*>("QEther*");
	qRegisterMetaType<QVariableDefinition*>("QVariableDefinition*");
	qRegisterMetaType<QVariableDefinitionList*>("QVariableDefinitionList*");
	qRegisterMetaType<QList<QVariableDefinition*>>("QList<QVariableDefinition*>");
	qRegisterMetaType<QList<QVariableDeclaration*>>("QList<QVariableDeclaration*>");
	qRegisterMetaType<QVariableDeclaration*>("QVariableDeclaration*");
	qRegisterMetaType<AssemblyDebuggerData>("AssemblyDebuggerData");

	connect(this, &ClientModel::dataAvailable, this, &ClientModel::showDebugger, Qt::QueuedConnection);
	m_client.reset(new MixClient());

	m_qWebThree = new QWebThree(this);
	m_qWebThreeConnector.reset(new QWebThreeConnector());
	m_qWebThreeConnector->setQWeb(m_qWebThree);
	m_web3Server.reset(new Web3Server(*m_qWebThreeConnector.get(), std::vector<dev::KeyPair> { m_client->userAccount() }, m_client.get()));
	connect(m_qWebThree, &QWebThree::response, this, &ClientModel::apiResponse);

	_context->appEngine()->rootContext()->setContextProperty("clientModel", this);
}

ClientModel::~ClientModel()
{
}

void ClientModel::apiRequest(QString const& _message)
{
	(void)_message;
//	m_qWebThree->postMessage(_message);
}

QString ClientModel::contractAddress() const
{
	QString address = QString::fromStdString(dev::toJS(m_client->lastContractAddress()));
	return address;
}

void ClientModel::debugDeployment()
{
	executeSequence(std::vector<TransactionSettings>(), 10000000 * ether);
}

void ClientModel::debugState(QVariantMap _state)
{
	u256 balance = (qvariant_cast<QEther*>(_state.value("balance")))->toU256Wei();
	QVariantList transactions = _state.value("transactions").toList();

	std::vector<TransactionSettings> transactionSequence;
	TransactionSettings constructorTr;
	for (auto const& t: transactions)
	{
		QVariantMap transaction = t.toMap();
		QString functionId = transaction.value("functionId").toString();
		u256 gas = (qvariant_cast<QEther*>(transaction.value("gas")))->toU256Wei();
		u256 value = (qvariant_cast<QEther*>(transaction.value("value")))->toU256Wei();
		u256 gasPrice = (qvariant_cast<QEther*>(transaction.value("gasPrice")))->toU256Wei();
		QVariantMap params = transaction.value("parameters").toMap();
		TransactionSettings transactionSettings(functionId, value, gas, gasPrice);

		for (auto p = params.cbegin(); p != params.cend(); ++p)
		{
			QBigInt* param = qvariant_cast<QBigInt*>(p.value());
			transactionSettings.parameterValues.insert(std::make_pair(p.key(), boost::get<dev::u256>(param->internalValue())));
		}

		if (transaction.value("executeConstructor").toBool())
			constructorTr = transactionSettings;
		else
			transactionSequence.push_back(transactionSettings);
	}
	executeSequence(transactionSequence, balance, constructorTr);
}

void ClientModel::executeSequence(std::vector<TransactionSettings> const& _sequence, u256 _balance, TransactionSettings const& ctrTransaction)
{
	if (m_running)
		throw (std::logic_error("debugging already running"));
	auto compilerRes = m_context->codeModel()->code();
	std::shared_ptr<QContractDefinition> contractDef = compilerRes->sharedContract();
	m_running = true;

	emit runStarted();
	emit stateChanged();

	//run sequence
	QtConcurrent::run([=]()
	{
		try
		{
			bytes contractCode = compilerRes->bytes();
			std::vector<dev::bytes> transactonData;
			QFunctionDefinition* f = nullptr;
			ContractCallDataEncoder c;
			//encode data for all transactions
			for (auto const& t: _sequence)
			{
				f = nullptr;
				for (int tf = 0; tf < contractDef->functionsList().size(); tf++)
				{
					if (contractDef->functionsList().at(tf)->name() == t.functionId)
					{
						f = contractDef->functionsList().at(tf);
						break;
					}
				}
				if (!f)
					throw std::runtime_error("function " + t.functionId.toStdString() + " not found");

				c.encode(f);
				for (int p = 0; p < f->parametersList().size(); p++)
				{
					QVariableDeclaration* var = (QVariableDeclaration*)f->parametersList().at(p);
					u256 value = 0;
					auto v = t.parameterValues.find(var->name());
					if (v != t.parameterValues.cend())
						value = v->second;
					c.encode(var, value);
				}
				transactonData.emplace_back(c.encodedData());
			}

			//run contract creation first
			m_client->resetState(_balance);
			ExecutionResult debuggingContent = deployContract(contractCode, ctrTransaction);
			Address address = debuggingContent.contractAddress;
			for (unsigned i = 0; i < _sequence.size(); ++i)
				debuggingContent = callContract(address, transactonData.at(i), _sequence.at(i));

			QList<QVariableDefinition*> returnParameters;

			if (f)
				returnParameters = c.decode(f->returnParameters(), debuggingContent.returnValue);

			//we need to wrap states in a QObject before sending to QML.
			QList<QObject*> wStates;
			for (unsigned i = 0; i < debuggingContent.machineStates.size(); i++)
			{
				QPointer<DebuggingStateWrapper> s(new DebuggingStateWrapper(debuggingContent.executionCode, debuggingContent.executionData.toBytes()));
				s->setState(debuggingContent.machineStates[i]);
				wStates.append(s);
			}
			//collect states for last transaction
			AssemblyDebuggerData code = DebuggingStateWrapper::getHumanReadableCode(debuggingContent.executionCode);
			emit dataAvailable(returnParameters, wStates, code);
			emit runComplete();
		}
		catch(boost::exception const&)
		{
			emit runFailed(QString::fromStdString(boost::current_exception_diagnostic_information()));
		}

		catch(std::exception const& e)
		{
			emit runFailed(e.what());
		}
		m_running = false;
		emit stateChanged();
	});
}

void ClientModel::showDebugger(QList<QVariableDefinition*> const& _returnParam, QList<QObject*> const& _wStates, AssemblyDebuggerData const& _code)
{
	m_context->appEngine()->rootContext()->setContextProperty("debugStates", QVariant::fromValue(_wStates));
	m_context->appEngine()->rootContext()->setContextProperty("humanReadableExecutionCode", QVariant::fromValue(std::get<0>(_code)));
	m_context->appEngine()->rootContext()->setContextProperty("bytesCodeMapping", QVariant::fromValue(std::get<1>(_code)));
	m_context->appEngine()->rootContext()->setContextProperty("contractCallReturnParameters", QVariant::fromValue(new QVariableDefinitionList(_returnParam)));
	showDebuggerWindow();
}

void ClientModel::showDebugError(QString const& _error)
{
	//TODO: change that to a signal
	m_context->displayMessageDialog(tr("Debugger"), _error);
}

ExecutionResult ClientModel::deployContract(bytes const& _code, TransactionSettings const& _ctrTransaction)
{
	Address newAddress;
	if (!_ctrTransaction.isEmpty())
		newAddress = m_client->transact(m_client->userAccount().secret(), _ctrTransaction.value, _code, _ctrTransaction.gas, _ctrTransaction.gasPrice);
	else
	{
		u256 gasPrice = 10000000000000;
		u256 gas = 125000;
		u256 amount = 100;
		newAddress = m_client->transact(m_client->userAccount().secret(), amount, _code, gas, gasPrice);
	}

	Address lastAddress = m_client->lastContractAddress();
	ExecutionResult r = m_client->lastExecutionResult();
	if (newAddress != lastAddress)
		contractAddressChanged();
	return r;
}

ExecutionResult ClientModel::callContract(Address const& _contract, bytes const& _data, TransactionSettings const& _tr)
{
	m_client->transact(m_client->userAccount().secret(), _tr.value, _contract, _data, _tr.gas, _tr.gasPrice);
	ExecutionResult r = m_client->lastExecutionResult();
	r.contractAddress = _contract;
	return r;
}
<|endoftext|>


<commit_before>#include "SphericalProjectionSurface.h"
#include "utils/Const.h"

namespace carto {

    SphericalProjectionSurface::SphericalProjectionSurface() {
    }

    MapPos SphericalProjectionSurface::calculateMapPos(const cglib::vec3<double>& pos) const {
        return SphericalToInternal(pos * (1.0 / SPHERE_SIZE));
    }

    MapVec SphericalProjectionSurface::calculateMapVec(const cglib::vec3<double>& pos, const cglib::vec3<double>& vec) const {
        cglib::vec3<double> xyzPos = pos * (1.0 / SPHERE_SIZE);
        cglib::mat3x3<double> xyzFrame = LocalFrame(xyzPos);
        double scale = std::sqrt(xyzPos(0) * xyzPos(0) + xyzPos(1) * xyzPos(1)) / Const::PI;
        cglib::vec3<double> mapVec = cglib::transform(vec, cglib::transpose(xyzFrame));
        return MapVec(mapVec(0) * scale, mapVec(1) * scale, mapVec(2) * scale);
    }

    cglib::vec3<double> SphericalProjectionSurface::calculatePosition(const MapPos& mapPos) const {
        return InternalToSpherical(mapPos) * SPHERE_SIZE;
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateNormal(const MapPos& mapPos) const {
        return InternalToSpherical(mapPos);
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateVector(const MapPos& mapPos, const MapVec& mapVec) const {
        cglib::vec3<double> xyzPos = InternalToSpherical(mapPos);
        cglib::mat3x3<double> xyzFrame = LocalFrame(xyzPos);
        double scale = Const::PI / std::sqrt(xyzPos(0) * xyzPos(0) + xyzPos(1) * xyzPos(1));
        return cglib::transform(cglib::vec3<double>(mapVec.getX() * scale, mapVec.getY() * scale, mapVec.getZ() * scale), xyzFrame);
    }

    void SphericalProjectionSurface::tesselateSegment(const MapPos& mapPos0, const MapPos& mapPos1, std::vector<MapPos>& mapPoses) const {
        // TODO: make it faster, non-recursive
        MapPos mapPosM;
        if (SplitSegment(mapPos0, mapPos1, mapPosM)) {
            tesselateSegment(mapPos0, mapPosM, mapPoses);
            tesselateSegment(mapPosM, mapPos1, mapPoses);
            return;
        }

        mapPoses.push_back(mapPos0);
        mapPoses.push_back(mapPos1);
    }

    void SphericalProjectionSurface::tesselateTriangle(unsigned int i0, unsigned int i1, unsigned int i2, std::vector<unsigned int>& indices, std::vector<MapPos>& mapPoses) const {
        const MapPos& mapPos0 = mapPoses.at(i0);
        const MapPos& mapPos1 = mapPoses.at(i1);
        const MapPos& mapPos2 = mapPoses.at(i2);

        // TODO: make it faster, non-recursive
        MapPos mapPosM;
        if (SplitSegment(mapPos0, mapPos1, mapPosM)) {
            unsigned int iM = static_cast<int>(mapPoses.size());
            mapPoses.push_back(mapPosM);
            tesselateTriangle(i2, i0, iM, indices, mapPoses);
            tesselateTriangle(i1, i2, iM, indices, mapPoses);
            return;
        } else if (SplitSegment(mapPos0, mapPos2, mapPosM)) {
            unsigned int iM = static_cast<int>(mapPoses.size());
            mapPoses.push_back(mapPosM);
            tesselateTriangle(i0, i1, iM, indices, mapPoses);
            tesselateTriangle(i1, i2, iM, indices, mapPoses);
            return;
        } else if (SplitSegment(mapPos1, mapPos2, mapPosM)) {
            unsigned int iM = static_cast<int>(mapPoses.size());
            mapPoses.push_back(mapPosM);
            tesselateTriangle(i0, i1, iM, indices, mapPoses);
            tesselateTriangle(i2, i0, iM, indices, mapPoses);
            return;
        }

        indices.push_back(i0);
        indices.push_back(i1);
        indices.push_back(i2);
    }

    double SphericalProjectionSurface::calculateDistance(const cglib::vec3<double> pos0, const cglib::vec3<double>& pos1) const {
        double dot = cglib::dot_product(cglib::unit(pos0), cglib::unit(pos1));
        double angle = std::acos(std::min(1.0, std::max(-1.0, dot)));
        if (angle < PLANAR_APPROX_ANGLE) {
            cglib::vec3<double> delta = cglib::unit(pos0) - cglib::unit(pos1);
            return cglib::length(delta) * Const::WORLD_SIZE;
        }
        return angle * Const::WORLD_SIZE;
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateNearestPoint(const cglib::vec3<double>& pos, double height) const {
        return cglib::unit(pos) * (SPHERE_SIZE + height);
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateNearestPoint(const cglib::ray3<double>& ray, double height, double& t) const {
        if (calculateHitPoint(ray, height, t)) {
            return ray(t);
        }
        t = -cglib::dot_product(ray.origin, ray.direction) / cglib::norm(ray.direction);
        return cglib::unit(ray(t)) * (SPHERE_SIZE + height);
    }

    bool SphericalProjectionSurface::calculateHitPoint(const cglib::ray3<double>& ray, double height, double& t) const {
        double a = cglib::norm(ray.direction);
        double b = 2.0 * cglib::dot_product(ray.direction, ray.origin);
        double c = cglib::norm(ray.origin) - (SPHERE_SIZE + height) * (SPHERE_SIZE + height);
        double d = b * b - 4.0 * a * c;
        if (d < 0.0) {
            return false;
        }
        t = (-b - std::sqrt(d)) / (2.0 * a);
        return true;
    }

    cglib::mat4x4<double> SphericalProjectionSurface::calculateLocalFrameMatrix(const cglib::vec3<double>& pos) const {
        cglib::mat3x3<double> localFrameMat3 = LocalFrame(pos * (1.0 / SPHERE_SIZE));
        cglib::mat4x4<double> localFrameMat4 = cglib::mat4x4<double>::identity();
        for (int i = 0; i < 3; i++) {
            cglib::vec3<double> dir = cglib::unit(cglib::col_vector(localFrameMat3, i));
            for (int j = 0; j < 3; j++) {
                localFrameMat4(j, i) = dir(j) * SPHERE_SIZE / Const::EARTH_RADIUS;
            }
            localFrameMat4(i, 3) = pos(i);
        }
        return localFrameMat4;
    }

    cglib::mat4x4<double> SphericalProjectionSurface::calculateTranslateMatrix(const cglib::vec3<double>& pos0, const cglib::vec3<double>& pos1, double t) const {
        double scale = (1 - t) + (cglib::length(pos1) / cglib::length(pos0)) * t;
        cglib::mat4x4<double> scaleMat = cglib::scale4_matrix(cglib::vec3<double>(scale, scale, scale));

        double dot = cglib::dot_product(cglib::unit(pos0), cglib::unit(pos1));
        double angle = std::acos(std::min(1.0, std::max(-1.0, dot)));
        cglib::vec3<double> axis = cglib::vector_product(cglib::unit(pos0), cglib::unit(pos1));
        if (cglib::length(axis) > 0) {
            if (angle < PLANAR_APPROX_ANGLE) { // less than 10m
                return cglib::translate4_matrix((cglib::unit(pos1) - cglib::unit(pos0)) * (SPHERE_SIZE * t)) * scaleMat;
            }
            return cglib::rotate4_matrix(axis, angle * t) * scaleMat;
        }
        return scaleMat;
    }

    bool SphericalProjectionSurface::SplitSegment(const MapPos& mapPos0, const MapPos& mapPos1, MapPos& mapPosM) {
        cglib::vec3<double> pos0 = cglib::unit(InternalToSpherical(mapPos0));
        cglib::vec3<double> pos1 = cglib::unit(InternalToSpherical(mapPos1));
        double dot = cglib::dot_product(pos0, pos1);
        if (dot <= -1) {
            return false; // exactly on the opposite side, no way to split
        }
        double angle = std::acos(std::min(1.0, std::max(-1.0, dot)));
        if (angle * Const::EARTH_RADIUS < SEGMENT_SPLIT_THRESHOLD) {
            return false;
        }
        mapPosM = SphericalToInternal(cglib::unit(pos0 + pos1));
        return true;
    }

    MapPos SphericalProjectionSurface::SphericalToInternal(const cglib::vec3<double>& pos) {
        double scale = Const::WORLD_SIZE / (2 * Const::PI);
        double length = cglib::length(pos);
        double x1 = pos(0) != 0 || pos(1) != 0 ? std::atan2(pos(1), pos(0)) : 0;
        double y1 = std::atanh(std::max(-1.0, std::min(1.0, pos(2) / length)));
        double z1 = length - 1.0;
        return MapPos(x1 * scale, y1 * scale, z1 * scale);
    }

    cglib::vec3<double> SphericalProjectionSurface::InternalToSpherical(const MapPos& mapPos) {
        double scale = 2 * Const::PI / Const::WORLD_SIZE;
        double x1 = mapPos.getX() * scale;
        double y1 = mapPos.getY() * scale;
        double z1 = mapPos.getZ() * scale;
        double rz = std::tanh(y1);
        double ss = std::sqrt(std::max(0.0, 1.0 - rz * rz));
        double rx = ss * std::cos(x1);
        double ry = ss * std::sin(x1);
        return cglib::vec3<double>(rx, ry, rz) * (1.0 + z1);
    }

    cglib::mat3x3<double> SphericalProjectionSurface::LocalFrame(const cglib::vec3<double>& pos) {
        double x = pos(0);
        double y = pos(1);
        double z = pos(2);

        double dx_du = -y;
        double dy_du = x;
        double dz_du = 0;

        double dx_dv = -z * x;
        double dy_dv = -z * y;
        double dz_dv = x * x + y * y;

        double dx_dh = x;
        double dy_dh = y;
        double dz_dh = z;

        return cglib::mat3x3<double> { { dx_du, dx_dv, dx_dh }, { dy_du, dy_dv, dy_dh }, { dz_du, dz_dv, dz_dh } };
    }

    const double SphericalProjectionSurface::SPHERE_SIZE = Const::WORLD_SIZE / Const::PI;

    const double SphericalProjectionSurface::PLANAR_APPROX_ANGLE = 1.0e-6;

    const double SphericalProjectionSurface::SEGMENT_SPLIT_THRESHOLD = Const::EARTH_CIRCUMFERENCE / 120.0;

}
<commit_msg>Fix Z coordinate handling in SphericalProjectionSurface<commit_after>#include "SphericalProjectionSurface.h"
#include "utils/Const.h"

namespace carto {

    SphericalProjectionSurface::SphericalProjectionSurface() {
    }

    MapPos SphericalProjectionSurface::calculateMapPos(const cglib::vec3<double>& pos) const {
        return SphericalToInternal(pos * (1.0 / SPHERE_SIZE));
    }

    MapVec SphericalProjectionSurface::calculateMapVec(const cglib::vec3<double>& pos, const cglib::vec3<double>& vec) const {
        cglib::vec3<double> xyzPos = pos * (1.0 / SPHERE_SIZE);
        cglib::mat3x3<double> xyzFrame = LocalFrame(xyzPos);
        double scale = std::sqrt(xyzPos(0) * xyzPos(0) + xyzPos(1) * xyzPos(1)) / Const::PI;
        cglib::vec3<double> mapVec = cglib::transform(vec, cglib::transpose(xyzFrame));
        return MapVec(mapVec(0) * scale, mapVec(1) * scale, mapVec(2) * scale);
    }

    cglib::vec3<double> SphericalProjectionSurface::calculatePosition(const MapPos& mapPos) const {
        return InternalToSpherical(mapPos) * SPHERE_SIZE;
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateNormal(const MapPos& mapPos) const {
        return InternalToSpherical(mapPos);
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateVector(const MapPos& mapPos, const MapVec& mapVec) const {
        cglib::vec3<double> xyzPos = InternalToSpherical(mapPos);
        cglib::mat3x3<double> xyzFrame = LocalFrame(xyzPos);
        double scale = Const::PI / std::sqrt(xyzPos(0) * xyzPos(0) + xyzPos(1) * xyzPos(1));
        return cglib::transform(cglib::vec3<double>(mapVec.getX() * scale, mapVec.getY() * scale, mapVec.getZ() * scale), xyzFrame);
    }

    void SphericalProjectionSurface::tesselateSegment(const MapPos& mapPos0, const MapPos& mapPos1, std::vector<MapPos>& mapPoses) const {
        // TODO: make it faster, non-recursive
        MapPos mapPosM;
        if (SplitSegment(mapPos0, mapPos1, mapPosM)) {
            tesselateSegment(mapPos0, mapPosM, mapPoses);
            tesselateSegment(mapPosM, mapPos1, mapPoses);
            return;
        }

        mapPoses.push_back(mapPos0);
        mapPoses.push_back(mapPos1);
    }

    void SphericalProjectionSurface::tesselateTriangle(unsigned int i0, unsigned int i1, unsigned int i2, std::vector<unsigned int>& indices, std::vector<MapPos>& mapPoses) const {
        const MapPos& mapPos0 = mapPoses.at(i0);
        const MapPos& mapPos1 = mapPoses.at(i1);
        const MapPos& mapPos2 = mapPoses.at(i2);

        // TODO: make it faster, non-recursive
        MapPos mapPosM;
        if (SplitSegment(mapPos0, mapPos1, mapPosM)) {
            unsigned int iM = static_cast<int>(mapPoses.size());
            mapPoses.push_back(mapPosM);
            tesselateTriangle(i2, i0, iM, indices, mapPoses);
            tesselateTriangle(i1, i2, iM, indices, mapPoses);
            return;
        } else if (SplitSegment(mapPos0, mapPos2, mapPosM)) {
            unsigned int iM = static_cast<int>(mapPoses.size());
            mapPoses.push_back(mapPosM);
            tesselateTriangle(i0, i1, iM, indices, mapPoses);
            tesselateTriangle(i1, i2, iM, indices, mapPoses);
            return;
        } else if (SplitSegment(mapPos1, mapPos2, mapPosM)) {
            unsigned int iM = static_cast<int>(mapPoses.size());
            mapPoses.push_back(mapPosM);
            tesselateTriangle(i0, i1, iM, indices, mapPoses);
            tesselateTriangle(i2, i0, iM, indices, mapPoses);
            return;
        }

        indices.push_back(i0);
        indices.push_back(i1);
        indices.push_back(i2);
    }

    double SphericalProjectionSurface::calculateDistance(const cglib::vec3<double> pos0, const cglib::vec3<double>& pos1) const {
        double dot = cglib::dot_product(cglib::unit(pos0), cglib::unit(pos1));
        double angle = std::acos(std::min(1.0, std::max(-1.0, dot)));
        if (angle < PLANAR_APPROX_ANGLE) {
            cglib::vec3<double> delta = cglib::unit(pos0) - cglib::unit(pos1);
            return cglib::length(delta) * Const::WORLD_SIZE;
        }
        return angle * Const::WORLD_SIZE;
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateNearestPoint(const cglib::vec3<double>& pos, double height) const {
        return cglib::unit(pos) * (SPHERE_SIZE + height);
    }

    cglib::vec3<double> SphericalProjectionSurface::calculateNearestPoint(const cglib::ray3<double>& ray, double height, double& t) const {
        if (calculateHitPoint(ray, height, t)) {
            return ray(t);
        }
        t = -cglib::dot_product(ray.origin, ray.direction) / cglib::norm(ray.direction);
        return cglib::unit(ray(t)) * (SPHERE_SIZE + height);
    }

    bool SphericalProjectionSurface::calculateHitPoint(const cglib::ray3<double>& ray, double height, double& t) const {
        double a = cglib::norm(ray.direction);
        double b = 2.0 * cglib::dot_product(ray.direction, ray.origin);
        double c = cglib::norm(ray.origin) - (SPHERE_SIZE + height) * (SPHERE_SIZE + height);
        double d = b * b - 4.0 * a * c;
        if (d < 0.0) {
            return false;
        }
        t = (-b - std::sqrt(d)) / (2.0 * a);
        return true;
    }

    cglib::mat4x4<double> SphericalProjectionSurface::calculateLocalFrameMatrix(const cglib::vec3<double>& pos) const {
        cglib::mat3x3<double> localFrameMat3 = LocalFrame(pos * (1.0 / SPHERE_SIZE));
        cglib::mat4x4<double> localFrameMat4 = cglib::mat4x4<double>::identity();
        for (int i = 0; i < 3; i++) {
            cglib::vec3<double> dir = cglib::unit(cglib::col_vector(localFrameMat3, i));
            for (int j = 0; j < 3; j++) {
                localFrameMat4(j, i) = dir(j) * SPHERE_SIZE / Const::EARTH_RADIUS;
            }
            localFrameMat4(i, 3) = pos(i);
        }
        return localFrameMat4;
    }

    cglib::mat4x4<double> SphericalProjectionSurface::calculateTranslateMatrix(const cglib::vec3<double>& pos0, const cglib::vec3<double>& pos1, double t) const {
        double scale = (1 - t) + (cglib::length(pos1) / cglib::length(pos0)) * t;
        cglib::mat4x4<double> scaleMat = cglib::scale4_matrix(cglib::vec3<double>(scale, scale, scale));

        double dot = cglib::dot_product(cglib::unit(pos0), cglib::unit(pos1));
        double angle = std::acos(std::min(1.0, std::max(-1.0, dot)));
        cglib::vec3<double> axis = cglib::vector_product(cglib::unit(pos0), cglib::unit(pos1));
        if (cglib::length(axis) > 0) {
            if (angle < PLANAR_APPROX_ANGLE) { // less than 10m
                return cglib::translate4_matrix((cglib::unit(pos1) - cglib::unit(pos0)) * (SPHERE_SIZE * t)) * scaleMat;
            }
            return cglib::rotate4_matrix(axis, angle * t) * scaleMat;
        }
        return scaleMat;
    }

    bool SphericalProjectionSurface::SplitSegment(const MapPos& mapPos0, const MapPos& mapPos1, MapPos& mapPosM) {
        cglib::vec3<double> pos0 = cglib::unit(InternalToSpherical(mapPos0));
        cglib::vec3<double> pos1 = cglib::unit(InternalToSpherical(mapPos1));
        double dot = cglib::dot_product(pos0, pos1);
        if (dot <= -1) {
            return false; // exactly on the opposite side, no way to split
        }
        double angle = std::acos(std::min(1.0, std::max(-1.0, dot)));
        if (angle * Const::EARTH_RADIUS < SEGMENT_SPLIT_THRESHOLD) {
            return false;
        }
        mapPosM = SphericalToInternal(cglib::unit(pos0 + pos1));
        return true;
    }

    MapPos SphericalProjectionSurface::SphericalToInternal(const cglib::vec3<double>& pos) {
        double scale = Const::WORLD_SIZE / (2 * Const::PI);
        double len = cglib::length(pos);
        double rz = pos(2) / len;
        double ss = std::sqrt(std::max(0.0, 1.0 - rz * rz));
        double x1 = pos(0) != 0 || pos(1) != 0 ? std::atan2(pos(1), pos(0)) : 0;
        double y1 = std::atanh(std::max(-1.0, std::min(1.0, rz)));
        double z1 = (len - 1.0) / ss;
        return MapPos(x1 * scale, y1 * scale, z1 * scale);
    }

    cglib::vec3<double> SphericalProjectionSurface::InternalToSpherical(const MapPos& mapPos) {
        double scale = 2 * Const::PI / Const::WORLD_SIZE;
        double x1 = mapPos.getX() * scale;
        double y1 = mapPos.getY() * scale;
        double z1 = mapPos.getZ() * scale;
        double rz = std::tanh(y1);
        double ss = std::sqrt(std::max(0.0, 1.0 - rz * rz));
        double rx = ss * std::cos(x1);
        double ry = ss * std::sin(x1);
        double len = 1.0 + z1 * ss;
        return cglib::vec3<double>(rx, ry, rz) * len;
    }

    cglib::mat3x3<double> SphericalProjectionSurface::LocalFrame(const cglib::vec3<double>& pos) {
        double x = pos(0);
        double y = pos(1);
        double z = pos(2);

        double dx_du = -y;
        double dy_du = x;
        double dz_du = 0;

        double dx_dv = -z * x;
        double dy_dv = -z * y;
        double dz_dv = x * x + y * y;

        double dx_dh = x;
        double dy_dh = y;
        double dz_dh = z;

        return cglib::mat3x3<double> { { dx_du, dx_dv, dx_dh }, { dy_du, dy_dv, dy_dh }, { dz_du, dz_dv, dz_dh } };
    }

    const double SphericalProjectionSurface::SPHERE_SIZE = Const::WORLD_SIZE / Const::PI;

    const double SphericalProjectionSurface::PLANAR_APPROX_ANGLE = 1.0e-6;

    const double SphericalProjectionSurface::SEGMENT_SPLIT_THRESHOLD = Const::EARTH_CIRCUMFERENCE / 120.0;

}
<|endoftext|>


<commit_before>#include "types.h"
#include "stat.h"
#include "user.h"
#include "mtrace.h"
#include "amd64.h"
#include "uspinlock.h"
#include "pthread.h"

enum { readaccess = 0 };
enum { verbose = 0 };
enum { npg = 1 };

void*
thr(void *arg)
{
  u64 tid = (u64)arg;

  for (int i = 0; i < 1000000; i++) {
    if (verbose && ((i % 100) == 0))
      fprintf(1, "%d: %d ops\n", tid, i);

    volatile char *p = (char*) (0x40000UL + tid * npg * 4096);
    if (map((void *) p, npg * 4096) < 0) {
      fprintf(1, "%d: map failed\n", tid);
      exit();
    }

    if (readaccess) {
      for (int j = 0; j < npg * 4096; j++)
	p[j] = '\0';
    }

    if (unmap((void *) p, npg * 4096) < 0) {
      fprintf(1, "%d: unmap failed\n", tid);
      exit();
    }
  }
  return 0;
}

int
main(int ac, char **av)
{
  mtenable("xv6-mapbench");

  if (ac != 2) {
    fprintf(1, "usage: %s nthreads\n", av[0]);
    exit();
  }

  int nthread = atoi(av[1]);

  // fprintf(1, "mapbench[%d]: start esp %x, nthread=%d\n", getpid(), rrsp(), nthread);

  for(u64 i = 0; i < nthread; i++) {
    pthread_t tid;
    pthread_create(&tid, 0, thr, (void*) i);
    if (0) fprintf(1, "mapbench[%d]: child %d\n", getpid(), tid);
  }

  for(int i = 0; i < nthread; i++)
    wait();

  // fprintf(1, "mapbench[%d]: done\n", getpid());
  mtdisable("xv6-mapbench");
  // halt();
  exit();
}
<commit_msg>get rid of mtenable/mtdisable in mapbench<commit_after>#include "types.h"
#include "stat.h"
#include "user.h"
#include "amd64.h"
#include "uspinlock.h"
#include "pthread.h"

enum { readaccess = 0 };
enum { verbose = 0 };
enum { npg = 1 };

void*
thr(void *arg)
{
  u64 tid = (u64)arg;

  for (int i = 0; i < 1000000; i++) {
    if (verbose && ((i % 100) == 0))
      fprintf(1, "%d: %d ops\n", tid, i);

    volatile char *p = (char*) (0x40000UL + tid * npg * 4096);
    if (map((void *) p, npg * 4096) < 0) {
      fprintf(1, "%d: map failed\n", tid);
      exit();
    }

    if (readaccess) {
      for (int j = 0; j < npg * 4096; j++)
	p[j] = '\0';
    }

    if (unmap((void *) p, npg * 4096) < 0) {
      fprintf(1, "%d: unmap failed\n", tid);
      exit();
    }
  }
  return 0;
}

int
main(int ac, char **av)
{
  if (ac != 2) {
    fprintf(1, "usage: %s nthreads\n", av[0]);
    exit();
  }

  int nthread = atoi(av[1]);

  // fprintf(1, "mapbench[%d]: start esp %x, nthread=%d\n", getpid(), rrsp(), nthread);

  for(u64 i = 0; i < nthread; i++) {
    pthread_t tid;
    pthread_create(&tid, 0, thr, (void*) i);
    if (0) fprintf(1, "mapbench[%d]: child %d\n", getpid(), tid);
  }

  for(int i = 0; i < nthread; i++)
    wait();

  // fprintf(1, "mapbench[%d]: done\n", getpid());
  // halt();
  exit();
}
<|endoftext|>