/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#ifndef _XSENS_LIST_HPP_2006_06_08
#define _XSENS_LIST_HPP_2006_06_08

#ifndef _XSENS_LIST_H_2006_06_08
#	include "xsens_list.h"
#endif

#ifdef _XSENS_LIST_IO
#	include <iostream>
#endif

/* Jerome Monceaux : bilock@gmail.com
 * Add a specific case for apple
 */
#ifdef __APPLE__
# include <malloc/malloc.h>
#else
# include <malloc.h>
#endif

#include <stdlib.h>
#include <string.h>

namespace xsens {

#define CMT_LIST_LINEAR_SEARCH_TRESHOLD	10

template <typename T>
List<T>::List()
{
	m_max = 16;
	m_count = 0;
	m_data = (T*) malloc(m_max * sizeof(T));
//	_ASSERT((void*) m_data != (void*) 0x00392E90);
	m_jcf = NULL;
	m_manage = true;
}

template <typename T>
List<T>::List(uint32_t size)
{
	m_max = size;
	if (m_max == 0)
		m_max = 1;
	m_count = 0;
	m_data = (T*) malloc(m_max * sizeof(T));
	m_jcf = NULL;
	m_manage = true;
}

template <typename T>
List<T>::List(const List<T>& src)
{
	m_max = src.m_max;
	if (m_max == 0)
		m_max = 1;
	m_count = src.m_count;
	m_data = (T*) malloc(m_max * sizeof(T));
	m_jcf = NULL;
	if (m_count > 0)
		memcpy(m_data,src.m_data,m_count*sizeof(T));
	m_manage = true;
}

template <typename T>
List<T>::List(const uint32_t size, const T* src)
{
	m_max = size;
	if (m_max == 0)
		m_max = 1;
	m_count = size;
	m_data = (T*) malloc(m_max * sizeof(T));
	m_jcf = NULL;
	if (m_count > 0)
		memcpy(m_data,src,m_count * sizeof(T));
	m_manage = true;
}

template <typename T>
List<T>::List(const uint32_t size, T* src, bool manage)
{
	m_max = size;
	m_count = size;
	m_data = src;
	m_jcf = NULL;
	m_manage = manage;
}

template <typename T>
List<T>::~List()
{
	if (m_jcf != NULL)
		delete m_jcf;
	if (m_manage && m_data != NULL)
		free(m_data);
	m_jcf = NULL;
	m_data = NULL;
}

template <typename T>
void List<T>::deleteAndClear(void)
{
	for (unsigned i=0;i<m_count;++i)
		delete m_data[i];
	m_count = 0;
}

template <typename T>
void List<T>::freeAndClear(void)
{
	for (unsigned i=0;i<m_count;++i)
		free(m_data[i]);
	m_count = 0;
}

template <typename T>
void List<T>::clear(void)
{
	m_count = 0;
}

template <typename T>
void List<T>::resize(uint32_t newSize)
{
	if (m_manage)
	{
		if (newSize == m_max)
			return;
		if (m_count > newSize)
			m_max = m_count;
		else
			m_max = newSize;
		if (m_max == 0)
			m_max = 1;	// size 0 is not allowed

		m_data = (T*) realloc(m_data,m_max * sizeof(T));
	}
}

template <typename T>
void List<T>::append(const T& item)
{
	if (m_count == m_max)
		resize(m_max + 1 + m_max/2);
	m_data[m_count++] = item;
}

template <typename T>
void List<T>::appendList(uint32_t count, const T* lst)
{
	if (m_count+count > m_max)
		resize(m_max + count + m_max/2);
	for (unsigned i = 0; i < count; ++i)
		m_data[m_count++] = lst[i];
}

template <typename T>
void List<T>::appendDeepCopy(const List<T>& source)
{
	if (m_max < source.m_count + m_count)
		resize(source.m_count + m_count);

	for (uint32_t i = 0;i<source.m_count;++i)
		m_data[m_count++] = new T(*source.m_data[i]);
}

template <typename T>
void List<T>::appendShallowCopy(const List<T>& source)
{
	if (m_max < source.m_count + m_count)
		resize(source.m_count + m_count);

	for (uint32_t i = 0;i<source.m_count;++i)
		m_data[m_count++] = source.m_data[i];
}

template <typename T>
template <typename TB>
void List<T>::appendCopy(const TB& item)
{
	if (m_count == m_max)
		resize(m_max + 1 + m_max/2);
	m_data[m_count++] = new TB(item);
}

template <typename T>
template <typename TR>
void List<T>::appendRelated(const TR& item)
{
	if (m_count == m_max)
		resize(m_max + 1 + m_max/2);
	m_data[m_count++] = item;
}

template <typename T>
T& List<T>::last(void) const XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (m_count == 0)
			throw "List.last: empty list";
	#endif
	return m_data[m_count-1];
}

