+/* $Id$
+
+This file is part of R²C²
+Copyright © 2011 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include "speedquantizer.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+SpeedQuantizer::SpeedQuantizer(unsigned n):
+ steps(n+1)
+{
+ if(n<1)
+ throw InvalidParameterValue("Must have at leats one speed step");
+}
+
+void SpeedQuantizer::learn(unsigned i, float s, float w)
+{
+ if(i>=steps.size())
+ throw InvalidParameterValue("Speed step index out of range");
+ steps[i].add(s, w);
+}
+
+float SpeedQuantizer::get_speed(unsigned i) const
+{
+ if(i==0)
+ return 0;
+ if(steps[i].weight)
+ return steps[i].speed;
+
+ unsigned low;
+ unsigned high;
+ for(low=i; low>0; --low)
+ if(steps[low].weight)
+ break;
+ for(high=i; high+1<steps.size(); ++high)
+ if(steps[high].weight)
+ break;
+
+ if(steps[high].weight)
+ {
+ if(steps[low].weight)
+ return (steps[low].speed*(high-i)+steps[high].speed*(i-low))/(high-low);
+ else
+ return steps[high].speed*i/high;
+ }
+ else if(steps[low].weight)
+ return steps[low].speed*i/low;
+ else
+ return 0;
+}
+
+float SpeedQuantizer::quantize_speed(float speed) const
+{
+ return get_speed(find_speed_step(speed));
+}
+
+unsigned SpeedQuantizer::find_speed_step(float speed) const
+{
+ if(speed<=steps[1].speed*0.5)
+ return 0;
+
+ unsigned low = 0;
+ unsigned high = 0;
+ unsigned last = 0;
+ for(unsigned i=0; (!high && i<steps.size()); ++i)
+ if(steps[i].weight)
+ {
+ last = i;
+ if(steps[i].speed>=speed)
+ high = i;
+ else if(steps[i].speed>steps[low].speed)
+ low = i;
+ }
+
+ if(!high)
+ {
+ unsigned limit = steps.size()/5;
+ if(!low)
+ {
+ if(speed)
+ return limit;
+ else
+ return 0;
+ }
+ return min(min(static_cast<unsigned>(low*speed/steps[low].speed), steps.size()-1), last+limit);
+ }
+
+ float f = (speed-steps[low].speed)/(steps[high].speed-steps[low].speed);
+ return static_cast<unsigned>(low*(1-f)+high*f+0.5);
+}
+
+void SpeedQuantizer::save(list<DataFile::Statement> &st) const
+{
+ for(unsigned i=0; i<steps.size(); ++i)
+ if(steps[i].weight)
+ st.push_back((DataFile::Statement("step"), i, steps[i].speed, steps[i].weight));
+}
+
+
+SpeedQuantizer::SpeedStep::SpeedStep():
+ speed(0),
+ weight(0)
+{ }
+
+void SpeedQuantizer::SpeedStep::add(float s, float w)
+{
+ speed = (speed*weight+s*w)/(weight+w);
+ weight = min(weight+w, 300.0f);
+}
+
+
+SpeedQuantizer::Loader::Loader(SpeedQuantizer &q):
+ DataFile::ObjectLoader<SpeedQuantizer>(q)
+{
+ add("step", &Loader::step);
+}
+
+void SpeedQuantizer::Loader::step(unsigned i, float s, float w)
+{
+ if(i>=obj.steps.size())
+ return;
+
+ obj.steps[i].speed = s;
+ obj.steps[i].weight = w;
+}
+
+} // namespace R2C2