bool DataSample::operator<(const DataSample &rhs) const
{
assert(this->getDimX() == rhs.getDimX());
for (unsigned int i = 0; i < this->getDimX(); i++)
{
if (x.at(i) < rhs.getX().at(i))
return true;
else if (x.at(i) > rhs.getX().at(i))
return false;
}
return false;
}
bool RBFSpline::dist_sort(DataSample x, DataSample y) const
{
std::vector<double> zeros(x.getDimX(), 0);
DataSample origin(zeros, 0.0);
double x_dist = dist(x, origin);
double y_dist = dist(y, origin);
return (x_dist<y_dist);
}
long TimePermRingBuffer::read(int index, DataSample &data)
{
get(index, (void*)data.data());
long time;
m_lastTimeStamp.readData((void*)&time);
time -= ( index % bufferSize() ) * m_period;
return time;
}
/*
* Computes Euclidean distance ||x-y||
*/
double RBFSpline::dist(DataSample x, DataSample y) const
{
return dist(x.getX(), y.getX());
}
bool TimePermRingBuffer::insert(DataSample &data, long current_time)
{
long last_time, delta, steps;
m_lastTimeStamp.readData((void*)&last_time);
delta = current_time - last_time;
#ifdef SERIAL_DEBUG
Serial.print("==== TimePermRingBuffer[");
Serial.print(m_bufferStart, DEC);
Serial.print("]::insert -> current=");
Serial.print(current_time, DEC);
Serial.print(" - last_time=");
Serial.print(last_time, DEC);
Serial.print(" : delta=");
Serial.println(delta, DEC);
#endif
if ( delta < 0 ) {
// something happened: we are going back in the past
// just let clear everything and start orer
#ifdef SERIAL_DEBUG
Serial.println("------ back to the past -> clear");
#endif
clear();
m_lastTimeStamp.writeData((void*)¤t_time);
push(data.data());
return true;
}
if ( delta > timeSpan() ) {
// elapsed time greater than buffer time span
#ifdef SERIAL_DEBUG
Serial.println("------ elapsed time greater than buffer time span -> clear");
#endif
clear();
m_lastTimeStamp.writeData((void*)¤t_time);
push(data.data());
}
else {
if ( 0 == delta % m_period ) {
steps = delta / m_period;
if ( steps > 0 ) {
// this is time to insert the new sample
if ( steps > 1 ) {
// elapsed time more than one single period
#ifdef SERIAL_DEBUG
Serial.print("------ elapsed time more than a single period -> rotate steps = ");
Serial.println(steps-1, DEC);
#endif
rotate(steps-1);
}
m_lastTimeStamp.writeData((void*)¤t_time);
push(data.data());
}
}
else {
#ifdef SERIAL_DEBUG
Serial.println("------ period not elapsed -> no insertion");
#endif
return false;
}
}
return true;
}
