libs/rubberband/src/StretcherProcess.cpp

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2
   3 /*
   4     Rubber Band
   5     An audio time-stretching and pitch-shifting library.
   6     Copyright 2007-2008 Chris Cannam.
   7
   8     This program is free software; you can redistribute it and/or
   9     modify it under the terms of the GNU General Public License as
  10     published by the Free Software Foundation; either version 2 of the
  11     License, or (at your option) any later version.  See the file
  12     COPYING included with this distribution for more information.
  13 */
  14
  15 #include "StretcherImpl.h"
  16 #include "PercussiveAudioCurve.h"
  17 #include "HighFrequencyAudioCurve.h"
  18 #include "ConstantAudioCurve.h"
  19 #include "StretchCalculator.h"
  20 #include "StretcherChannelData.h"
  21 #include "Resampler.h"
  22 #include "Profiler.h"
  23
  24 #include <cassert>
  25 #include <cmath>
  26 #include <set>
  27 #include <map>
  28 #include <deque>
  29
  30
  31 using std::cerr;
  32 using std::endl;
  33
  34 namespace RubberBand {
  35
  36 RubberBandStretcher::Impl::ProcessThread::ProcessThread(Impl *s, size_t c) :
  37     m_s(s),
  38     m_channel(c),
  39     m_dataAvailable(std::string("data ") + char('A' + c)),
  40     m_abandoning(false)
  41 { }
  42
  43 void
  44 RubberBandStretcher::Impl::ProcessThread::run()
  45 {
  46     if (m_s->m_debugLevel > 1) {
  47         cerr << "thread " << m_channel << " getting going" << endl;
  48     }
  49
  50     ChannelData &cd = *m_s->m_channelData[m_channel];
  51
  52     while (cd.inputSize == -1 ||
  53            cd.inbuf->getReadSpace() > 0) {
  54
  55 //        if (cd.inputSize != -1) {
  56 //            cerr << "inputSize == " << cd.inputSize
  57 //                 << ", readSpace == " << cd.inbuf->getReadSpace() << endl;
  58 //        }
  59
  60         bool any = false, last = false;
  61         m_s->processChunks(m_channel, any, last);
  62
  63         if (last) break;
  64
  65         if (any) m_s->m_spaceAvailable.signal();
  66
  67         m_dataAvailable.lock();
  68         if (!m_s->testInbufReadSpace(m_channel) && !m_abandoning) {
  69             m_dataAvailable.wait();
  70         } else {
  71             m_dataAvailable.unlock();
  72         }
  73
  74         if (m_abandoning) {
  75             if (m_s->m_debugLevel > 1) {
  76                 cerr << "thread " << m_channel << " abandoning" << endl;
  77             }
  78             return;
  79         }
  80     }
  81
  82     bool any = false, last = false;
  83     m_s->processChunks(m_channel, any, last);
  84     m_s->m_spaceAvailable.signal();
  85
  86     if (m_s->m_debugLevel > 1) {
  87         cerr << "thread " << m_channel << " done" << endl;
  88     }
  89 }
  90
  91 void
  92 RubberBandStretcher::Impl::ProcessThread::signalDataAvailable()
  93 {
  94     m_dataAvailable.signal();
  95 }
  96
  97 void
  98 RubberBandStretcher::Impl::ProcessThread::abandon()
  99 {
 100     m_abandoning = true;
 101 }
 102
 103 bool
 104 RubberBandStretcher::Impl::resampleBeforeStretching() const
 105 {
 106     // We can't resample before stretching in offline mode, because
 107     // the stretch calculation is based on doing it the other way
 108     // around.  It would take more work (and testing) to enable this.
 109     if (!m_realtime) return false;
 110
 111     if (m_options & OptionPitchHighQuality) {
 112         return (m_pitchScale < 1.0); // better sound
 113     } else if (m_options & OptionPitchHighConsistency) {
 114         return false;
 115     } else {
 116         return (m_pitchScale > 1.0); // better performance
 117     }
 118 }
 119
 120 size_t
 121 RubberBandStretcher::Impl::consumeChannel(size_t c, const float *input,
 122                                           size_t samples, bool final)
 123 {
 124     Profiler profiler("RubberBandStretcher::Impl::consumeChannel");
 125
 126     ChannelData &cd = *m_channelData[c];
 127     RingBuffer<float> &inbuf = *cd.inbuf;
 128
 129     size_t toWrite = samples;
 130     size_t writable = inbuf.getWriteSpace();
 131
 132     bool resampling = resampleBeforeStretching();
 133
 134     if (resampling) {
 135
 136         toWrite = int(ceil(samples / m_pitchScale));
 137         if (writable < toWrite) {
 138             samples = int(floor(writable * m_pitchScale));
 139             if (samples == 0) return 0;
 140         }
 141
 142         size_t reqSize = int(ceil(samples / m_pitchScale));
 143         if (reqSize > cd.resamplebufSize) {
 144             cerr << "WARNING: RubberBandStretcher::Impl::consumeChannel: resizing resampler buffer from "
 145                  << cd.resamplebufSize << " to " << reqSize << endl;
 146             cd.setResampleBufSize(reqSize);
 147         }
 148
 149
 150         toWrite = cd.resampler->resample(&input,
 151                                          &cd.resamplebuf,
 152                                          samples,
 153                                          1.0 / m_pitchScale,
 154                                          final);
 155
 156     }
 157
 158     if (writable < toWrite) {
 159         if (resampling) {
 160             return 0;
 161         }
 162         toWrite = writable;
 163     }
 164
 165     if (resampling) {
 166         inbuf.write(cd.resamplebuf, toWrite);
 167         cd.inCount += samples;
 168         return samples;
 169     } else {
 170         inbuf.write(input, toWrite);
 171         cd.inCount += toWrite;
 172         return toWrite;
 173     }
 174 }
 175
 176 void
 177 RubberBandStretcher::Impl::processChunks(size_t c, bool &any, bool &last)
 178 {
 179     Profiler profiler("RubberBandStretcher::Impl::processChunks");
 180
 181     // Process as many chunks as there are available on the input
 182     // buffer for channel c.  This requires that the increments have
 183     // already been calculated.
 184
 185     ChannelData &cd = *m_channelData[c];
 186
 187     last = false;
 188     any = false;
 189
 190     while (!last) {
 191
 192         if (!testInbufReadSpace(c)) {
 193 //            cerr << "not enough input" << endl;
 194             break;
 195         }
 196
 197         any = true;
 198
 199         if (!cd.draining) {
 200             size_t got = cd.inbuf->peek(cd.fltbuf, m_windowSize);
 201             assert(got == m_windowSize || cd.inputSize >= 0);
 202             cd.inbuf->skip(m_increment);
 203             analyseChunk(c);
 204         }
 205
 206         bool phaseReset = false;
 207         size_t phaseIncrement, shiftIncrement;
 208         getIncrements(c, phaseIncrement, shiftIncrement, phaseReset);
 209
 210         last = processChunkForChannel(c, phaseIncrement, shiftIncrement, phaseReset);
 211         cd.chunkCount++;
 212         if (m_debugLevel > 2) {
 213             cerr << "channel " << c << ": last = " << last << ", chunkCount = " << cd.chunkCount << endl;
 214         }
 215     }
 216 }
 217
 218 bool
 219 RubberBandStretcher::Impl::processOneChunk()
 220 {
 221     Profiler profiler("RubberBandStretcher::Impl::processOneChunk");
 222
 223     // Process a single chunk for all channels, provided there is
 224     // enough data on each channel for at least one chunk.  This is
 225     // able to calculate increments as it goes along.
 226
 227     for (size_t c = 0; c < m_channels; ++c) {
 228         if (!testInbufReadSpace(c)) return false;
 229         ChannelData &cd = *m_channelData[c];
 230         if (!cd.draining) {
 231             size_t got = cd.inbuf->peek(cd.fltbuf, m_windowSize);
 232             assert(got == m_windowSize || cd.inputSize >= 0);
 233             cd.inbuf->skip(m_increment);
 234             analyseChunk(c);
 235         }
 236     }
 237
 238     bool phaseReset = false;
 239     size_t phaseIncrement, shiftIncrement;
 240     if (!getIncrements(0, phaseIncrement, shiftIncrement, phaseReset)) {
 241         calculateIncrements(phaseIncrement, shiftIncrement, phaseReset);
 242     }
 243
 244     bool last = false;
 245     for (size_t c = 0; c < m_channels; ++c) {
 246         last = processChunkForChannel(c, phaseIncrement, shiftIncrement, phaseReset);
 247         m_channelData[c]->chunkCount++;
 248     }
 249
 250     return last;
 251 }
 252
 253 bool
 254 RubberBandStretcher::Impl::testInbufReadSpace(size_t c)
 255 {
 256     Profiler profiler("RubberBandStretcher::Impl::testInbufReadSpace");
 257
 258     ChannelData &cd = *m_channelData[c];
 259     RingBuffer<float> &inbuf = *cd.inbuf;
 260
 261     size_t rs = inbuf.getReadSpace();
 262
 263     if (rs < m_windowSize && !cd.draining) {
 264
 265         if (cd.inputSize == -1) {
 266
 267             // Not all the input data has been written to the inbuf
 268             // (that's why the input size is not yet set).  We can't
 269             // process, because we don't have a full chunk of data, so
 270             // our process chunk would contain some empty padding in
 271             // its input -- and that would give incorrect output, as
 272             // we know there is more input to come.
 273
 274             if (!m_threaded) {
 275 //                cerr << "WARNING: RubberBandStretcher: read space < chunk size ("
 276 //                          << inbuf.getReadSpace() << " < " << m_windowSize
 277 //                          << ") when not all input written, on processChunks for channel " << c << endl;
 278             }
 279             return false;
 280         }
 281
 282         if (rs == 0) {
 283
 284             if (m_debugLevel > 1) {
 285                 cerr << "read space = 0, giving up" << endl;
 286             }
 287             return false;
 288
 289         } else if (rs < m_windowSize/2) {
 290
 291             if (m_debugLevel > 1) {
 292                 cerr << "read space = " << rs << ", setting draining true" << endl;
 293             }
 294
 295             cd.draining = true;
 296         }
 297     }
 298
 299     return true;
 300 }
 301
 302 bool
 303 RubberBandStretcher::Impl::processChunkForChannel(size_t c,
 304                                                   size_t phaseIncrement,
 305                                                   size_t shiftIncrement,
 306                                                   bool phaseReset)
 307 {
 308     Profiler profiler("RubberBandStretcher::Impl::processChunkForChannel");
 309
 310     // Process a single chunk on a single channel.  This assumes
 311     // enough input data is available; caller must have tested this
 312     // using e.g. testInbufReadSpace first.  Return true if this is
 313     // the last chunk on the channel.
 314
 315     if (phaseReset && (m_debugLevel > 1)) {
 316         cerr << "processChunkForChannel: phase reset found, incrs "
 317                   << phaseIncrement << ":" << shiftIncrement << endl;
 318     }
 319
 320     ChannelData &cd = *m_channelData[c];
 321
 322     if (!cd.draining) {
 323
 324         // This is the normal processing case -- draining is only
 325         // set when all the input has been used and we only need
 326         // to write from the existing accumulator into the output.
 327
 328         // We know we have enough samples available in m_inbuf --
 329         // this is usually m_windowSize, but we know that if fewer
 330         // are available, it's OK to use zeroes for the rest
 331         // (which the ring buffer will provide) because we've
 332         // reached the true end of the data.
 333
 334         // We need to peek m_windowSize samples for processing, and
 335         // then skip m_increment to advance the read pointer.
 336
 337         modifyChunk(c, phaseIncrement, phaseReset);
 338         synthesiseChunk(c); // reads from cd.mag, cd.phase
 339
 340         if (m_debugLevel > 2) {
 341             if (phaseReset) {
 342                 for (int i = 0; i < 10; ++i) {
 343                     cd.accumulator[i] = 1.2f - (i % 3) * 1.2f;
 344                 }
 345             }
 346         }
 347     }
 348
 349     bool last = false;
 350
 351     if (cd.draining) {
 352         if (m_debugLevel > 1) {
 353             cerr << "draining: accumulator fill = " << cd.accumulatorFill << " (shiftIncrement = " << shiftIncrement << ")" <<  endl;
 354         }
 355         if (shiftIncrement == 0) {
 356             cerr << "WARNING: draining: shiftIncrement == 0, can't handle that in this context: setting to " << m_increment << endl;
 357             shiftIncrement = m_increment;
 358         }
 359         if (cd.accumulatorFill <= shiftIncrement) {
 360             if (m_debugLevel > 1) {
 361                 cerr << "reducing shift increment from " << shiftIncrement
 362                           << " to " << cd.accumulatorFill
 363                           << " and marking as last" << endl;
 364             }
 365             shiftIncrement = cd.accumulatorFill;
 366             last = true;
 367         }
 368     }
 369
 370     if (m_threaded) {
 371
 372         int required = shiftIncrement;
 373
 374         if (m_pitchScale != 1.0) {
 375             required = int(required / m_pitchScale) + 1;
 376         }
 377
 378         if (cd.outbuf->getWriteSpace() < required) {
 379             if (m_debugLevel > 0) {
 380                 cerr << "Buffer overrun on output for channel " << c << endl;
 381             }
 382
 383             //!!! The only correct thing we can do here is resize the
 384             // buffer.  We can't wait for the client thread to read
 385             // some data out from the buffer so as to make more space,
 386             // because the client thread is probably stuck in a
 387             // process() call waiting for us to stow away enough input
 388             // increments to allow the process() call to complete.
 389
 390         }
 391     }
 392
 393     writeChunk(c, shiftIncrement, last);
 394     return last;
 395 }
 396
 397 void
 398 RubberBandStretcher::Impl::calculateIncrements(size_t &phaseIncrementRtn,
 399                                                size_t &shiftIncrementRtn,
 400                                                bool &phaseReset)
 401 {
 402     Profiler profiler("RubberBandStretcher::Impl::calculateIncrements");
 403
 404 //    cerr << "calculateIncrements" << endl;
 405
 406     // Calculate the next upcoming phase and shift increment, on the
 407     // basis that both channels are in sync.  This is in contrast to
 408     // getIncrements, which requires that all the increments have been
 409     // calculated in advance but can then return increments
 410     // corresponding to different chunks in different channels.
 411
 412     // Requires frequency domain representations of channel data in
 413     // the mag and phase buffers in the channel.
 414
 415     // This function is only used in real-time mode.
 416
 417     phaseIncrementRtn = m_increment;
 418     shiftIncrementRtn = m_increment;
 419     phaseReset = false;
 420
 421     if (m_channels == 0) return;
 422
 423     ChannelData &cd = *m_channelData[0];
 424
 425     size_t bc = cd.chunkCount;
 426     for (size_t c = 1; c < m_channels; ++c) {
 427         if (m_channelData[c]->chunkCount != bc) {
 428             cerr << "ERROR: RubberBandStretcher::Impl::calculateIncrements: Channels are not in sync" << endl;
 429             return;
 430         }
 431     }
 432
 433     const int hs = m_windowSize/2 + 1;
 434
 435     // Normally we would mix down the time-domain signal and apply a
 436     // single FFT, or else mix down the Cartesian form of the
 437     // frequency-domain signal.  Both of those would be inefficient
 438     // from this position.  Fortunately, the onset detectors should
 439     // work reasonably well (maybe even better?) if we just sum the
 440     // magnitudes of the frequency-domain channel signals and forget
 441     // about phase entirely.  Normally we don't expect the channel
 442     // phases to cancel each other, and broadband effects will still
 443     // be apparent.
 444
 445     float df = 0.f;
 446     bool silent = false;
 447
 448     if (m_channels == 1) {
 449
 450         df = m_phaseResetAudioCurve->process(cd.mag, m_increment);
 451         silent = (m_silentAudioCurve->process(cd.mag, m_increment) > 0.f);
 452
 453     } else {
 454
 455         double *tmp = (double *)alloca(hs * sizeof(double));
 456
 457         for (int i = 0; i < hs; ++i) {
 458             tmp[i] = 0.0;
 459         }
 460         for (size_t c = 0; c < m_channels; ++c) {
 461             for (int i = 0; i < hs; ++i) {
 462                 tmp[i] += m_channelData[c]->mag[i];
 463             }
 464         }
 465
 466         df = m_phaseResetAudioCurve->process(tmp, m_increment);
 467         silent = (m_silentAudioCurve->process(tmp, m_increment) > 0.f);
 468     }
 469
 470     int incr = m_stretchCalculator->calculateSingle
 471             (getEffectiveRatio(), df, m_increment);
 472
 473     m_lastProcessPhaseResetDf.write(&df, 1);
 474     m_lastProcessOutputIncrements.write(&incr, 1);
 475
 476     if (incr < 0) {
 477         phaseReset = true;
 478         incr = -incr;
 479     }
 480
 481     // The returned increment is the phase increment.  The shift
 482     // increment for one chunk is the same as the phase increment for
 483     // the following chunk (see comment below).  This means we don't
 484     // actually know the shift increment until we see the following
 485     // phase increment... which is a bit of a problem.
 486
 487     // This implies we should use this increment for the shift
 488     // increment, and make the following phase increment the same as
 489     // it.  This means in RT mode we'll be one chunk later with our
 490     // phase reset than we would be in non-RT mode.  The sensitivity
 491     // of the broadband onset detector may mean that this isn't a
 492     // problem -- test it and see.
 493
 494     shiftIncrementRtn = incr;
 495
 496     if (cd.prevIncrement == 0) {
 497         phaseIncrementRtn = shiftIncrementRtn;
 498     } else {
 499         phaseIncrementRtn = cd.prevIncrement;
 500     }
 501
 502     cd.prevIncrement = shiftIncrementRtn;
 503
 504     if (silent) ++m_silentHistory;
 505     else m_silentHistory = 0;
 506
 507     if (m_silentHistory >= int(m_windowSize / m_increment) && !phaseReset) {
 508         phaseReset = true;
 509         if (m_debugLevel > 1) {
 510             cerr << "calculateIncrements: phase reset on silence (silent history == "
 511                  << m_silentHistory << ")" << endl;
 512         }
 513     }
 514 }
 515
 516 bool
 517 RubberBandStretcher::Impl::getIncrements(size_t channel,
 518                                          size_t &phaseIncrementRtn,
 519                                          size_t &shiftIncrementRtn,
 520                                          bool &phaseReset)
 521 {
 522     Profiler profiler("RubberBandStretcher::Impl::getIncrements");
 523
 524     if (channel >= m_channels) {
 525         phaseIncrementRtn = m_increment;
 526         shiftIncrementRtn = m_increment;
 527         phaseReset = false;
 528         return false;
 529     }
 530
 531     // There are two relevant output increments here.  The first is
 532     // the phase increment which we use when recalculating the phases
 533     // for the current chunk; the second is the shift increment used
 534     // to determine how far to shift the processing buffer after
 535     // writing the chunk.  The shift increment for one chunk is the
 536     // same as the phase increment for the following chunk.
 537
 538     // When an onset occurs for which we need to reset phases, the
 539     // increment given will be negative.
 540
 541     // When we reset phases, the previous shift increment (and so
 542     // current phase increments) must have been m_increment to ensure
 543     // consistency.
 544
 545     // m_outputIncrements stores phase increments.
 546
 547     ChannelData &cd = *m_channelData[channel];
 548     bool gotData = true;
 549
 550     if (cd.chunkCount >= m_outputIncrements.size()) {
 551 //        cerr << "WARNING: RubberBandStretcher::Impl::getIncrements:"
 552 //             << " chunk count " << cd.chunkCount << " >= "
 553 //             << m_outputIncrements.size() << endl;
 554         if (m_outputIncrements.size() == 0) {
 555             phaseIncrementRtn = m_increment;
 556             shiftIncrementRtn = m_increment;
 557             phaseReset = false;
 558             return false;
 559         } else {
 560             cd.chunkCount = m_outputIncrements.size()-1;
 561             gotData = false;
 562         }
 563     }
 564
 565     int phaseIncrement = m_outputIncrements[cd.chunkCount];
 566
 567     int shiftIncrement = phaseIncrement;
 568     if (cd.chunkCount + 1 < m_outputIncrements.size()) {
 569         shiftIncrement = m_outputIncrements[cd.chunkCount + 1];
 570     }
 571
 572     if (phaseIncrement < 0) {
 573         phaseIncrement = -phaseIncrement;
 574         phaseReset = true;
 575     }
 576
 577     if (shiftIncrement < 0) {
 578         shiftIncrement = -shiftIncrement;
 579     }
 580
 581     if (shiftIncrement >= int(m_windowSize)) {
 582         cerr << "*** ERROR: RubberBandStretcher::Impl::processChunks: shiftIncrement " << shiftIncrement << " >= windowSize " << m_windowSize << " at " << cd.chunkCount << " (of " << m_outputIncrements.size() << ")" << endl;
 583         shiftIncrement = m_windowSize;
 584     }
 585
 586     phaseIncrementRtn = phaseIncrement;
 587     shiftIncrementRtn = shiftIncrement;
 588     if (cd.chunkCount == 0) phaseReset = true; // don't mess with the first chunk
 589     return gotData;
 590 }
 591
 592 void
 593 RubberBandStretcher::Impl::analyseChunk(size_t channel)
 594 {
 595     Profiler profiler("RubberBandStretcher::Impl::analyseChunk");
 596
 597     int i;
 598
 599     ChannelData &cd = *m_channelData[channel];
 600
 601     double *const R__ dblbuf = cd.dblbuf;
 602     float *const R__ fltbuf = cd.fltbuf;
 603
 604     int sz = m_windowSize;
 605     int hs = m_windowSize/2;
 606
 607     // cd.fltbuf is known to contain m_windowSize samples
 608
 609     m_window->cut(fltbuf);
 610
 611     if (cd.oversample > 1) {
 612
 613         int bufsiz = sz * cd.oversample;
 614         int offset = (bufsiz - sz) / 2;
 615
 616         // eek
 617
 618         for (i = 0; i < offset; ++i) {
 619             dblbuf[i] = 0.0;
 620         }
 621         for (i = 0; i < offset; ++i) {
 622             dblbuf[bufsiz - i - 1] = 0.0;
 623         }
 624         for (i = 0; i < sz; ++i) {
 625             dblbuf[offset + i] = fltbuf[i];
 626         }
 627         for (i = 0; i < bufsiz / 2; ++i) {
 628             double tmp = dblbuf[i];
 629             dblbuf[i] = dblbuf[i + bufsiz/2];
 630             dblbuf[i + bufsiz/2] = tmp;
 631         }
 632     } else {
 633         for (i = 0; i < hs; ++i) {
 634             dblbuf[i] = fltbuf[i + hs];
 635             dblbuf[i + hs] = fltbuf[i];
 636         }
 637     }
 638
 639     cd.fft->forwardPolar(dblbuf, cd.mag, cd.phase);
 640 }
 641
 642 static inline double mod(double x, double y) { return x - (y * floor(x / y)); }
 643 static inline double princarg(double a) { return mod(a + M_PI, -2.0 * M_PI) + M_PI; }
 644
 645 void
 646 RubberBandStretcher::Impl::modifyChunk(size_t channel,
 647                                        size_t outputIncrement,
 648                                        bool phaseReset)
 649 {
 650     Profiler profiler("RubberBandStretcher::Impl::modifyChunk");
 651
 652     ChannelData &cd = *m_channelData[channel];
 653
 654     if (phaseReset && m_debugLevel > 1) {
 655         cerr << "phase reset: leaving phases unmodified" << endl;
 656     }
 657
 658     const double rate = m_sampleRate;
 659     const int sz = m_windowSize;
 660     const int count = (sz * cd.oversample) / 2;
 661
 662     bool unchanged = cd.unchanged && (outputIncrement == m_increment);
 663     bool fullReset = phaseReset;
 664     bool laminar = !(m_options & OptionPhaseIndependent);
 665     bool bandlimited = (m_options & OptionTransientsMixed);
 666     int bandlow = lrint((150 * sz * cd.oversample) / rate);
 667     int bandhigh = lrint((1000 * sz * cd.oversample) / rate);
 668
 669     float freq0 = m_freq0;
 670     float freq1 = m_freq1;
 671     float freq2 = m_freq2;
 672
 673     if (laminar) {
 674         float r = getEffectiveRatio();
 675         if (r > 1) {
 676             float rf0 = 600 + (600 * ((r-1)*(r-1)*(r-1)*2));
 677             float f1ratio = freq1 / freq0;
 678             float f2ratio = freq2 / freq0;
 679             freq0 = std::max(freq0, rf0);
 680             freq1 = freq0 * f1ratio;
 681             freq2 = freq0 * f2ratio;
 682         }
 683     }
 684
 685     int limit0 = lrint((freq0 * sz * cd.oversample) / rate);
 686     int limit1 = lrint((freq1 * sz * cd.oversample) / rate);
 687     int limit2 = lrint((freq2 * sz * cd.oversample) / rate);
 688
 689     if (limit1 < limit0) limit1 = limit0;
 690     if (limit2 < limit1) limit2 = limit1;
 691
 692     double prevInstability = 0.0;
 693     bool prevDirection = false;
 694
 695     double distance = 0.0;
 696     const double maxdist = 8.0;
 697
 698     const int lookback = 1;
 699
 700     double distacc = 0.0;
 701
 702     for (int i = count; i >= 0; i -= lookback) {
 703
 704         bool resetThis = phaseReset;
 705
 706         if (bandlimited) {
 707             if (resetThis) {
 708                 if (i > bandlow && i < bandhigh) {
 709                     resetThis = false;
 710                     fullReset = false;
 711                 }
 712             }
 713         }
 714
 715         double p = cd.phase[i];
 716         double perr = 0.0;
 717         double outphase = p;
 718
 719         double mi = maxdist;
 720         if (i <= limit0) mi = 0.0;
 721         else if (i <= limit1) mi = 1.0;
 722         else if (i <= limit2) mi = 3.0;
 723
 724         if (!resetThis) {
 725
 726             double omega = (2 * M_PI * m_increment * i) / (sz * cd.oversample);
 727
 728             double pp = cd.prevPhase[i];
 729             double ep = pp + omega;
 730             perr = princarg(p - ep);
 731
 732             double instability = fabs(perr - cd.prevError[i]);
 733             bool direction = (perr > cd.prevError[i]);
 734
 735             bool inherit = false;
 736
 737             if (laminar) {
 738                 if (distance >= mi) {
 739                     inherit = false;
 740                 } else if (bandlimited && (i == bandhigh || i == bandlow)) {
 741                     inherit = false;
 742                 } else if (instability > prevInstability &&
 743                            direction == prevDirection) {
 744                     inherit = true;
 745                 }
 746             }
 747
 748             double advance = outputIncrement * ((omega + perr) / m_increment);
 749
 750             if (inherit) {
 751                 double inherited =
 752                     cd.unwrappedPhase[i + lookback] - cd.prevPhase[i + lookback];
 753                 advance = ((advance * distance) +
 754                            (inherited * (maxdist - distance)))
 755                     / maxdist;
 756                 outphase = p + advance;
 757                 distacc += distance;
 758                 distance += 1.0;
 759             } else {
 760                 outphase = cd.unwrappedPhase[i] + advance;
 761                 distance = 0.0;
 762             }
 763
 764             prevInstability = instability;
 765             prevDirection = direction;
 766
 767         } else {
 768             distance = 0.0;
 769         }
 770
 771         cd.prevError[i] = perr;
 772         cd.prevPhase[i] = p;
 773         cd.phase[i] = outphase;
 774         cd.unwrappedPhase[i] = outphase;
 775     }
 776
 777     if (m_debugLevel > 1) {
 778         cerr << "mean inheritance distance = " << distacc / count << endl;
 779     }
 780
 781     if (fullReset) unchanged = true;
 782     cd.unchanged = unchanged;
 783
 784     if (unchanged && m_debugLevel > 1) {
 785         cerr << "frame unchanged on channel " << channel << endl;
 786     }
 787 }
 788
 789
 790 void
 791 RubberBandStretcher::Impl::formantShiftChunk(size_t channel)
 792 {
 793     Profiler profiler("RubberBandStretcher::Impl::formantShiftChunk");
 794
 795     ChannelData &cd = *m_channelData[channel];
 796
 797     double *const R__ mag = cd.mag;
 798     double *const R__ envelope = cd.envelope;
 799     double *const R__ dblbuf = cd.dblbuf;
 800
 801     const int sz = m_windowSize;
 802     const int hs = m_windowSize/2;
 803     const double denom = sz;
 804
 805
 806     cd.fft->inverseCepstral(mag, dblbuf);
 807
 808     for (int i = 0; i < sz; ++i) {
 809         dblbuf[i] /= denom;
 810     }
 811
 812     const int cutoff = m_sampleRate / 700;
 813
 814 //    cerr <<"cutoff = "<< cutoff << ", m_sampleRate/cutoff = " << m_sampleRate/cutoff << endl;
 815
 816     dblbuf[0] /= 2;
 817     dblbuf[cutoff-1] /= 2;
 818
 819     for (int i = cutoff; i < sz; ++i) {
 820         dblbuf[i] = 0.0;
 821     }
 822
 823     cd.fft->forward(dblbuf, envelope, 0);
 824
 825
 826     for (int i = 0; i <= hs; ++i) {
 827         envelope[i] = exp(envelope[i]);
 828     }
 829     for (int i = 0; i <= hs; ++i) {
 830         mag[i] /= envelope[i];
 831     }
 832
 833     if (m_pitchScale > 1.0) {
 834         // scaling up, we want a new envelope that is lower by the pitch factor
 835         for (int target = 0; target <= hs; ++target) {
 836             int source = lrint(target * m_pitchScale);
 837             if (source > int(m_windowSize)) {
 838                 envelope[target] = 0.0;
 839             } else {
 840                 envelope[target] = envelope[source];
 841             }
 842         }
 843     } else {
 844         // scaling down, we want a new envelope that is higher by the pitch factor
 845         for (int target = hs; target > 0; ) {
 846             --target;
 847             int source = lrint(target * m_pitchScale);
 848             envelope[target] = envelope[source];
 849         }
 850     }
 851
 852     for (int i = 0; i <= hs; ++i) {
 853         mag[i] *= envelope[i];
 854     }
 855
 856     cd.unchanged = false;
 857 }
 858
 859 void
 860 RubberBandStretcher::Impl::synthesiseChunk(size_t channel)
 861 {
 862     Profiler profiler("RubberBandStretcher::Impl::synthesiseChunk");
 863
 864
 865     if ((m_options & OptionFormantPreserved) &&
 866         (m_pitchScale != 1.0)) {
 867         formantShiftChunk(channel);
 868     }
 869
 870     ChannelData &cd = *m_channelData[channel];
 871
 872     double *const R__ dblbuf = cd.dblbuf;
 873     float *const R__ fltbuf = cd.fltbuf;
 874     float *const R__ accumulator = cd.accumulator;
 875     float *const R__ windowAccumulator = cd.windowAccumulator;
 876
 877     int sz = m_windowSize;
 878     int hs = m_windowSize/2;
 879     int i;
 880
 881
 882     if (!cd.unchanged) {
 883
 884         cd.fft->inversePolar(cd.mag, cd.phase, cd.dblbuf);
 885
 886         if (cd.oversample > 1) {
 887
 888             int bufsiz = sz * cd.oversample;
 889             int hbs = hs * cd.oversample;
 890             int offset = (bufsiz - sz) / 2;
 891
 892             for (i = 0; i < hbs; ++i) {
 893                 double tmp = dblbuf[i];
 894                 dblbuf[i] = dblbuf[i + hbs];
 895                 dblbuf[i + hbs] = tmp;
 896             }
 897             for (i = 0; i < sz; ++i) {
 898                 fltbuf[i] = float(dblbuf[i + offset]);
 899             }
 900         } else {
 901             for (i = 0; i < hs; ++i) {
 902                 fltbuf[i] = float(dblbuf[i + hs]);
 903             }
 904             for (i = 0; i < hs; ++i) {
 905                 fltbuf[i + hs] = float(dblbuf[i]);
 906             }
 907         }
 908
 909         float denom = float(sz * cd.oversample);
 910
 911         // our ffts produced unscaled results
 912         for (i = 0; i < sz; ++i) {
 913             fltbuf[i] = fltbuf[i] / denom;
 914         }
 915     }
 916
 917     m_window->cut(fltbuf);
 918
 919     for (i = 0; i < sz; ++i) {
 920         accumulator[i] += fltbuf[i];
 921     }
 922
 923     cd.accumulatorFill = m_windowSize;
 924
 925     float fixed = m_window->getArea() * 1.5f;
 926
 927     for (i = 0; i < sz; ++i) {
 928         float val = m_window->getValue(i);
 929         windowAccumulator[i] += val * fixed;
 930     }
 931 }
 932
 933 void
 934 RubberBandStretcher::Impl::writeChunk(size_t channel, size_t shiftIncrement, bool last)
 935 {
 936     Profiler profiler("RubberBandStretcher::Impl::writeChunk");
 937
 938     ChannelData &cd = *m_channelData[channel];
 939
 940     float *const R__ accumulator = cd.accumulator;
 941     float *const R__ windowAccumulator = cd.windowAccumulator;
 942
 943     const int sz = m_windowSize;
 944     const int si = shiftIncrement;
 945
 946     int i;
 947
 948     if (m_debugLevel > 2) {
 949         cerr << "writeChunk(" << channel << ", " << shiftIncrement << ", " << last << ")" << endl;
 950     }
 951
 952     for (i = 0; i < si; ++i) {
 953         if (windowAccumulator[i] > 0.f) {
 954             accumulator[i] /= windowAccumulator[i];
 955         }
 956     }
 957
 958     // for exact sample scaling (probably not meaningful if we
 959     // were running in RT mode)
 960     size_t theoreticalOut = 0;
 961     if (cd.inputSize >= 0) {
 962         theoreticalOut = lrint(cd.inputSize * m_timeRatio);
 963     }
 964
 965     bool resampledAlready = resampleBeforeStretching();
 966
 967     if (!resampledAlready &&
 968         (m_pitchScale != 1.0 || m_options & OptionPitchHighConsistency) &&
 969         cd.resampler) {
 970
 971         size_t reqSize = int(ceil(si / m_pitchScale));
 972         if (reqSize > cd.resamplebufSize) {
 973             // This shouldn't normally happen -- the buffer is
 974             // supposed to be initialised with enough space in the
 975             // first place.  But we retain this check in case the
 976             // pitch scale has changed since then, or the stretch
 977             // calculator has gone mad, or something.
 978             cerr << "WARNING: RubberBandStretcher::Impl::writeChunk: resizing resampler buffer from "
 979                       << cd.resamplebufSize << " to " << reqSize << endl;
 980             cd.setResampleBufSize(reqSize);
 981         }
 982
 983
 984         size_t outframes = cd.resampler->resample(&cd.accumulator,
 985                                                   &cd.resamplebuf,
 986                                                   si,
 987                                                   1.0 / m_pitchScale,
 988                                                   last);
 989
 990
 991         writeOutput(*cd.outbuf, cd.resamplebuf,
 992                     outframes, cd.outCount, theoreticalOut);
 993
 994     } else {
 995         writeOutput(*cd.outbuf, accumulator,
 996                     si, cd.outCount, theoreticalOut);
 997     }
 998
 999     for (i = 0; i < sz - si; ++i) {
1000         accumulator[i] = accumulator[i + si];
1001     }
1002
1003     for (i = sz - si; i < sz; ++i) {
1004         accumulator[i] = 0.0f;
1005     }
1006
1007     for (i = 0; i < sz - si; ++i) {
1008         windowAccumulator[i] = windowAccumulator[i + si];
1009     }
1010
1011     for (i = sz - si; i < sz; ++i) {
1012         windowAccumulator[i] = 0.0f;
1013     }
1014
1015     if (int(cd.accumulatorFill) > si) {
1016         cd.accumulatorFill -= si;
1017     } else {
1018         cd.accumulatorFill = 0;
1019         if (cd.draining) {
1020             if (m_debugLevel > 1) {
1021                 cerr << "RubberBandStretcher::Impl::processChunks: setting outputComplete to true" << endl;
1022             }
1023             cd.outputComplete = true;
1024         }
1025     }
1026 }
1027
1028 void
1029 RubberBandStretcher::Impl::writeOutput(RingBuffer<float> &to, float *from, size_t qty, size_t &outCount, size_t theoreticalOut)
1030 {
1031     Profiler profiler("RubberBandStretcher::Impl::writeOutput");
1032
1033     // In non-RT mode, we don't want to write the first startSkip
1034     // samples, because the first chunk is centred on the start of the
1035     // output.  In RT mode we didn't apply any pre-padding in
1036     // configure(), so we don't want to remove any here.
1037
1038     size_t startSkip = 0;
1039     if (!m_realtime) {
1040         startSkip = lrintf((m_windowSize/2) / m_pitchScale);
1041     }
1042
1043     if (outCount > startSkip) {
1044
1045         // this is the normal case
1046
1047         if (theoreticalOut > 0) {
1048             if (m_debugLevel > 1) {
1049                 cerr << "theoreticalOut = " << theoreticalOut
1050                      << ", outCount = " << outCount
1051                      << ", startSkip = " << startSkip
1052                      << ", qty = " << qty << endl;
1053             }
1054             if (outCount - startSkip <= theoreticalOut &&
1055                 outCount - startSkip + qty > theoreticalOut) {
1056                 qty = theoreticalOut - (outCount - startSkip);
1057                 if (m_debugLevel > 1) {
1058                     cerr << "reduce qty to " << qty << endl;
1059                 }
1060             }
1061         }
1062
1063         if (m_debugLevel > 2) {
1064             cerr << "writing " << qty << endl;
1065         }
1066
1067         size_t written = to.write(from, qty);
1068
1069         if (written < qty) {
1070             cerr << "WARNING: RubberBandStretcher::Impl::writeOutput: "
1071                  << "Buffer overrun on output: wrote " << written
1072                  << " of " << qty << " samples" << endl;
1073         }
1074
1075         outCount += written;
1076         return;
1077     }
1078
1079     // the rest of this is only used during the first startSkip samples
1080
1081     if (outCount + qty <= startSkip) {
1082         if (m_debugLevel > 1) {
1083             cerr << "qty = " << qty << ", startSkip = "
1084                  << startSkip << ", outCount = " << outCount
1085                  << ", discarding" << endl;
1086         }
1087         outCount += qty;
1088         return;
1089     }
1090
1091     size_t off = startSkip - outCount;
1092     if (m_debugLevel > 1) {
1093         cerr << "qty = " << qty << ", startSkip = "
1094              << startSkip << ", outCount = " << outCount
1095              << ", writing " << qty - off
1096              << " from start offset " << off << endl;
1097     }
1098     to.write(from + off, qty - off);
1099     outCount += qty;
1100 }
1101
1102 int
1103 RubberBandStretcher::Impl::available() const
1104 {
1105     Profiler profiler("RubberBandStretcher::Impl::available");
1106
1107     if (m_threaded) {
1108         MutexLocker locker(&m_threadSetMutex);
1109         if (m_channelData.empty()) return 0;
1110     } else {
1111         if (m_channelData.empty()) return 0;
1112     }
1113
1114     if (!m_threaded) {
1115         for (size_t c = 0; c < m_channels; ++c) {
1116             if (m_channelData[c]->inputSize >= 0) {
1117 //                cerr << "available: m_done true" << endl;
1118                 if (m_channelData[c]->inbuf->getReadSpace() > 0) {
1119 //                    cerr << "calling processChunks(" << c << ") from available" << endl;
1120                     //!!! do we ever actually do this? if so, this method should not be const
1121                     // ^^^ yes, we do sometimes -- e.g. when fed a very short file
1122                     bool any = false, last = false;
1123                     ((RubberBandStretcher::Impl *)this)->processChunks(c, any, last);
1124                 }
1125             }
1126         }
1127     }
1128
1129     size_t min = 0;
1130     bool consumed = true;
1131     bool haveResamplers = false;
1132
1133     for (size_t i = 0; i < m_channels; ++i) {
1134         size_t availIn = m_channelData[i]->inbuf->getReadSpace();
1135         size_t availOut = m_channelData[i]->outbuf->getReadSpace();
1136         if (m_debugLevel > 2) {
1137             cerr << "available on channel " << i << ": " << availOut << " (waiting: " << availIn << ")" << endl;
1138         }
1139         if (i == 0 || availOut < min) min = availOut;
1140         if (!m_channelData[i]->outputComplete) consumed = false;
1141         if (m_channelData[i]->resampler) haveResamplers = true;
1142     }
1143
1144     if (min == 0 && consumed) return -1;
1145     if (m_pitchScale == 1.0) return min;
1146
1147     if (haveResamplers) return min; // resampling has already happened
1148     return int(floor(min / m_pitchScale));
1149 }
1150
1151 size_t
1152 RubberBandStretcher::Impl::retrieve(float *const *output, size_t samples) const
1153 {
1154     Profiler profiler("RubberBandStretcher::Impl::retrieve");
1155
1156     size_t got = samples;
1157
1158     for (size_t c = 0; c < m_channels; ++c) {
1159         size_t gotHere = m_channelData[c]->outbuf->read(output[c], got);
1160         if (gotHere < got) {
1161             if (c > 0) {
1162                 if (m_debugLevel > 0) {
1163                     cerr << "RubberBandStretcher::Impl::retrieve: WARNING: channel imbalance detected" << endl;
1164                 }
1165             }
1166             got = gotHere;
1167         }
1168     }
1169
1170     return got;
1171 }
1172
1173 }
1174