template <typename T>
T& List<T>::get(const uint32_t index) const XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (index >= m_count)
			throw "List.get: index out of bounds";
	#endif
	if (index >= m_count)
		return m_data[m_count-1];
	return m_data[index];
}

template <typename T>
void List<T>::insert(const T& item, const uint32_t index)
{
	if (m_count == m_max)
		resize(1 + m_max + (m_max >> 1));
	for (unsigned i=m_count;i>index;--i)
		m_data[i] = m_data[i-1];
	if (index <= m_count)
		m_data[index] = item;
	else
		m_data[m_count] = item;
	m_count++;
}

template <typename T>
template <typename TB>
void List<T>::insertCopy(const TB& item, const uint32_t index)
{
	if (m_count == m_max)
		resize(1 + m_max + (m_max >> 1));
	for (unsigned i=m_count;i>index;--i)
		m_data[i] = m_data[i-1];
	if (index <= m_count)
		m_data[index] = new TB(item);
	else
		m_data[m_count] = new TB(item);
	m_count++;
}

template <typename T>
T& List<T>::operator [] (const uint32_t index) const XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (index >= m_count)
			throw "List[]: index out of bounds";
	#endif
	return m_data[index];
}

#if defined(_XSENS_LIST_WITH_MATH) && defined(_XSENS_LIST_IO)

template <typename T>
std::ostream& operator << (std::ostream& os, List<T>& t)
{
	os << '[' << t.length() << "]{ ";
	for (unsigned i=0 ; i<t.length() ; ++i)
		os << t[i] << " ";
	os << '}';
	return os;
}

#ifndef _CMTMATLABHEADERS
#define _CMTMATLABHEADERS
	struct MatlabFileHeader {
		char	description[116];
		int32_t	data_offset1;
		int32_t	data_offset2;
		int16_t	version;
		int16_t	endian;
	};

	struct MatlabDataHeader {
		int32_t data_type;
		int32_t n_bytes;
	};

	struct MatlabMatrixHeader {
		int32_t flags_data_type;
		int32_t flags_data_size;
		int32_t flags0,flags1;

		int32_t dimensions_data_type;
		int32_t dimensions_data_size;
		int32_t dimensions_m, dimensions_n;

		int32_t name_type;
		int32_t name_length;
	};
#endif

