#include"HRTF.h"
#include"global/Base.h"

HRTF::HRTF():
	m_filter(128)
{
	BuildReacherTree();
}

HRTF::~HRTF()
{

}

void HRTF::GetDirection(int& p_elevation, int& p_azimuth)
{
	p_elevation = m_elevation;
	p_azimuth = m_is_swap_left_and_right ? -m_azimuth : m_azimuth;
}

int HRTF::SetDirection(int p_elevation, int p_azimuth)
{
	while (p_azimuth > 180)
		p_azimuth -= 360;
	while (p_azimuth < -180)
		p_azimuth += 360;

	if (m_real_elevation == p_elevation && m_real_azimuth == p_azimuth)
		return 0;

	m_real_elevation = p_elevation;
	m_real_azimuth = p_azimuth;

	m_is_swap_left_and_right = (p_azimuth < 0);
	p_azimuth = m_is_swap_left_and_right ? -p_azimuth : p_azimuth;

	int index = SearchNearestIndex(p_elevation, p_azimuth);
	if (index == m_direction_index)
		return 0;

	m_direction_index = index;
	m_elevation = kHRTFDirection[index][0];
	m_azimuth = kHRTFDirection[index][1];
	return 1;
}

void HRTF::GetLeftEarTimeHRTF(std::vector<float>& p_data)
{
	const short* target = nullptr;
	if (m_is_swap_left_and_right)
		target = kHRTFData[m_direction_index][1];
	else
		target = kHRTFData[m_direction_index][0];
	p_data.clear();
	p_data.reserve(kHRTFFilterLen); 
	for (int i = 0; i < kHRTFFilterLen; i++) {
		p_data.push_back(static_cast<float>(target[i]));
	}
}

void HRTF::GetLeftEarFreqHRTF(std::vector<float>& p_data)
{
	std::vector<float> m_timeHTRF;
	GetLeftEarTimeHRTF(m_timeHTRF);
	m_filter.ForwardTransform(m_timeHTRF, &p_data);
}

void HRTF::GetRightEarTimeHRTF(std::vector<float>& p_data)
{
	const short* target = nullptr;
	if (m_is_swap_left_and_right)
		target = kHRTFData[m_direction_index][0];
	else
		target = kHRTFData[m_direction_index][1];
	p_data.clear();
	p_data.reserve(kHRTFFilterLen);
	for (int i = 0; i < kHRTFFilterLen; i++) {
		p_data.push_back(static_cast<float>(target[i]));
	}
}

void HRTF::GetRightEarFreqHRTF(std::vector<float>& p_data)
{
	std::vector<float> m_timeHTRF;
	GetRightEarTimeHRTF(m_timeHTRF);
	m_filter.ForwardTransform(m_timeHTRF, &p_data);
}


void HRTF::BuildReacherTree()
{
	for (int i = -40; i < 100; i += 10){
		hrtf_directions.insert(std::make_pair(i, std::make_pair(std::vector<int>(), std::vector<int>())));
	}
	for (int i = 0; i < kHRTFNum; i++)
	{
		hrtf_directions[kHRTFDirection[i][0]].first.push_back(kHRTFDirection[i][1]);
		hrtf_directions[kHRTFDirection[i][0]].second.push_back(i);
	}
}

int HRTF::SearchNearestIndex(int p_elevation, int p_azimuth)
{
	int low_elevation = 0;
	int high_elevation = 0;
	if (p_elevation <= -40) { 
		low_elevation = -40; 
		high_elevation = low_elevation + 10;
	}
	else if (p_elevation >= 90) {
		high_elevation = 90;
		low_elevation = high_elevation - 10;
	}
	else {
		low_elevation = p_elevation < 0 ? 10 * (int)(p_elevation / 10. - 1) : 10 * (int)(p_elevation / 10.);
		high_elevation = low_elevation + 10;
	}
	


	int low_small = 0;
	int low_large = 0;
	for (int i = 0; i < hrtf_directions[low_elevation].first.size(); i++){
		if (p_azimuth < hrtf_directions[low_elevation].first[i]){
			low_small = i - 1;
			low_large = i;
			break;
		}
	}

	int high_small = 0;
	int high_large = 0;
	for (int i = 0; i < hrtf_directions[high_elevation].first.size(); i++){
		if (p_azimuth < hrtf_directions[high_elevation].first[i]) {
			high_small = i - 1;
			high_large = i;
			break;
		}
	}

	int index = 0;
	double distance_min = CaculateSphereDistance(p_elevation, p_azimuth, low_elevation, hrtf_directions[low_elevation].first[low_small]);
	index = hrtf_directions[low_elevation].second[low_small];

	if (double newdistance = CaculateSphereDistance(p_elevation, p_azimuth, low_elevation, hrtf_directions[low_elevation].first[low_large]); newdistance < distance_min) {
		distance_min = newdistance;
		index = hrtf_directions[low_elevation].second[low_large];
	}

	if (double newdistance = CaculateSphereDistance(p_elevation, p_azimuth, high_elevation, hrtf_directions[high_elevation].first[high_small]); newdistance < distance_min) {
		distance_min = newdistance;
		index = hrtf_directions[high_elevation].second[high_small];
	}

	if (double newdistance = CaculateSphereDistance(p_elevation, p_azimuth, high_elevation, hrtf_directions[high_elevation].first[high_large]); newdistance < distance_min) {
		distance_min = newdistance;
		index = hrtf_directions[high_elevation].second[high_large];
	}

	return index;
}

double HRTF::CaculateSphereDistance(int p_elevation1, int p_azimuth1, int p_elevation2, int p_azimuth2)
{
	double elevation1 = p_elevation1 * PI / 180.;
	double elevation2 = p_elevation2 * PI / 180.;
	double azimuth1 = p_azimuth1 * PI / 180.;
	double azimuth2 = p_azimuth2 * PI / 180.;

	double distance = acos(sin(elevation1) * sin(elevation2) + cos(elevation1) * cos(elevation2) * cos(azimuth1 - azimuth2));
	return distance;
}