alexbrandmeyer@609: //
alexbrandmeyer@609: //  ear.cc
alexbrandmeyer@609: //  CARFAC Open Source C++ Library
alexbrandmeyer@609: //
alexbrandmeyer@609: //  Created by Alex Brandmeyer on 5/10/13.
alexbrandmeyer@609: //
alexbrandmeyer@609: // This C++ file is part of an implementation of Lyon's cochlear model:
alexbrandmeyer@609: // "Cascade of Asymmetric Resonators with Fast-Acting Compression"
alexbrandmeyer@609: // to supplement Lyon's upcoming book "Human and Machine Hearing"
alexbrandmeyer@609: //
alexbrandmeyer@609: // Licensed under the Apache License, Version 2.0 (the "License");
alexbrandmeyer@609: // you may not use this file except in compliance with the License.
alexbrandmeyer@609: // You may obtain a copy of the License at
alexbrandmeyer@609: //
alexbrandmeyer@609: //     http://www.apache.org/licenses/LICENSE-2.0
alexbrandmeyer@609: //
alexbrandmeyer@609: // Unless required by applicable law or agreed to in writing, software
alexbrandmeyer@609: // distributed under the License is distributed on an "AS IS" BASIS,
alexbrandmeyer@609: // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
alexbrandmeyer@609: // See the License for the specific language governing permissions and
alexbrandmeyer@609: // limitations under the License.
alexbrandmeyer@609: 
ronw@628: #include <assert.h>
alexbrandmeyer@609: #include "ear.h"
alexbrandmeyer@609: 
alexbrandmeyer@610: // The 'InitEar' function takes a set of model parameters and initializes the
alexbrandmeyer@610: // design coefficients and model state variables needed for running the model
alexbrandmeyer@626: // on a single audio channel.
alexbrandmeyer@637: void Ear::InitEar(const int n_ch, const FPType fs, const CARCoeffs& car_coeffs,
alexbrandmeyer@637:                   const IHCCoeffs& ihc_coeffs,
alexbrandmeyer@637:                   const std::vector<AGCCoeffs>& agc_coeffs) {
alexbrandmeyer@610:   // The first section of code determines the number of channels that will be
alexbrandmeyer@610:   // used in the model on the basis of the sample rate and the CAR parameters
alexbrandmeyer@610:   // that have been passed to this function.
alexbrandmeyer@610:   n_ch_ = n_ch;
alexbrandmeyer@636:   car_coeffs_ = car_coeffs;
alexbrandmeyer@636:   ihc_coeffs_ = ihc_coeffs;
alexbrandmeyer@636:   agc_coeffs_ = agc_coeffs;
alexbrandmeyer@610:   // Once the coefficients have been determined, we can initialize the state
alexbrandmeyer@610:   // variables that will be used during runtime.
alexbrandmeyer@610:   InitCARState();
alexbrandmeyer@610:   InitIHCState();
alexbrandmeyer@610:   InitAGCState();
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: void Ear::InitCARState() {
alexbrandmeyer@640:   car_state_.z1_memory.setZero(n_ch_);
alexbrandmeyer@640:   car_state_.z2_memory.setZero(n_ch_);
alexbrandmeyer@640:   car_state_.za_memory.setZero(n_ch_);
alexbrandmeyer@640:   car_state_.zb_memory = car_coeffs_.zr_coeffs;
alexbrandmeyer@640:   car_state_.dzb_memory.setZero(n_ch_);
alexbrandmeyer@640:   car_state_.zy_memory.setZero(n_ch_);
alexbrandmeyer@640:   car_state_.g_memory = car_coeffs_.g0_coeffs;
alexbrandmeyer@640:   car_state_.dg_memory.setZero(n_ch_);
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::InitIHCState() {
alexbrandmeyer@640:   ihc_state_.ihc_accum = FloatArray::Zero(n_ch_);
alexbrandmeyer@640:   if (! ihc_coeffs_.just_half_wave_rectify) {
alexbrandmeyer@640:     ihc_state_.ac_coupler.setZero(n_ch_);
alexbrandmeyer@640:     ihc_state_.lpf1_state.setConstant(n_ch_, ihc_coeffs_.rest_output);
alexbrandmeyer@640:     ihc_state_.lpf2_state.setConstant(n_ch_, ihc_coeffs_.rest_output);
alexbrandmeyer@640:     if (ihc_coeffs_.one_capacitor) {
alexbrandmeyer@640:       ihc_state_.cap1_voltage.setConstant(n_ch_, ihc_coeffs_.rest_cap1);
alexbrandmeyer@610:     } else {
alexbrandmeyer@640:       ihc_state_.cap1_voltage.setConstant(n_ch_, ihc_coeffs_.rest_cap1);
alexbrandmeyer@640:       ihc_state_.cap2_voltage.setConstant(n_ch_, ihc_coeffs_.rest_cap2);
alexbrandmeyer@610:     }
alexbrandmeyer@610:   }
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::InitAGCState() {
alexbrandmeyer@626:   int n_agc_stages = agc_coeffs_.size();
alexbrandmeyer@626:   agc_state_.resize(n_agc_stages);
alexbrandmeyer@636:   for (auto& stage_state : agc_state_) {
alexbrandmeyer@640:     stage_state.decim_phase = 0;
alexbrandmeyer@640:     stage_state.agc_memory.setZero(n_ch_);
alexbrandmeyer@640:     stage_state.input_accum.setZero(n_ch_);
alexbrandmeyer@610:   }
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@637: void Ear::CARStep(const FPType input) {
alexbrandmeyer@626:   // This interpolates g.
alexbrandmeyer@640:   car_state_.g_memory = car_state_.g_memory + car_state_.dg_memory;
alexbrandmeyer@610:   // This calculates the AGC interpolation state.
alexbrandmeyer@640:   car_state_.zb_memory = car_state_.zb_memory + car_state_.dzb_memory;
alexbrandmeyer@610:   // This updates the nonlinear function of 'velocity' along with zA, which is
alexbrandmeyer@610:   // a delay of z2.
alexbrandmeyer@626:   FloatArray nonlinear_fun(n_ch_);
alexbrandmeyer@640:   FloatArray velocities = car_state_.z2_memory - car_state_.za_memory;
alexbrandmeyer@626:   OHCNonlinearFunction(velocities, &nonlinear_fun);
alexbrandmeyer@626:   // Here, zb_memory_ * nonlinear_fun is "undamping" delta r.
alexbrandmeyer@640:   FloatArray r = car_coeffs_.r1_coeffs + (car_state_.zb_memory *
alexbrandmeyer@626:                                            nonlinear_fun);
alexbrandmeyer@640:   car_state_.za_memory = car_state_.z2_memory;
alexbrandmeyer@610:   // Here we reduce the CAR state by r and rotate with the fixed cos/sin coeffs.
alexbrandmeyer@640:   FloatArray z1  = r * ((car_coeffs_.a0_coeffs * car_state_.z1_memory) -
alexbrandmeyer@640:                         (car_coeffs_.c0_coeffs * car_state_.z2_memory));
alexbrandmeyer@640:   car_state_.z2_memory = r *
alexbrandmeyer@640:     ((car_coeffs_.c0_coeffs * car_state_.z1_memory) +
alexbrandmeyer@640:      (car_coeffs_.a0_coeffs * car_state_.z2_memory));
alexbrandmeyer@640:   car_state_.zy_memory = car_coeffs_.h_coeffs * car_state_.z2_memory;
alexbrandmeyer@610:   // This section ripples the input-output path, to avoid delay...
alexbrandmeyer@610:   // It's the only part that doesn't get computed "in parallel":
alexbrandmeyer@626:   FPType in_out = input;
alexbrandmeyer@610:   for (int ch = 0; ch < n_ch_; ch++) {
alexbrandmeyer@609:     z1(ch) = z1(ch) + in_out;
alexbrandmeyer@610:     // This performs the ripple, and saves the final channel outputs in zy.
alexbrandmeyer@640:     in_out = car_state_.g_memory(ch) * (in_out + car_state_.zy_memory(ch));
alexbrandmeyer@640:     car_state_.zy_memory(ch) = in_out;
alexbrandmeyer@609:   }
alexbrandmeyer@640:   car_state_.z1_memory = z1;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: // We start with a quadratic nonlinear function, and limit it via a
alexbrandmeyer@610: // rational function. This makes the result go to zero at high
alexbrandmeyer@609: // absolute velocities, so it will do nothing there.
alexbrandmeyer@626: void Ear::OHCNonlinearFunction(const FloatArray& velocities,
alexbrandmeyer@626:                                FloatArray* nonlinear_fun) {
alexbrandmeyer@640:   *nonlinear_fun = (1 + ((velocities * car_coeffs_.velocity_scale) +
alexbrandmeyer@640:                          car_coeffs_.v_offset).square()).inverse();
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: // This step is a one sample-time update of the inner-hair-cell (IHC) model,
alexbrandmeyer@610: // including the detection nonlinearity and either one or two capacitor state
alexbrandmeyer@610: // variables.
alexbrandmeyer@637: void Ear::IHCStep(const FloatArray& car_out) {
alexbrandmeyer@640:   FloatArray ac_diff = car_out - ihc_state_.ac_coupler;
alexbrandmeyer@640:   ihc_state_.ac_coupler = ihc_state_.ac_coupler +
alexbrandmeyer@640:     (ihc_coeffs_.ac_coeff * ac_diff);
alexbrandmeyer@640:   if (ihc_coeffs_.just_half_wave_rectify) {
alexbrandmeyer@626:     FloatArray output(n_ch_);
alexbrandmeyer@626:     for (int ch = 0; ch < n_ch_; ++ch) {
alexbrandmeyer@626:       FPType a = (ac_diff(ch) > 0.0) ? ac_diff(ch) : 0.0;
alexbrandmeyer@626:       output(ch) = (a < 2) ? a : 2;
alexbrandmeyer@609:     }
alexbrandmeyer@640:     ihc_state_.ihc_out = output;
alexbrandmeyer@609:   } else {
alexbrandmeyer@626:     FloatArray conductance = CARFACDetect(ac_diff);
alexbrandmeyer@640:     if (ihc_coeffs_.one_capacitor) {
alexbrandmeyer@640:       ihc_state_.ihc_out = conductance * ihc_state_.cap1_voltage;
alexbrandmeyer@640:       ihc_state_.cap1_voltage = ihc_state_.cap1_voltage -
alexbrandmeyer@640:         (ihc_state_.ihc_out * ihc_coeffs_.out1_rate) +
alexbrandmeyer@640:         ((1 - ihc_state_.cap1_voltage) * ihc_coeffs_.in1_rate);
alexbrandmeyer@609:     } else {
alexbrandmeyer@640:       ihc_state_.ihc_out = conductance * ihc_state_.cap2_voltage;
alexbrandmeyer@640:       ihc_state_.cap1_voltage = ihc_state_.cap1_voltage -
alexbrandmeyer@640:         ((ihc_state_.cap1_voltage - ihc_state_.cap2_voltage)
alexbrandmeyer@640:          * ihc_coeffs_.out1_rate) + ((1 - ihc_state_.cap1_voltage) *
alexbrandmeyer@640:                                       ihc_coeffs_.in1_rate);
alexbrandmeyer@640:       ihc_state_.cap2_voltage = ihc_state_.cap2_voltage -
alexbrandmeyer@640:         (ihc_state_.ihc_out * ihc_coeffs_.out2_rate) +
alexbrandmeyer@640:         ((ihc_state_.cap1_voltage - ihc_state_.cap2_voltage)
alexbrandmeyer@640:          * ihc_coeffs_.in2_rate);
alexbrandmeyer@609:     }
alexbrandmeyer@610:     // Here we smooth the output twice using a LPF.
alexbrandmeyer@640:     ihc_state_.ihc_out *= ihc_coeffs_.output_gain;
alexbrandmeyer@640:     ihc_state_.lpf1_state += ihc_coeffs_.lpf_coeff *
alexbrandmeyer@640:       (ihc_state_.ihc_out - ihc_state_.lpf1_state);
alexbrandmeyer@640:     ihc_state_.lpf2_state += ihc_coeffs_.lpf_coeff *
alexbrandmeyer@640:       (ihc_state_.lpf1_state - ihc_state_.lpf2_state);
alexbrandmeyer@640:     ihc_state_.ihc_out = ihc_state_.lpf2_state - ihc_coeffs_.rest_output;
alexbrandmeyer@609:   }
alexbrandmeyer@640:   ihc_state_.ihc_accum += ihc_state_.ihc_out;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@626: bool Ear::AGCStep(const FloatArray& ihc_out) {
alexbrandmeyer@609:   int stage = 0;
alexbrandmeyer@640:   int n_stages = agc_coeffs_[0].n_agc_stages;
alexbrandmeyer@640:   FPType detect_scale = agc_coeffs_[n_stages - 1].detect_scale;
alexbrandmeyer@626:   bool updated = AGCRecurse(stage, detect_scale * ihc_out);
alexbrandmeyer@609:   return updated;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@626: bool Ear::AGCRecurse(const int stage, FloatArray agc_in) {
alexbrandmeyer@609:   bool updated = true;
alexbrandmeyer@636:   const auto& agc_coeffs = agc_coeffs_[stage];
alexbrandmeyer@636:   auto& agc_state = agc_state_[stage];
alexbrandmeyer@610:   // This is the decim factor for this stage, relative to input or prev. stage:
alexbrandmeyer@640:   int decim = agc_coeffs.decimation;
alexbrandmeyer@610:   // This is the decim phase of this stage (do work on phase 0 only):
alexbrandmeyer@640:   int decim_phase = agc_state.decim_phase + 1;
alexbrandmeyer@609:   decim_phase = decim_phase % decim;
alexbrandmeyer@640:   agc_state.decim_phase = decim_phase;
alexbrandmeyer@610:   // Here we accumulate input for this stage from the previous stage:
alexbrandmeyer@640:   agc_state.input_accum += agc_in;
alexbrandmeyer@610:   // We don't do anything if it's not the right decim_phase.
alexbrandmeyer@610:   if (decim_phase == 0) {
alexbrandmeyer@610:     // Now we do lots of work, at the decimated rate.
alexbrandmeyer@610:     // These are the decimated inputs for this stage, which will be further
alexbrandmeyer@610:     // decimated at the next stage.
alexbrandmeyer@640:     agc_in = agc_state.input_accum / decim;
alexbrandmeyer@610:     // This resets the accumulator.
alexbrandmeyer@640:     agc_state.input_accum = FloatArray::Zero(n_ch_);
alexbrandmeyer@626:     if (stage < (agc_coeffs_.size() - 1)) {
alexbrandmeyer@610:       // Now we recurse to evaluate the next stage(s).
alexbrandmeyer@610:       updated = AGCRecurse(stage + 1, agc_in);
alexbrandmeyer@610:       // Afterwards we add its output to this stage input, whether it updated or
alexbrandmeyer@610:       // not.
alexbrandmeyer@640:       agc_in += agc_coeffs.agc_stage_gain *
alexbrandmeyer@640:         agc_state_[stage + 1].agc_memory;
alexbrandmeyer@609:     }
alexbrandmeyer@610:     // This performs a first-order recursive smoothing filter update, in time.
alexbrandmeyer@640:     agc_state.agc_memory += agc_coeffs.agc_epsilon *
alexbrandmeyer@640:       (agc_in - agc_state.agc_memory);
alexbrandmeyer@640:     AGCSpatialSmooth(agc_coeffs_[stage], &agc_state.agc_memory);
alexbrandmeyer@626:     updated = true;
alexbrandmeyer@609:   } else {
alexbrandmeyer@609:     updated = false;
alexbrandmeyer@609:   }
alexbrandmeyer@609:   return updated;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@637: void Ear::AGCSpatialSmooth(const AGCCoeffs& agc_coeffs,
alexbrandmeyer@637:                            FloatArray* stage_state) {
alexbrandmeyer@640:   int n_iterations = agc_coeffs.agc_spatial_iterations;
alexbrandmeyer@609:   bool use_fir;
alexbrandmeyer@610:   use_fir = (n_iterations < 4) ? true : false;
alexbrandmeyer@609:   if (use_fir) {
alexbrandmeyer@640:     FPType fir_coeffs_left = agc_coeffs.agc_spatial_fir_left;
alexbrandmeyer@640:     FPType fir_coeffs_mid = agc_coeffs.agc_spatial_fir_mid;
alexbrandmeyer@640:     FPType fir_coeffs_right = agc_coeffs.agc_spatial_fir_right;
alexbrandmeyer@609:     FloatArray ss_tap1(n_ch_);
alexbrandmeyer@609:     FloatArray ss_tap2(n_ch_);
alexbrandmeyer@609:     FloatArray ss_tap3(n_ch_);
alexbrandmeyer@609:     FloatArray ss_tap4(n_ch_);
alexbrandmeyer@640:     int n_taps = agc_coeffs.agc_spatial_n_taps;
alexbrandmeyer@610:     // This initializes the first two taps of stage state, which are used for
alexbrandmeyer@610:     // both possible cases.
alexbrandmeyer@637:     ss_tap1(0) = (*stage_state)(0);
alexbrandmeyer@637:     ss_tap1.block(1, 0, n_ch_ - 1, 1) = stage_state->block(0, 0, n_ch_ - 1, 1);
alexbrandmeyer@637:     ss_tap2(n_ch_ - 1) = (*stage_state)(n_ch_ - 1);
alexbrandmeyer@637:     ss_tap2.block(0, 0, n_ch_ - 1, 1) = stage_state->block(1, 0, n_ch_ - 1, 1);
alexbrandmeyer@609:     switch (n_taps) {
alexbrandmeyer@609:       case 3:
alexbrandmeyer@637:         *stage_state = (fir_coeffs_left * ss_tap1) +
alexbrandmeyer@637:           (fir_coeffs_mid * *stage_state) + (fir_coeffs_right * ss_tap2);
alexbrandmeyer@609:         break;
alexbrandmeyer@609:       case 5:
alexbrandmeyer@610:         // Now we initialize last two taps of stage state, which are only used
alexbrandmeyer@610:         // for the 5-tap case.
alexbrandmeyer@637:         ss_tap3(0) = (*stage_state)(0);
alexbrandmeyer@637:         ss_tap3(1) = (*stage_state)(1);
alexbrandmeyer@610:         ss_tap3.block(2, 0, n_ch_ - 2, 1) =
alexbrandmeyer@637:           stage_state->block(0, 0, n_ch_ - 2, 1);
alexbrandmeyer@637:         ss_tap4(n_ch_ - 2) = (*stage_state)(n_ch_ - 1);
alexbrandmeyer@637:         ss_tap4(n_ch_ - 1) = (*stage_state)(n_ch_ - 2);
alexbrandmeyer@610:         ss_tap4.block(0, 0, n_ch_ - 2, 1) =
alexbrandmeyer@637:         stage_state->block(2, 0, n_ch_ - 2, 1);
alexbrandmeyer@637:         *stage_state = (fir_coeffs_left * (ss_tap3 + ss_tap1)) +
alexbrandmeyer@637:           (fir_coeffs_mid * *stage_state) +
alexbrandmeyer@637:           (fir_coeffs_right * (ss_tap2 + ss_tap4));
alexbrandmeyer@609:         break;
alexbrandmeyer@609:       default:
ronw@628:         assert(true && "Bad n_taps in AGCSpatialSmooth; should be 3 or 5.");
alexbrandmeyer@611:         break;
alexbrandmeyer@609:     }
alexbrandmeyer@609:   } else {
alexbrandmeyer@640:     AGCSmoothDoubleExponential(agc_coeffs.agc_pole_z1, agc_coeffs.agc_pole_z2,
alexbrandmeyer@640:                                stage_state);
alexbrandmeyer@609:   }
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@637: void Ear::AGCSmoothDoubleExponential(const FPType pole_z1,  const FPType pole_z2,
alexbrandmeyer@637:                                 FloatArray* stage_state) {
alexbrandmeyer@637:   int32_t n_pts = stage_state->size();
alexbrandmeyer@610:   FPType input;
alexbrandmeyer@611:   FPType state = 0.0;
alexbrandmeyer@626:   // TODO (alexbrandmeyer): I'm assuming one dimensional input for now, but this
alexbrandmeyer@610:   // should be verified with Dick for the final version
alexbrandmeyer@637:   for (int i = n_pts - 11; i < n_pts; ++i) {
alexbrandmeyer@637:     input = (*stage_state)(i);
alexbrandmeyer@610:     state = state + (1 - pole_z1) * (input - state);
alexbrandmeyer@610:   }
alexbrandmeyer@637:   for (int i = n_pts - 1; i > -1; --i) {
alexbrandmeyer@637:     input = (*stage_state)(i);
alexbrandmeyer@610:     state = state + (1 - pole_z2) * (input - state);
alexbrandmeyer@610:   }
alexbrandmeyer@637:   for (int i = 0; i < n_pts; ++i) {
alexbrandmeyer@637:     input = (*stage_state)(i);
alexbrandmeyer@610:     state = state + (1 - pole_z1) * (input - state);
alexbrandmeyer@637:     (*stage_state)(i) = state;
alexbrandmeyer@610:   }
alexbrandmeyer@609: }
alexbrandmeyer@610: 
alexbrandmeyer@626: FloatArray Ear::StageGValue(const FloatArray& undamping) {
alexbrandmeyer@640:   FloatArray r = car_coeffs_.r1_coeffs + car_coeffs_.zr_coeffs * undamping;
alexbrandmeyer@640:   return (1 - 2 * r * car_coeffs_.a0_coeffs + (r * r)) /
alexbrandmeyer@640:     (1 - 2 * r * car_coeffs_.a0_coeffs + car_coeffs_.h_coeffs * r *
alexbrandmeyer@640:      car_coeffs_.c0_coeffs + (r * r));
alexbrandmeyer@637: }