//! save list of vectors as matrix in ".mat" file for MATLAB
template <typename T>
void List<T>::saveAsMatlab(const char* filename, const char *varname) const
{
	// new header
	
	size_t i, j;
	MatlabFileHeader file_header;
	MatlabMatrixHeader matrix_header;
	int32_t name_pad;
	MatlabDataHeader inner_header;
	MatlabDataHeader outer_header;

	FILE* fp = fopen(filename,"wb");
	if (fp == NULL)
		return;

	strcpy(file_header.description,"cmtMath/Xsens");
	for (i = strlen(file_header.description); i <116; ++i)
		file_header.description[i] = ' ';
	file_header.data_offset1 = 0;
	file_header.data_offset2 = 0;
	file_header.version = 0x0100;
	file_header.endian = (int16_t) 'M' << 8 | 'I';

	//matlab  mat;

	matrix_header.flags_data_type = 6;
	matrix_header.flags_data_size = 8;
	matrix_header.flags0 = 6;	// mxDOUBLE_CLASS (double precision array)
	matrix_header.flags1 = 0;
	matrix_header.dimensions_data_type = 5;
	matrix_header.dimensions_data_size = 8;
	matrix_header.dimensions_m = m_count;
	matrix_header.dimensions_n = m_data[0]->size();
	matrix_header.name_type = 1;
	
	if ( varname == (char *)NULL )
		matrix_header.name_length = 0;
	else
		matrix_header.name_length = (int32_t) strlen(varname);

	name_pad = matrix_header.name_length & 7;

	inner_header.data_type = 9;	// double
	inner_header.n_bytes = sizeof(double) * m_count * matrix_header.dimensions_n;

	outer_header.data_type = 14;
	outer_header.n_bytes = sizeof(matrix_header) + matrix_header.name_length + name_pad
						+ sizeof(MatlabDataHeader) + inner_header.n_bytes;
	
	fwrite((char*) &file_header,sizeof(MatlabFileHeader),1,fp);
	fwrite((char*) &outer_header,sizeof(MatlabDataHeader),1,fp);
	fwrite((char*) &matrix_header,sizeof(MatlabMatrixHeader),1,fp);
	fwrite(varname,sizeof(char),matrix_header.name_length,fp);

	if (name_pad != 0)
		fwrite("\0\0\0\0\0\0\0",sizeof(char),8-name_pad,fp);

	fwrite((char*) &inner_header,sizeof(MatlabDataHeader),1,fp);

	// write actual data
	// column major order: ORDER == COL_ORDER
	double tmp;
	for ( j = 0; j < (size_t) matrix_header.dimensions_n; j++ )
		for ( i = 0; i < m_count; i++ )
		{
			tmp = (*m_data[i])[(unsigned)j];
			fwrite(&tmp,sizeof(double),1,fp);
		}

	fclose(fp);
}
#endif	// _XSENS_LIST_WITH_MATH && _XSENS_LIST_IO

template <typename T>
void List<T>::qSort(uint32_t left, uint32_t right)
{
	uint32_t l_hold, r_hold;
	T pivot;

	l_hold = left;
	r_hold = right;
	pivot = m_data[left];
	while (left < right)
	{
		while (!(m_data[right] < pivot) && (left < right))
			right--;
		if (left != right)
		{
			m_data[left] = m_data[right];
			left++;
		}
		while (!(pivot < m_data[left]) && (left < right))
			left++;
		if (!(left == right))
		{
			m_data[right] = m_data[left];
			right--;
		}
	}
	m_data[left] = pivot;
	if (l_hold < left)
		qSort(l_hold, left-1);
	if (r_hold > left)
		qSort(left+1, r_hold);
}

template <typename T>
void List<T>::qSortDeref(uint32_t left, uint32_t right)
{
	uint32_t l_hold, r_hold;
	T pivot;

	l_hold = left;
	r_hold = right;
	pivot = m_data[left];
	while (left < right)
	{
		while (!(*m_data[right] < *pivot) && (left < right))
			right--;
		if (left != right)
		{
			m_data[left] = m_data[right];
			left++;
		}
		while (!(*pivot < *m_data[left]) && (left < right))
			left++;
		if (!(left == right))
		{
			m_data[right] = m_data[left];
			right--;
		}
	}
	m_data[left] = pivot;
	if (l_hold < left)
		qSortDeref(l_hold, left-1);
	if (r_hold > left)
		qSortDeref(left+1, r_hold);
}

//#define XSENS_LIST_QSORT
//#define XSENS_LIST_COMBSORT
#define XSENS_LIST_JOBSORT
// when nothing is defined, bubble sort is used

