<commit_before>/*
 * Copyright (c) 2008 <NAME> <<EMAIL>>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */


#include <assert.h>
#include <sstream>
#include <iostream>
#include <wcl/tracking/Polhemus.h>
#include <wcl/tracking/TrackedObject.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <sstream>

namespace wcl
{

	Polhemus::Polhemus(std::string path, TrackerType t) : Tracker(),  activeSensorCount(-1), type(t),continuous(false)
	{
		std::cout << "About to connect to " << path << std::endl;
		if (!connection.open(path.c_str(), Serial::BAUD_115200))
		{
			throw std::string(strerror(errno));
		}
		usleep(2000);
		connection.setBlockingMode(Serial::BLOCKING);
		setContinuous(false);
		usleep(2000);
		clearInput();
		// clear whatever existing half completed commands exist
		if (!connection.flush())
		{
			throw std::string(strerror(errno));
		}
		setAsciiOutput();
		setDataFormat();
		setUnits(MM);

		if (type == PATRIOT)
		{
			sensors.push_back(PolhemusTrackedObject("0"));
			sensors.push_back(PolhemusTrackedObject("1"));
			//getSensorCount();
		}
		else
		{
			sensors.push_back(PolhemusTrackedObject("0"));
			sensors.push_back(PolhemusTrackedObject("1"));
			sensors.push_back(PolhemusTrackedObject("2"));
			sensors.push_back(PolhemusTrackedObject("3"));
		}

		//setContinuous(true);
	}

	Polhemus::~Polhemus()
	{
		if (continuous)
			setContinuous(false);

		connection.close();
	}

	void Polhemus::clearInput()
	{
		int bytesAvailable = connection.getAvailableCount();
		char* rubbish = new char[bytesAvailable];
		connection.read((void*) rubbish, bytesAvailable);
		delete[] rubbish;
	}

	void Polhemus::update()
	{
		char response[70] = {0};
		double x, y, z, rx, ry, rz, rw;
		int number;
		int expectedBytes=0;
		int bytesRead = 0;
		if (type == PATRIOT)
			expectedBytes = 69;
		else if (type == FASTRAK)
			expectedBytes = 54;

		if (activeSensorCount > 0)
		{
			if (!continuous)
			{
				connection.write("P");
			}

			for (int i=0; i<activeSensorCount; i++)
			{
				if (connection.getAvailableCount() >= expectedBytes)
				{
					if (type == PATRIOT)
					{
						bytesRead = connection.read((void*) &response, 69);
						response[69] = '\0';
					}
					else if (type == FASTRAK)
					{
						bytesRead = connection.read((void*) &response, 54);
						response[54] = '\0';
					}

					if (bytesRead == expectedBytes)
					{
						//std::cout << "BytesRead: " << bytesRead << std::endl;
						//std::cout << "Reading Update Response: " << response;
						(void)sscanf(response, "%d%lf%lf%lf%lf%lf%lf%lf", &number, &x, &y, &z, &rw, &rx, &ry, &rz);
						if (units == MM)
						{
							sensors[number-1].update(x*10,y*10,z*10,rw,rx,ry,rz);
						}
						else
						{
							sensors[number-1].update(x,y,z,rw,rx,ry,rz);
						}
					}
					else
					{
						std::cout << "Did not read the right number of bytes, ignoring" << std::endl;
						connection.flush();
						clearInput();
					}
				}
			}
		}
		else
		{
			throw std::string("I don't know how many sensors there are!");
		}
	}

	TrackedObject* Polhemus::getObject(std::string name)
	{
		if (name == "sensor1" && activeSensorCount >= 1)<commit_msg>Fixed read buffer length in polhemus tracker class<commit_after>/*
 * Copyright (c) 2008 <NAME> <<EMAIL>>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */


#include <assert.h>
#include <sstream>
#include <iostream>
#include <wcl/tracking/Polhemus.h>
#include <wcl/tracking/TrackedObject.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <sstream>

namespace wcl
{

	Polhemus::Polhemus(std::string path, TrackerType t) : Tracker(),  activeSensorCount(-1), type(t),continuous(false)
	{
		std::cout << "About to connect to " << path << std::endl;
		if (!connection.open(path.c_str(), Serial::BAUD_115200))
		{
			throw std::string(strerror(errno));
		}
		usleep(2000);
		connection.setBlockingMode(Serial::BLOCKING);
	
		setContinuous(false);
		usleep(2000);
		// clear whatever existing half completed commands exist
		if (!connection.flush())
		{
			throw std::string(strerror(errno));
		}
		
		
		clearInput();
		setAsciiOutput();
		setDataFormat();
		setUnits(MM);


		clearInput();

		if (type == PATRIOT)
		{
			sensors.push_back(PolhemusTrackedObject("0"));
			sensors.push_back(PolhemusTrackedObject("1"));
			//getSensorCount();
		}
		else
		{
			sensors.push_back(PolhemusTrackedObject("0"));
			sensors.push_back(PolhemusTrackedObject("1"));
			sensors.push_back(PolhemusTrackedObject("2"));
			sensors.push_back(PolhemusTrackedObject("3"));
		}

		//setContinuous(true);
		//printf("connection.getAvailableCount init: %d\n",connection.getAvailableCount());
	}

	Polhemus::~Polhemus()
	{
		if (continuous)
			setContinuous(false);

		connection.close();
	}

	void Polhemus::clearInput()
	{
		connection.drain();
		connection.flush();
		int bytesAvailable = connection.getAvailableCount();
		char* rubbish = new char[bytesAvailable];
		connection.read((void*) rubbish, bytesAvailable);
		delete[] rubbish;
	}

	void Polhemus::update()
	{
		char response[70] = {0};
		double x, y, z, rx, ry, rz, rw;
		int number;
		int expectedBytes=0;
		int bytesRead = 0;
		if (type == PATRIOT)
			expectedBytes = 69;
		else if (type == FASTRAK)
			expectedBytes = 54;


		if (activeSensorCount > 0)
		{
			if (!continuous)
			{
				connection.write("P");
				connection.drain();
			}
			
			/*
			char response[140];
			memset(response, 0, 140);
			size_t sz = connection.getAvailableCount();
			connection.readUntil(response, 138);
			printf("%d:%s\n", sz, response);
			*/
	

			for (int i=0; i<activeSensorCount; i++)
			{
				connection.readUntil((void*) &response, expectedBytes);
				size_t sz = connection.getAvailableCount();
				response[expectedBytes] = '\0';
					
				//printf("%d %s\n", sz, response);
				
				int count = sscanf(response, "%d%lf%lf%lf%lf%lf%lf%lf", &number, &x, &y, &z, &rw, &rx, &ry, &rz);
				assert (number <= sensors.size());
				
				if (units == MM)
				{
					sensors[number-1].update(x*10,y*10,z*10,rw,rx,ry,rz);
				}
				else
				{
					sensors[number-1].update(x,y,z,rw,rx,ry,rz);
				}
				
			}
		}
		else
		{
			throw std::string("I don't know how many sensors there are!");
		}
	}

	TrackedObject* Polhemus::getObject(std::string name)
	{
		if (name == "sensor1" && activeSensorCount >= 1)
{'fraction_non_alphanumeric': 0.1335670662745985, 'fraction_numerical': 0.013232300384528387, 'mean_word_length': 3.8138268916712033, 'pattern_counts': {'":': 0, '<': 56, '<?xml version=': 0, '>': 32, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1659179', 'n_tokens_mistral': 3444, 'n_tokens_neox': 2792, 'n_words': 805}