+ fir_empty = false;
+}
+
+
+PIChaser::PIChaser() {
+ pic = new PIController( 1.0, 16 );
+ array_index = 0;
+ for( int i=0; i<ESTIMATOR_SIZE; i++ ) {
+ realtime_stamps[i] = 0;
+ chasetime_stamps[i] = 0;
+ }
+
+ speed_threshold = 0.2;
+ pos_threshold = 4000;
+ want_locate_val = 0;
+}
+
+void
+PIChaser::reset() {
+ array_index = 0;
+ for( int i=0; i<ESTIMATOR_SIZE; i++ ) {
+ realtime_stamps[i] = 0;
+ chasetime_stamps[i] = 0;
+ }
+ pic->reset(1.0);
+}
+PIChaser::~PIChaser() {
+ delete pic;
+}
+
+double
+PIChaser::get_ratio(framepos_t chasetime_measured, framepos_t chasetime, framepos_t slavetime_measured, framepos_t slavetime, bool in_control, int period_size ) {
+
+ feed_estimator( chasetime_measured, chasetime );
+ std::cerr << (double)chasetime_measured/48000.0 << " " << chasetime << " " << slavetime << " ";
+ double crude = get_estimate();
+ double fine;
+ framepos_t massaged_chasetime = chasetime + (framepos_t)( (double)(slavetime_measured - chasetime_measured) * crude );
+
+ fine = pic->get_ratio (slavetime - massaged_chasetime, period_size);
+ if (in_control) {
+ if (fabs(fine-crude) > crude*speed_threshold) {
+ std::cout << "reset to " << crude << " fine = " << fine << "\n";
+ pic->reset( crude );
+ speed = crude;
+ } else {
+ speed = fine;
+ }
+
+ if (abs(chasetime-slavetime) > pos_threshold) {
+ pic->reset( crude );
+ speed = crude;
+ want_locate_val = chasetime;
+ std::cout << "we are off by " << chasetime-slavetime << " want_locate:" << chasetime << "\n";
+ } else {
+ want_locate_val = 0;
+ }
+ } else {
+ std::cout << "not in control..." << crude << "\n";
+ speed = crude;
+ pic->reset( crude );
+ }
+
+ return speed;
+}
+
+void
+PIChaser::feed_estimator (framepos_t realtime, framepos_t chasetime ) {
+ array_index += 1;
+ realtime_stamps [ array_index%ESTIMATOR_SIZE ] = realtime;
+ chasetime_stamps[ array_index%ESTIMATOR_SIZE ] = chasetime;
+}
+
+double
+PIChaser::get_estimate() {
+ double est = 0;
+ int num=0;
+ int i;
+ framepos_t n1_realtime;
+ framepos_t n1_chasetime;
+ for( i=(array_index + 1); i<=(array_index + ESTIMATOR_SIZE); i++ ) {
+ if( realtime_stamps[(i)%ESTIMATOR_SIZE] ) {
+ n1_realtime = realtime_stamps[(i)%ESTIMATOR_SIZE];
+ n1_chasetime = chasetime_stamps[(i)%ESTIMATOR_SIZE];
+ i+=1;
+ break;
+ }
+ }
+
+ for( ; i<=(array_index + ESTIMATOR_SIZE); i++ ) {
+ if( realtime_stamps[(i)%ESTIMATOR_SIZE] ) {
+ if( (realtime_stamps[(i)%ESTIMATOR_SIZE] - n1_realtime) > 200 ) {
+ framepos_t n_realtime = realtime_stamps[(i)%ESTIMATOR_SIZE];
+ framepos_t n_chasetime = chasetime_stamps[(i)%ESTIMATOR_SIZE];
+ est += ((double)( n_chasetime - n1_chasetime ))
+ / ((double)( n_realtime - n1_realtime ));
+ n1_realtime = n_realtime;
+ n1_chasetime = n_chasetime;
+ num += 1;
+ }
+ }
+ }
+
+ if(num)
+ return est/(double)num;
+ else
+ return 0.0;