template <typename T>
void List<T>::sortAscending(void)
{
	if (m_count <= 1)
		return;
#if defined(XSENS_LIST_QSORT)
	qSort(0,m_count-1);
#elif defined(XSENS_LIST_COMBSORT)

	uint32_t gap = m_count;
	double dgap;
	uint32_t swaps = 0;
	T temp;

	while (gap > 1 || swaps > 0)
	{
		if (gap > 1)
		{
			dgap = floor(((double) gap) / 1.247330950103979);
			gap = (uint32_t) dgap;
			if (gap == 10 || gap == 9)
				gap = 11;
		}

		uint32_t gappedCount = m_count-gap;
		swaps = 0;
		for (uint32_t i = 0; i < gappedCount; ++i)
		{
			if (m_data[i] > m_data[i+gap])
			{
				temp = m_data[i];
				m_data[i] = m_data[i+gap];
				m_data[i+gap] = temp;
				++swaps;
			}
		}
	}
#elif defined(XSENS_LIST_JOBSORT)
	struct Linker {
		Linker *prev, *next;
		uint32_t index;

		T item;
	};

	Linker* list = (Linker*) malloc(m_count*sizeof(Linker));

	list[0].prev = NULL;
	list[0].next = NULL;
	list[0].index = 0;
	list[0].item = m_data[0];

	Linker* curr = list;

	for (uint32_t i = 1; i < m_count; ++i)
	{
		list[i].index = i;
		list[i].item = m_data[i];
		if (m_data[i] < m_data[curr->index])
		{
			while (curr->prev != NULL)
			{
				curr = curr->prev;
				if (!(m_data[i] < m_data[curr->index]))
				{
					// insert after this
					list[i].next = curr->next;
					list[i].prev = curr;
					curr->next->prev = &list[i];
					curr->next = &list[i];
					curr = &list[i];
					break;
				}
			}
			if (curr != &list[i])
			{
				list[i].prev = NULL;
				list[i].next = curr;
				curr->prev = &list[i];
				curr = &list[i];
			}
		}
		else
		{
			while (curr->next != NULL)
			{
				curr = curr->next;
				if (m_data[i] < m_data[curr->index])
				{
					// insert before this
					list[i].next = curr;
					list[i].prev = curr->prev;
					curr->prev->next = &list[i];
					curr->prev = &list[i];
					curr = &list[i];
					break;
				}
			}
			if (curr != &list[i])
			{
				list[i].prev = curr;
				list[i].next = NULL;
				curr->next = &list[i];
				curr = &list[i];
			}
		}
	}

	// go to start of list
	while (curr->prev != NULL) curr = curr->prev;

	// copy sorted list back
	for (uint32_t i = 0; i < m_count; ++i)
	{
		m_data[i] = curr->item;
		curr = curr->next;
	}

	free(list);
#else
	uint32_t swaps;
	T temp;

	for (uint32_t i = 1; i < m_count; ++i)
	{
		swaps = 0;
		for (uint32_t j = end-1; j >= i; --j)
		{
			if (m_data[j] < m_data[j-1])
			{
				temp = m_data[j];
				m_data[j] = m_data[j-1];
				m_data[j-1] = temp;
				++swaps;
			}
		}
		if (swaps == 0)
			break;
	}
#endif
}

