#include"util.h"
#include <iostream>
using namespace std;

int Limit_Int(int value, int minValue, int maxValue)
{
	if (value < minValue)
	{
		return minValue;
	}
	else if (value > maxValue)
	{
		return maxValue;
	}
	else
	{
		return value;
	}
}

double Limit_Double(double value, double minValue, double maxValue)
{
	if (value < minValue)
	{
		return minValue;
	}
	else if (value > maxValue)
	{
		return maxValue;
	}
	else
	{
		return value;
	}
}

void QuickSort(Eigen::VectorXi& array, Eigen::VectorXi& index, int low, int high) {
	if (low < high) {
		//分段位置下标 
		int standard = GetStandard(array, index, low, high);
		//递归调用排序
		//左边排序 
		QuickSort(array, index, low, standard - 1);
		//右边排序 
		QuickSort(array, index, standard + 1, high);
	}
}

int GetStandard(Eigen::VectorXi& array, Eigen::VectorXi& index, int i, int j) {
	//基准数据 
	int key = array(i);
	int key2 = index(i);
	while (i < j) {
		//因为默认基准是从左边开始，所以从右边开始比较 
		//当队尾的元素大于等于基准数据 时,就一直向前挪动 j 指针 
		while (i < j && (array(j) > key || array(j) == key)) {
			j--;
		}
		//当找到比 array[i] 小的时，就把后面的值 array[j] 赋给它 
		if (i < j) {
			array(i) = array(j);
			index(i) = index(j);
		}
		//当队首元素小于等于基准数据 时,就一直向后挪动 i 指针 
		while (i < j && (array(i) < key || (array(i) == key))) {
			i++;
		}
		//当找到比 array[j] 大的时，就把前面的值 array[i] 赋给它
		if (i < j) {
			array(j) = array(i);
			index(j) = index(i);
		}
	}
	//跳出循环时 i 和 j 相等,此时的 i 或 j 就是 key 的正确索引位置
	//把基准数据赋给正确位置 
	array(i) = key;
	index(i) = key2;
	return i;
}

void Unique(Eigen::VectorXi& array, Eigen::VectorXi& index)
{
	int overNum = 0;
	for (int i = 0; i < array.rows(); i++)
	{
		if (i + overNum == array.rows())
			break;

		index(i) = index(i + overNum);
		array(i) = array(i + overNum);

		if (i + overNum + 1 == array.rows())
			break;

		if (array(i + overNum) == array(i + overNum + 1))
		{
			overNum++;
		}
	}

	if (overNum == 0)
		return;

	int nbr = array.rows() - overNum;
	Eigen::VectorXi tempindex = index.head(nbr);
	Eigen::VectorXi temparray = array.head(nbr);
	index = tempindex;
	array = temparray;
}