#include "Tracker.hpp"

#include <iostream>

using namespace std;

namespace Radio
{
namespace Metrics
{

Tracker::Tracker(RealScalar rho, RealScalar smoothness)
: m_rho(rho)
, m_smoothness(smoothness)
, m_trackers(NUM_QUADRRANTS)
{
	reset();
}

Tracker::~Tracker()
{
}

void Tracker::reset()
{
	CVec initVals = {ComplexScalar(-0.5,-0.5), ComplexScalar(+0.5,-0.5), ComplexScalar(-0.5,+0.5), ComplexScalar(+0.5,+0.5)};

	for (auto v : initVals)
	{
		m_trackers[getQuadrant(v)] = v;
	}
}

CVec Tracker::trackers() const
{
	return m_trackers;
}

void Tracker::process(ComplexScalar const &x)
{
	size_t qu = getQuadrant(x);

	ComplexScalar diff = x - m_trackers[qu];
	RealScalar dist = abs(diff);
	RealScalar weight = exp(-16*dist);

	if (abs(x) > abs(m_trackers[qu]))
	{
		m_trackers[qu] = m_smoothness*x + (RealScalar(1)-m_smoothness)*m_trackers[qu];
	}
	else
	{
		m_trackers[qu] += RealScalar(0.5)*weight*diff;
	}
}

size_t Tracker::getQuadrant(ComplexScalar const &x)
{
	size_t result = 0;

	if (x.real() < 0)
	{
		result |= 0x02;
	}
	if (x.imag() < 0)
	{
		result |= 0x01;
	}
	return result;
}

} // Metrics
} // Radio