template <typename T>
void List<T>::sortAscendingDeref(void)
{
	if (m_count <= 1)
		return;
#if defined(XSENS_LIST_QSORT)
	qSortDeref(0,m_count-1);
#elif defined(XSENS_LIST_COMBSORT)

	uint32_t gap = m_count;
	double dgap;
	uint32_t swaps = 0;
	T temp;

	while (gap > 1 || swaps > 0)
	{
		if (gap > 1)
		{
			dgap = floor(((double) gap) / 1.247330950103979);
			gap = (uint32_t) dgap;
			if (gap == 10 || gap == 9)
				gap = 11;
		}

		uint32_t gappedCount = m_count-gap;
		swaps = 0;
		for (uint32_t i = 0; i < gappedCount; ++i)
		{
			if (*m_data[i+gap] < *m_data[i])
			{
				temp = m_data[i];
				m_data[i] = m_data[i+gap];
				m_data[i+gap] = temp;
				++swaps;
			}
		}
	}
#elif defined(XSENS_LIST_JOBSORT)
	struct Linker {
		Linker *prev, *next;
		uint32_t index;

		T item;
	};

	Linker* list = (Linker*) malloc(m_count*sizeof(Linker));

	list[0].prev = NULL;
	list[0].next = NULL;
	list[0].index = 0;
	list[0].item = m_data[0];

	Linker* curr = list;

	for (uint32_t i = 1; i < m_count; ++i)
	{
		list[i].index = i;
		list[i].item = m_data[i];
		if (*m_data[i] < *m_data[curr->index])
		{
			while (curr->prev != NULL)
			{
				curr = curr->prev;
				if (!(*m_data[i] < *m_data[curr->index]))
				{
					// insert after this
					list[i].next = curr->next;
					list[i].prev = curr;
					curr->next->prev = &list[i];
					curr->next = &list[i];
					curr = &list[i];
					break;
				}
			}
			if (curr != &list[i])
			{
				list[i].prev = NULL;
				list[i].next = curr;
				curr->prev = &list[i];
				curr = &list[i];
			}
		}
		else
		{
			while (curr->next != NULL)
			{
				curr = curr->next;
				if (*m_data[i] < *m_data[curr->index])
				{
					// insert before this
					list[i].next = curr;
					list[i].prev = curr->prev;
					curr->prev->next = &list[i];
					curr->prev = &list[i];
					curr = &list[i];
					break;
				}
			}
			if (curr != &list[i])
			{
				list[i].prev = curr;
				list[i].next = NULL;
				curr->next = &list[i];
				curr = &list[i];
			}
		}
	}

	// go to start of list
	while (curr->prev != NULL) curr = curr->prev;

	// copy sorted list back
	for (uint32_t i = 0; i < m_count; ++i)
	{
		m_data[i] = curr->item;
		curr = curr->next;
	}

	free(list);
#else
	uint32_t swaps;
	T temp;

	for (uint32_t i = 1; i < m_count; ++i)
	{
		swaps = 0;
		for (uint32_t j = end-1; j >= i; --j)
		{
			if (*(m_data[j]) < *(m_data[j-1]))
			{
				temp = m_data[j];
				m_data[j] = m_data[j-1];
				m_data[j-1] = temp;
				++swaps;
			}
		}
		if (swaps == 0)
			break;
	}
#endif
}

template <typename T>
template <typename T2>
void List<T>::twinSortAscending(List<T2>& twin)
{
	if (m_count <= 1)
		return;

	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (m_count != twin.m_count)
			throw "List.twinSortAscending: sizes do not match";
	#endif
	uint32_t iteration = 0;
	uint32_t mini;
	T tmp;
	T2 tmp2;
	if (m_count > 1)
	while (iteration < m_count-1)
	{
		mini = iteration;
		for (uint32_t i=iteration+1;i<m_count;++i)
		{
			if (m_data[i] < m_data[mini])
				mini = i;
		}
		if (mini != iteration)
		{
			tmp = m_data[mini];
			m_data[mini] = m_data[iteration];
			m_data[iteration] = tmp;

			tmp2 = twin.m_data[mini];
			twin.m_data[mini] = twin.m_data[iteration];
			twin.m_data[iteration] = tmp2;
		}
		++iteration;
	}
}

template <typename T>
void List<T>::remove(const uint32_t index) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (index >= m_count)
			throw "List.remove: index out of bounds";
	#endif
	if (index == m_count-1)
	{
		--m_count;
		return;
	}
	--m_count;
	for (unsigned i = index;i < m_count;++i)
		m_data[i] = m_data[i+1];
}

template <typename T>
void List<T>::removeTail(const uint32_t count) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (count > m_count)
			throw "List.removeTail: list size less than remove count";
	#endif
	if (m_count > count)
	{
		m_count -= count;
		return;
	}
	m_count = 0;
}

template <typename T>
void List<T>::deleteAndRemoveTail(const uint32_t count) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (count > m_count)
			throw "List.deleteAndRemoveTail: list size less than remove count";
	#endif
	if (m_count > count)
	{
		for (unsigned i = 0;i < count;++i)
			delete m_data[--m_count];
		return;
	}
	deleteAndClear();
}

template <typename T>
void List<T>::freeAndRemoveTail(const uint32_t count) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (count > m_count)
			throw "List.freeAndRemoveTail: list size less than remove count";
	#endif
	if (m_count > count)
	{
		for (unsigned i = 0;i < count;++i)
			free(m_data[--m_count]);
		return;
	}
	freeAndClear();
}

