/**
* @brief Read a ACK message from USB
*/
void *waitForACK()
{
uint8_t ack[10] = {0};
while (1) {
if (fd == -1)
break;
if (select(fd + 1, &set, NULL, NULL, &timeout) > 0) {
read(fd, &ack[0], ACK_SIZE);
ack_lock.lock();
ack_queue.push(ack);
ack_lock.unlock();
}
}
return NULL;
}
/**
* Push an element onto the queue. If the queue has a max size, this
* call will block if the queue is full.
*/
void push(T value) {
if (m_max_size) {
while (size() >= m_max_size) {
std::this_thread::sleep_for(full_queue_sleep_duration);
#ifdef OSMIUM_DEBUG_QUEUE_SIZE
++m_full_counter;
#endif
}
}
std::lock_guard<std::mutex> lock(m_mutex);
m_queue.push(std::move(value));
#ifdef OSMIUM_DEBUG_QUEUE_SIZE
if (m_largest_size < m_queue.size()) {
m_largest_size = m_queue.size();
}
#endif
m_data_available.notify_one();
}
// Thread to import data from the USRP !Size of the arrays in complex -> 2*buffer_size !
void usrpGetData(uhd::rx_streamer::sptr rx_stream,uhd::usrp::multi_usrp::sptr dev, size_t buffer_size){
// Set highest priority for this thread
set_realtime_priority();
// Create storage for a single buffer from USRP
short *buff_short;
buff_short=new short[2*buffer_size];
// Initialisation
size_t n_rx_last;
uhd::rx_metadata_t md;
//int time=buffer_size/(25)-100; // microsecondes
while (1){
n_rx_last=0;
// Fill buff_short
while (n_rx_last==0) {
n_rx_last=rx_stream->recv(&buff_short[0], buffer_size, md, 3.0);
std::this_thread::yield(); // Avoid active waiting
};
// Check if no overflow
if (n_rx_last!=buffer_size) {
std::cerr << "I expect the buffer size to be always the same!\n";
std::cout<<"Read only:"<<n_rx_last<<"\n";
std::cout<<"Buffer:"<<buffer_size<<"\n";
//exit(1);
};
// Add the just received buffer to the queue
mtxUsrp.lock();
usrpQ.push(buff_short);
mtxUsrp.unlock();
// Change memory cell used
buff_short=new short [2*buffer_size];
// Gives the start to detection part
sem_post( &usrpReady);
std::this_thread::sleep_for(std::chrono::microseconds(5));
}//end while 1
}
//BBMSP_API bbmsp_result_t bbmsp_contact_copy(bbmsp_contact_t* destination, const bbmsp_contact_t* source);
FREObject bbm_ane_bbmsp_contact_copy(FREContext ctx, void* functionData, uint32_t argc, FREObject argv[]) {
uint32_t imageID;
contact_data_s *contactData;
FREGetObjectAsUint32(argv[0],&imageID);
contactData = (contact_data_s *)malloc( sizeof(contact_data_s) );
contactData->id = imageID;
contactData->action = COPY_CONTACT;
contactData->id2 = rand();
pthread_mutex_lock(&contactMutex);
contactQueue.push(contactData);
pthread_mutex_unlock(&contactMutex);
FREObject result;
FRENewObjectFromUint32(contactData->id2, &result);
return result;
}
int main()
{
std::ifstream input(BINARY_DIR"/input.txt", std::ios::binary);
SyncClass sc;
for(int i = 0;i < THREADS_COUNT; i++)
threads.create_thread(boost::bind(&SyncClass::writeMessage, &sc) );
while(!input.eof()){
Message msg(input);
if(input.eof())
break;
boost::mutex::scoped_lock lock(queueMutex);
msgQueue.push(msg);
}
sc.stopWorking();
threads.join_all();
return 0;
}
int push(const T &item, CThread *thr, unsigned int timeoutMsec)
{
std::chrono::duration<unsigned int, std::milli> duration(timeoutMsec);
std::unique_lock<std::mutex> lock(m_mutex);
while(isFull())
{
m_condNotEmpty.wait_for(lock, duration);
if (thr->isTerminated()) return -1;
}
if (thr->isTerminated()) return -1;
m_rwmut.lock();
m_que.push(item);
m_rwmut.unlock();
m_condNotFull.notify_all();
return 0;
}
void producer()
{
int i = 0;
while (1)
{
boost::this_thread::sleep(boost::posix_time::millisec(1000));
boost::mutex::scoped_lock lock(m);
while (itemQ.size() > max_size_q)
{
std::cout << "Q Full.notify_one Producer Waiting" << std::endl;
qFull.wait(lock);
std::cout << "Producer Notified to Continue" << std::endl;
}
itemQ.push(++i);
qEmpty.notify_one();
}
}
void CPluginTransportTCP::handleAsyncResolve(const boost::system::error_code & err, boost::asio::ip::tcp::resolver::iterator endpoint_iterator)
{
if (!err)
{
boost::asio::ip::tcp::endpoint endpoint = *endpoint_iterator;
m_Socket->async_connect(endpoint, boost::bind(&CPluginTransportTCP::handleAsyncConnect, this, boost::asio::placeholders::error, ++endpoint_iterator));
}
else
{
delete m_Resolver;
m_Resolver = NULL;
delete m_Socket;
m_Socket = NULL;
// _log.Log(LOG_ERROR, "Plugin: Connection Exception: '%s' connecting to '%s:%s'", err.message().c_str(), m_IP.c_str(), m_Port.c_str());
ConnectedMessage* Message = new ConnectedMessage(m_HwdID, err.value(), err.message());
boost::lock_guard<boost::mutex> l(PluginMutex);
PluginMessageQueue.push(Message);
}
}
void push(const hasher::job_t* const job) {
while(true) {
// get job queue size
lock();
size_t size = job_queue.size();
unlock();
if (size < max_queue_size) {
// add job to queue now
lock();
job_queue.push(job);
unlock();
break;
} else {
// try again later
sched_yield();
}
}
}
// Add an action to the queue
long queueAction(Action action) {
// Timeout for a lock on queue
::timeval i,e;
::gettimeofday(&i,NULL);
while(queue_lock) {
::gettimeofday(&e,NULL);
// 100 ms timeout
if((e.tv_usec-i.tv_usec) > 100000) {
return -1;
}
};
// Add an action to the queue
queue_lock = true;
actionQueue.push(action);
queue_lock = false;
// Set id to id_counter
action.id = id_counter;
id_counter++;
return id_counter;
}
static int audio_delivery(sp_session *sess, const sp_audioformat *format,
const void *frames, int num_frames)
{
SDL_mutexP(g_audio_mutex);
if (num_frames == 0)
{
SDL_mutexV(g_audio_mutex);
return 0;
}
if (g_audio_queue.size() > MAX_REQUEST_BUFFER_COUNT)
{
SDL_mutexV(g_audio_mutex);
return 0;
}
assert(format->sample_rate == SDL_AUDIO_SAMPLE_RATE);
assert(format->channels == SDL_AUDIO_CHANNELS);
assert(format->sample_type == SP_SAMPLETYPE_INT16_NATIVE_ENDIAN);
//
// Push SDL_AUDIO_BUFFER_FRAMES to the audio layer even if `num_frames`
// delievered by Spotify is less than this (worst case is 1 second silence
// if num_frames = 1 since we feed SDL chunks of 1 second audio at a time).
//
// NOTE: Abusing the memory allocator like this should be pretty OK since
// we're basically re-using the same chunks all over again. Yeah I'm lazy.'
//
{
uint8_t* chunk = new uint8_t[SDL_AUDIO_REQUEST_SIZE];
memset(chunk, 0, SDL_AUDIO_REQUEST_SIZE);
memcpy(chunk, frames, num_frames * SDL_AUDIO_SAMPLE_SIZE * SDL_AUDIO_CHANNELS);
g_audio_queue.push(chunk);
}
SDL_mutexV(g_audio_mutex);
return SDL_AUDIO_BUFFER_FRAMES;
}
void SteerLib::GJK_EPA::UpdateEars(std::queue<Util::Vector>& ears, std::vector<struct edges> edges, std::vector<Util::Vector> shape)
{
for (int i = 0; i < shape.size(); i++) {
Util::Vector curr = shape[i];
Util::Vector n1, n2, testpoint;
for (int j = 0; j < edges.size(); j++) {
if (edges[j].point == curr) {
n1 = edges[j].neighbor1;
n2 = edges[j].neighbor2;
break;
}
}
// Get midpoint of the two neighbors
testpoint = Midpoint(n1, n2);
// Test the midpoint
// If it lies within the polygon, it's good
bool result = false;
int k, j;
for (k = 0, j = shape.size() - 1; k < shape.size(); j = k++) {
if ((shape[k].z > testpoint.z) != (shape[j].z > testpoint.z) && (testpoint.x < (shape[j].x - shape[k].x) * (testpoint.z - shape[k].z) / (shape[j].z - shape[k].z) + shape[k].x)) {
result = !result;
}
}
// TODO ONLY IF THEY'RE GONNA HAVE OTHER TEST CASES
// Need to do check if point is on an edge
// If midpoint is a point on shape, it's not an ear
if (std::find(shape.begin(), shape.end(), testpoint) != shape.end())
result = false;
if (result) {
if(!EarCheck(ears, shape[i]))
ears.push(shape[i]);
}
}
}
void DataLibrary::split(std::string& s, std::string c,std::queue<std::string>& v)
{
/*std::string::size_type i = 0;
std::string::size_type j = s.find(c);
while (j != std::string::npos)
{
v.push(s.substr(i, j-i));
i = ++j;
j = s.find(c, j);
if (j == std::string::npos)
v.push(s.substr(i, s.length( )));
}*/
std::vector<std::string> a;
boost::algorithm::split(a,s, boost::is_any_of(c));
for(std::vector<std::string>::iterator it=a.begin();it!=a.end();it++)
{
v.push((*it));
}
}
void BlockDevRequest::transfer ( const BlockDevOp op, const uint32_t dramaddr, const uint32_t offset, const uint32_t size, const uint32_t tag )
{
if (0) fprintf(stderr, "BlockDevRequest::transfer op=%x dramaddr=%x offset=%x size=%x tag=%x\n", op, dramaddr, offset, size, tag);
if (fpgaDev->mainMemBuf) {
if (op == BlockDevRead) {
int numBytes = pread(driveFd, fpgaDev->mainMemBuf + dramaddr, size, offset);
if (numBytes != (long)size)
fprintf(stderr, "Read error size=%d numBytes=%d errno=%d:%s\n", size, numBytes, errno, strerror(errno));
} else {
int numBytes = pwrite(driveFd, fpgaDev->mainMemBuf + dramaddr, size, offset);
if (numBytes != (long)size)
fprintf(stderr, "Read error size=%d numBytes=%d errno=%d:%s\n", size, numBytes, errno, strerror(errno));
}
}
{
std::lock_guard<std::mutex> lock(rqmutex);
responseQueue.push(tag);
sem_post(&worker_sem);
}
}
void match_callback(const cyphy_vslam_msgs::MatchConstPtr &msg)
{
image_cache::GetImage srv;
srv.request.from = msg->fromImgSeq;
srv.request.to.clear();
for(unsigned int i=0; i < msg->toImgSeq.size(); i++)
{
m_proposed_++;
ROS_INFO("Verifying FAB-MAP match %d to %d (republished index) ...",msg->fromImgSeq,msg->toImgSeq[i]);
if( (msg->fromImgSeq-msg->toImgSeq[i]) < window_frame_size_)
{
ROS_INFO("Failed, match is within local window");
continue;
}
srv.request.to.push_back(msg->toImgSeq[i]);
}
srv.request.rectified = 1;
boost::mutex::scoped_lock lock(m_mutex_); // Don't touch the service when I'm adding to it
if(!srv.request.to.empty())
srv_queue_.push(srv);
}
/** read files and store data into proper vars */
void read_and_store(){
//spcial case read first line, if rr then tokenize and get quantum
std::string line;
std::getline(in, line);
std::istringstream iss(line);
if(!(iss >> alg.name >> alg.time_quantum))
iss >> alg.name;
//get 2nd line
in >> alg.number_of_proc;
//get process info
int i;
for(i=0; i<alg.number_of_proc; ++i){
struct Qu t = createQu();
in >> t.proc_num >> t.arrival_time >> t.cpu_burst >> t.priority;
t.total_burst = t.cpu_burst;
master.push(t);
}
}
void *cap(void *threadid) {
std::cout << "\n\n\n\tA Handsfree E-Reading Apparatus\n\t\t Group L02\n\n\n";
capture >> tempCap;
while (1) {
Mat frame;
capture >> frame;
resize(frame, frame, Size(GLOBAL_COLS, GLOBAL_ROWS), 0, 0, INTER_CUBIC);
pthread_mutex_lock(&mutex);
capBuffer.push(frame);
pthread_mutex_unlock(&mutex);
sem_post(s1);
if (bufIndex == 250) {
printf("buf getting full...\n");
}
usleep(100000); //increase delay if computer can not keep
//usleep (160000);
//return 0;
}
}
void collect(std::queue< std::pair< size_t, objVars_t> >& results ){
int evals = ( (requests.size() >= EVALS_AT_ONCE) ? (EVALS_AT_ONCE) : requests.size() );
std::vector< req > v( requests.begin(), requests.begin() + evals);
requests.erase( requests.begin(), requests.begin() + evals );
//std::random_shuffle( v.begin(), v.end() );
typename std::vector< req >::iterator it;
for(it=v.begin(); it!=v.end(); it++){
objVars_t r( this->P.size() );
int i;
for(i=0; i<this->P.size(); i++){
r[i] = (this->P[i])( it->x );
}
std::pair< size_t, objVars_t > p( it->id, r );
results.push( p );
}
}
void
GraphTools::__BFS( std::queue< raft::kernel* > &queue,
std::set< raft::kernel* > &visited_set,
vertex_func func,
void *data )
{
while( queue.size() > 0 )
{
auto *source( queue.front() );
if( source == nullptr ) break;
queue.pop();
/** iterate over all out-edges **/
/** 1) get lock **/
while( ! source->output.portmap.mutex_map.try_lock() )
{
std::this_thread::yield();
}
/** 2) get map **/
std::map< std::string, PortInfo > &map_of_ports( source->output.portmap.map );
/** 3) visit kernel **/
func( source, data );
/** 4) add children to queue **/
for( auto &port : map_of_ports )
{
PortInfo &source( port.second );
/** get dst edge to call function on **/
if( source.other_kernel != nullptr )
{
/** if the dst kernel hasn't been visited, visit it **/
if( visited_set.find( source.other_kernel ) == visited_set.end() )
{
queue.push( source.other_kernel );
visited_set.insert( source.other_kernel );
}
}
}
source->output.portmap.mutex_map.unlock();
}
return;
}
void LeeBacktrace()
{
//Some housekeeping so that nothing bad carries over.
if (!working_set.empty())
{
while (!working_set.empty())
working_set.pop();
}
int CurrentWeight = 0;
Coords Neighbour;
//Starting from Finish, add all the path nodes leading to goal
ActiveCoords = FinishCoords;
CurrentWeight = Map[FinishCoords.x][FinishCoords.y].getWeight();
while (!((ActiveCoords.x == StartCoords.x) && (ActiveCoords.y == StartCoords.y)))
{
FindNeighbours();
do
{
Neighbour = neighbour_set.front();
neighbour_set.pop();
if ((Neighbour.x == StartCoords.x) && (Neighbour.y == StartCoords.y))
break;
}
while (Map[Neighbour.x][Neighbour.y].getWeight() > CurrentWeight);
CurrentWeight = Map[Neighbour.x][Neighbour.y].getWeight();
path.push(Neighbour);
ActiveCoords = Neighbour;
while(!neighbour_set.empty())
neighbour_set.pop();
}
state = 3;
}
bool visitBFS(int node, bool firstGroup, std::vector<std::vector<int>>& graph,
std::pair<std::unordered_set<int>, std::unordered_set<int>>& groups,
std::queue<std::pair<int, bool>>& toVisit) {
// Check if node was visited and is already in correct group
if (firstGroup) {
if (groups.first.find(node) != groups.first.end()) return true;
else if (groups.second.find(node) != groups.second.end()) return false;
} else {
if (groups.second.find(node) != groups.second.end()) return true;
else if (groups.first.find(node) != groups.first.end()) return false;
}
// If node was not visited then put it in the correct group
if (firstGroup) groups.first.insert(node);
else groups.second.insert(node);
// Put neigbors
for (int i = 0; i < graph[node].size(); i++) {
toVisit.push(std::make_pair(graph[node][i], !firstGroup));
}
return true;
}
bool remove_nodes_degree_0_1(Graph& graph, std::queue<Graph::Node>& potential_nodes, bool store) {
bool didSomething = false;
while(!potential_nodes.empty()) {
Graph::Node node = potential_nodes.front();
potential_nodes.pop();
if( !graph.isEnabled(node) )
continue;
if (graph.degree(node) == 0) {
didSomething = true;
if( store ) {
graph.disableStore(node);
} else {
graph.disable(node);
}
} else if (graph.degree(node) == 1) {
didSomething = true;
potential_nodes.push(graph.oppositeNode(node, Graph::IncEdgeIt(graph, node)));
if( store ) {
graph.disableStore(node);
} else {
graph.disable(node);
}
// } else if (graph.degree(node) == 2 && graph.degreeUnique(node) == 2) {
// Graph::Node u, v;
// bool wasAlreadyDouble;
//// Graph::Node u = Graph::NeighborIt(graph,node);
//// Graph::NeighborIt v(graph,node);
//// ++v;
//// graph.disable(node);
//// graph.increaseAdjacency(u,v);
//// std::tie(u,v,wasAlreadyDouble) = graph.contractTwo(node);
//// potential_nodes.push(u);
//// potential_nodes.push(v);
// //if( u == v ) throw 0;
}
}
return didSomething;
}
/**
* Submit an buffer to the IPC queue.
*/
void
ipcDriverKernelSubmitRequest(IPCBuffer *buffer)
{
switch (cpu::this_core::id()) {
case 0:
buffer->cpuId = IOSCpuId::PPC0;
break;
case 1:
buffer->cpuId = IOSCpuId::PPC1;
break;
case 2:
buffer->cpuId = IOSCpuId::PPC2;
break;
default:
decaf_abort("Unexpected core id");
}
sIpcMutex.lock();
sIpcRequests.push(buffer);
sIpcCond.notify_all();
sIpcMutex.unlock();
}
int LabyrinthWave::FindWay ()
{
if (start_.x < 0)
{
printf ("Can't find start\n");
return -1;
}
queueOfVertex_.push (start_);
visits_ [start_.x * input_.cols + start_.y] = 0;
bool isFindFinish = 0;
while ((!queueOfVertex_.empty ()) && (!isFindFinish))
{
Point curr = queueOfVertex_.front ();
queueOfVertex_.pop ();
/// Vertical, horizontal
isFindFinish |= Wave (curr, Point (-1, 0));
isFindFinish |= Wave (curr, Point ( 0, 1));
isFindFinish |= Wave (curr, Point ( 1, 0));
isFindFinish |= Wave (curr, Point ( 0, -1));
/// Diagonal
isFindFinish |= Wave (curr, Point (-1, -1));
isFindFinish |= Wave (curr, Point (-1, 1));
isFindFinish |= Wave (curr, Point ( 1, -1));
isFindFinish |= Wave (curr, Point ( 1, 1));
}
if (!isFindFinish)
{
printf ("Can't find finish\n");
return -1;
}
return 0;
}
int get_image(cv::Mat& im, const cv::Mat& mapping, bool rotate, int socket,
const std::string& fname, int& ncount)
{
int nframes = 0;
/* empty the acquired frame buffer */
while (acq_frame_buffer.size() > 0) {
int width = acq_frame_buffer.front().width;
int height = acq_frame_buffer.front().height;
double timestamp = acq_frame_buffer.front().timestamp;
#ifndef LICKOMETER
if (fname != "") {
std::ostringstream jpgname;
jpgname << fname << std::setfill('0') << std::setw(7) << ncount << ".jpg";
pthread_mutex_lock( &save_buffer_mutex );
save_frame_buffer.push(saveframe(acq_frame_buffer.front().data,
width, height, timestamp, jpgname.str()));
pthread_mutex_unlock( &save_buffer_mutex );
}
#endif
pthread_mutex_lock( &acq_buffer_mutex );
im = cv::Mat(cv::Size(width, height), CV_8UC1, &acq_frame_buffer.front().data[0]).clone();
if (rotate) {
cv::Mat cpim = im.clone();
cv::warpAffine(cpim, im, mapping, im.size());
}
acq_frame_buffer.pop();
pthread_mutex_unlock( &acq_buffer_mutex );
if (fname != "") {
write_send(socket, timestamp, fname, ncount);
}
nframes++;
}
return nframes;
}
bool CPluginTransportTCP::handleConnect()
{
try
{
if (!m_Socket)
{
m_bConnected = false;
m_Resolver = new boost::asio::ip::tcp::resolver(ios);
m_Socket = new boost::asio::ip::tcp::socket(ios);
boost::system::error_code ec;
boost::asio::ip::tcp::resolver::query query(m_IP, m_Port);
boost::asio::ip::tcp::resolver::iterator iter = m_Resolver->resolve(query);
boost::asio::ip::tcp::endpoint endpoint = *iter;
//
// Async resolve/connect based on http://www.boost.org/doc/libs/1_45_0/doc/html/boost_asio/example/http/client/async_client.cpp
//
m_Resolver->async_resolve(query, boost::bind(&CPluginTransportTCP::handleAsyncResolve, this, boost::asio::placeholders::error, boost::asio::placeholders::iterator));
if (ios.stopped()) // make sure that there is a boost thread to service i/o operations
{
ios.reset();
_log.Log(LOG_NORM, "PluginSystem: Starting I/O service thread.");
boost::thread bt(boost::bind(&boost::asio::io_service::run, &ios));
}
}
}
catch (std::exception& e)
{
// _log.Log(LOG_ERROR, "Plugin: Connection Exception: '%s' connecting to '%s:%s'", e.what(), m_IP.c_str(), m_Port.c_str());
ConnectedMessage* Message = new ConnectedMessage(m_HwdID, -1, std::string(e.what()));
boost::lock_guard<boost::mutex> l(PluginMutex);
PluginMessageQueue.push(Message);
return false;
}
return true;
}
void push_queue( std::queue< query_t* >& q, indri::api::Parameters& queries,
int queryOffset ) {
for( size_t i=0; i<queries.size(); i++ ) {
std::string queryNumber;
std::string queryText;
std::string queryType = "indri";
if( queries[i].exists( "type" ) )
queryType = (std::string) queries[i]["type"];
if( queries[i].exists( "number" ) ) {
queryText = (std::string) queries[i]["text"];
queryNumber = (std::string) queries[i]["number"];
} else {
queryText = (std::string) queries[i];
int thisQuery=queryOffset + int(i);
std::stringstream s;
s << thisQuery;
queryNumber = s.str();
}
q.push( new query_t( i, queryNumber, queryText, queryType ) );
}
}
/**
*
* Adds a cell to the queue cellQueue, but only if it is passable
* and has not been visited until now.
* The function also marks the cell as visited.
*
*/
void BfsSolver::AddIfPassableAndNotVisited(std::queue<Cell*> & cellQueue, Cell* pCell)
{
std::cout << " Should we add ";
pCell->PrintInfo();
std::cout << "? ...";
if(pCell && pCell->IsPassable() && ! pCell->IsVisited())
{
std::cout << "Yes";
// Mark the cell as visited
pCell->MarkVisited();
// Enqeue the cell
cellQueue.push(pCell);
}
else
{
std::cout << "No";
}
std::cout << std::endl;
}
void rcvMessage()
{
msgBuff msg;
int msg_ret;
// receive message of QUERY_ID type
msg_ret = msgrcv(msqid, &msg, sizeof(msg) - sizeof(long), QUERY_ID, 0);
// exit if there was an error
if(msg_ret <1)
{
return;
}
// lock message_list_mutex
pthread_mutex_lock(&msg_list_mutex);
// wakeup record searching thread
pthread_cond_signal(&pool_cond);
// push received message onto message_list
message_list.push(msg);
// unlock message_list_mutex
pthread_mutex_unlock(&msg_list_mutex);
}
inline
void *semaphore_thread_producer(void *arg)
{
struct thread_info *tinfo = (struct thread_info *) arg;
printf("Semaphore producer thread id:%d staring\n", tinfo->num);
int i = 0;
while(i < 30)
{
int num = rand();
sleep(1);
while(locked) sem_wait(&lock);
locked = true;
printf("Semaphore producer thread id:%d produced: %d\n", tinfo->num, num);
jobs.push(num);
locked = false;
sem_post(&lock);
++ i;
}
printf("Semaphore producer thread id:%d ending\n", tinfo->num);
return NULL;
}