template <typename T>
uint32_t List<T>::removeDuplicateEntries(void)
{
	uint32_t removed = 0;
	for (uint32_t i=0;i < m_count; ++i)
	{
		for (uint32_t j=i+1;j < m_count; ++j)
		{
			if (m_data[i] == m_data[j])
			{
				remove(j);
				++removed;
				--j;
			}
		}
	}
	return removed;
}

template <typename T>
uint32_t List<T>::removeDuplicateEntriesDeref(void)
{
	uint32_t removed = 0;
	for (uint32_t i=0;i < m_count; ++i)
	{
		for (uint32_t j=i+1;j < m_count; ++j)
		{
			if (*(m_data[i]) == *(m_data[j]))
			{
				remove(j);
				++removed;
				--j;
			}
		}
	}
	return removed;
}

template <typename T>
void List<T>::deleteAndRemove(const uint32_t index) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (index >= m_count)
			throw "List.deleteAndRemove: index out of bounds";
	#endif
	delete m_data[index];
	if (index == m_count-1)
	{
		--m_count;
		return;
	}
	--m_count;
	for (unsigned i = index;i < m_count;++i)
		m_data[i] = m_data[i+1];
}

template <typename T>
void List<T>::freeAndRemove(const uint32_t index) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (index >= m_count)
			throw "List.freeAndRemove: index out of bounds";
	#endif
	free(m_data[index]);
	if (index == m_count-1)
	{
		--m_count;
		return;
	}
	--m_count;
	for (unsigned i = index;i < m_count;++i)
		m_data[i] = m_data[i+1];
}

template <typename T>
template <typename TB>
uint32_t List<T>::find(const TB& item) const
{
	for (uint32_t i=0;i<m_count;++i)
		if (((const T*)m_data)[i] == item)
			return i;
	return XSENS_LIST_NOTFOUND;
}

template <typename T>
uint32_t List<T>::find(const T item, InequalityFunction fnc) const
{
	for (uint32_t i=0;i<m_count;++i)
		if (!fnc(m_data[i],item))
			return i;
	return XSENS_LIST_NOTFOUND;
}

template <typename T>
template <typename TB>
uint32_t List<T>::findDeref(const TB& item) const
{
	for (uint32_t i=0;i<m_count;++i)
		if (*(m_data[i]) == item)
			return i;
	return XSENS_LIST_NOTFOUND;
}

template <typename T>
template <typename TB>
uint32_t List<T>::findSorted(const TB& item) const
{
	if (m_count < CMT_LIST_LINEAR_SEARCH_TRESHOLD)			// for small lists, it is faster to simply walk the list
		return find(item);

	uint32_t x = m_count;
	uint32_t n = 1;
	uint32_t i;

	while(x >= n)
	{
		i = (x + n) >> 1;

		if (m_data[i-1] == item)
			return i-1;
		if (m_data[i-1] < item)
			n = i+1;
		else
			x = i-1;
	}
	return XSENS_LIST_NOTFOUND;
}

template <typename T>
template <typename TB>
uint32_t List<T>::findSortedDeref(const TB& item) const
{
	if (m_count < CMT_LIST_LINEAR_SEARCH_TRESHOLD)			// for small lists, it is faster to simply walk the list
		return findDeref(item);

	uint32_t x = m_count;
	uint32_t n = 1;
	uint32_t i;

	while(x >= n)
	{
		i = (x + n) >> 1;

		if (*(m_data[i-1]) == item)
			return i-1;
		if (*(m_data[i-1]) < item)
			n = i+1;
		else
			x = i-1;
	}
	return XSENS_LIST_NOTFOUND;
}

template <typename T>
uint32_t List<T>::insertSorted(const T& item)
{
	uint32_t i;
	if (m_count < CMT_LIST_LINEAR_SEARCH_TRESHOLD)
	{
		for (i=0;i<m_count;++i)
			if (item < m_data[i])
			{
				insert(item,i);
				return i;
			}
		append(item);
		return m_count-1;
	}
	else
	{
		uint32_t x = m_count;
		uint32_t n = 1;

		while(x >= n)
		{
			i = (x + n) >> 1;

			if (m_data[i-1] == item)
			{
				insert(item,i-1);
				return i-1;
			}
			if (m_data[i-1] < item)
				n = i+1;
			else
				x = i-1;
		}
		insert(item,n-1);
		return n-1;
	}
}

template <typename T>
uint32_t List<T>::insertSortedDeref(const T& item)
{
	uint32_t i;
	if (m_count < CMT_LIST_LINEAR_SEARCH_TRESHOLD)
	{
		for (i=0;i<m_count;++i)
			if (*item < *m_data[i])
			{
				insert(item,i);
				return i;
			}
		append(item);
		return m_count-1;
	}
	else
	{
		uint32_t x = m_count;
		uint32_t n = 1;

		while(x >= n)
		{
			i = (x + n) >> 1;

			if (*(m_data[i-1]) == *item)
			{
				insert(item,i-1);
				return i-1;
			}
			if (*(m_data[i-1]) < *item)
				n = i+1;
			else
				x = i-1;
		}
		insert(item,n-1);
		return n-1;
	}
}

template <typename T>
template <typename TB>
uint32_t List<T>::insertSortedCopy(const TB& item)
{
	uint32_t i;
	if (m_count < CMT_LIST_LINEAR_SEARCH_TRESHOLD)
	{
		for (i=0;i<m_count;++i)
			if (item < m_data[i])
			{
				insertCopy<TB>(item,i);
				return i;
			}
		append(item);
		return m_count-1;
	}
	else
	{
		uint32_t x = m_count;
		uint32_t n = 1;

		while(x >= n)
		{
			i = (x + n) >> 1;

			if (m_data[i-1] == item)
			{
				insertCopy<TB>(item,i-1);
				return i-1;
			}
			if (m_data[i-1] < item)
				n = i+1;
			else
				x = i-1;
		}
		insertCopy<TB>(item,n-1);
		return n-1;
	}
}

template <typename T>
void List<T>::deleteItemsOnDestroy(void)
{
	if (m_jcf != NULL)
	{
		m_jcf->disable();
		delete m_jcf;
	}
	m_jcf = new JanitorClassFunc<List<T>, void>(*this,&List<T>::deleteAndClear);
}

template <typename T>
void List<T>::freeItemsOnDestroy(void)
{
	if (m_jcf != NULL)
	{
		m_jcf->disable();
		delete m_jcf;
	}
	m_jcf = new JanitorClassFunc<List<T>, void>(*this,&List<T>::freeAndClear);
}

template <typename T>
template <typename TB>
void List<T>::isDeepCopyOf(const List<T>& source)
{
	m_count = 0;
	if (m_max < source.m_count)
		resize(source.m_count);
	m_count = source.m_count;
	for (uint32_t i = 0;i<m_count;++i)
		m_data[i] = new TB(*source.m_data[i]);
}

template <typename T>
void List<T>::isShallowCopyOf(const List<T>& x)
{
	m_count = 0;
	if (m_max < x.m_count)
		resize(x.m_count);
	m_count = x.m_count;
	for (uint32_t i = 0;i<m_count;++i)
		m_data[i] = x.m_data[i];
}

template <typename T>
void List<T>::swap(const uint32_t i, const uint32_t j) XSENS_LIST_THROW
{
	#ifdef _XSENS_LIST_RANGE_CHECKS
		if (i >= m_count || j >= m_count)
			throw "List.swap: index out of bounds";
	#endif
	T tmp = m_data[i];
	m_data[i] = m_data[j];
	m_data[j] = tmp;
}

template <typename T>
void List<T>::reverse(void)
{
	uint32_t half = m_count / 2;
	for (uint32_t i = 0, end=m_count-1; i < half; ++i,--end)
	{
		T tmp = m_data[i];
		m_data[i] = m_data[end];
		m_data[end] = tmp;
	}
}

} // end of xsens namespace

#endif	// _XSENS_LIST_HPP_2006_06_08

