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: 
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@610: // on a single audio channel. 
alexbrandmeyer@610: void Ear::InitEar(int n_ch, long fs, FloatArray pole_freqs,
alexbrandmeyer@610:                   CARParams car_p, IHCParams ihc_p, AGCParams agc_p) {
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@610:   // These functions use the parameters that have been passed to design the
alexbrandmeyer@610:   // coefficients for the three stages of the model.
alexbrandmeyer@610:   DesignFilters(car_p, fs, pole_freqs);
alexbrandmeyer@610:   DesignIHC(ihc_p, fs);
alexbrandmeyer@610:   DesignAGC(agc_p, fs);
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::DesignFilters(CARParams car_params, long fs,
alexbrandmeyer@610:                               FloatArray pole_freqs) {
alexbrandmeyer@610:   car_coeffs_.velocity_scale_ = car_params.velocity_scale_;
alexbrandmeyer@610:   car_coeffs_.v_offset_ = car_params.v_offset_;
alexbrandmeyer@610:   car_coeffs_.r1_coeffs_.resize(n_ch_);
alexbrandmeyer@610:   car_coeffs_.a0_coeffs_.resize(n_ch_);
alexbrandmeyer@610:   car_coeffs_.c0_coeffs_.resize(n_ch_);
alexbrandmeyer@610:   car_coeffs_.h_coeffs_.resize(n_ch_);
alexbrandmeyer@610:   car_coeffs_.g0_coeffs_.resize(n_ch_);
alexbrandmeyer@610:   FPType f = car_params.zero_ratio_ * car_params.zero_ratio_ - 1;
alexbrandmeyer@610:   FloatArray theta = pole_freqs * ((2 * PI) / fs);
alexbrandmeyer@610:   car_coeffs_.c0_coeffs_ = sin(theta);
alexbrandmeyer@610:   car_coeffs_.a0_coeffs_ = cos(theta);
alexbrandmeyer@610:   FPType ff = car_params.high_f_damping_compression_;
alexbrandmeyer@610:   FloatArray x = theta / PI;
alexbrandmeyer@610:   car_coeffs_.zr_coeffs_ = PI * (x - (ff * (x * x * x)));
alexbrandmeyer@610:   FPType max_zeta = car_params.max_zeta_;
alexbrandmeyer@610:   FPType min_zeta = car_params.min_zeta_;
alexbrandmeyer@610:   car_coeffs_.r1_coeffs_ = (1 - (car_coeffs_.zr_coeffs_ * max_zeta));
alexbrandmeyer@610:   FPType curfreq;
alexbrandmeyer@610:   FloatArray erb_freqs(n_ch_);
alexbrandmeyer@610:   for (int ch=0; ch < n_ch_; ch++) {
alexbrandmeyer@610:     curfreq = pole_freqs(ch);
alexbrandmeyer@610:     erb_freqs(ch) = ERBHz(curfreq, car_params.erb_break_freq_,
alexbrandmeyer@610:                           car_params.erb_q_);
alexbrandmeyer@610:   }
alexbrandmeyer@610:   FloatArray min_zetas = min_zeta + (0.25 * ((erb_freqs / pole_freqs) - min_zeta));
alexbrandmeyer@610:   car_coeffs_.zr_coeffs_ *= max_zeta - min_zetas;
alexbrandmeyer@610:   car_coeffs_.h_coeffs_ = car_coeffs_.c0_coeffs_ * f;
alexbrandmeyer@610:   FloatArray relative_undamping = FloatArray::Ones(n_ch_);
alexbrandmeyer@610:   FloatArray r = car_coeffs_.r1_coeffs_ +
alexbrandmeyer@610:                  (car_coeffs_.zr_coeffs_ * relative_undamping);
alexbrandmeyer@610:   car_coeffs_.g0_coeffs_ = (1 - (2 * r * car_coeffs_.a0_coeffs_) + (r * r)) /
alexbrandmeyer@610:                           (1 - (2 * r * car_coeffs_.a0_coeffs_) +
alexbrandmeyer@610:                           (car_coeffs_.h_coeffs_ * r * car_coeffs_.c0_coeffs_) +
alexbrandmeyer@610:                           (r * r));
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::DesignAGC(AGCParams agc_params, long fs) {
alexbrandmeyer@610:   // These data members could probably be initialized locally within the design
alexbrandmeyer@610:   // function.
alexbrandmeyer@610:   agc_coeffs_.n_agc_stages_ = agc_params.n_stages_;
alexbrandmeyer@610:   agc_coeffs_.agc_stage_gain_ = agc_params.agc_stage_gain_;
alexbrandmeyer@610:   FloatArray agc1_scales = agc_params.agc1_scales_;
alexbrandmeyer@610:   FloatArray agc2_scales = agc_params.agc2_scales_;
alexbrandmeyer@610:   FloatArray time_constants = agc_params.time_constants_;
alexbrandmeyer@610:   agc_coeffs_.agc_epsilon_.resize(agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   agc_coeffs_.agc_pole_z1_.resize(agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   agc_coeffs_.agc_pole_z2_.resize(agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   agc_coeffs_.agc_spatial_iterations_.resize(agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   agc_coeffs_.agc_spatial_n_taps_.resize(agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   agc_coeffs_.agc_spatial_fir_.resize(3,agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   agc_coeffs_.agc_mix_coeffs_.resize(agc_coeffs_.n_agc_stages_);
alexbrandmeyer@610:   FPType mix_coeff = agc_params.agc_mix_coeff_;
alexbrandmeyer@610:   int decim = 1;
alexbrandmeyer@610:   agc_coeffs_.decimation_ = agc_params.decimation_;
alexbrandmeyer@610:   FPType total_dc_gain = 0;
alexbrandmeyer@610:   // Here we loop through each of the stages of the AGC.
alexbrandmeyer@610:   for (int stage=0; stage < agc_coeffs_.n_agc_stages_; stage++) {
alexbrandmeyer@610:     FPType tau = time_constants(stage);
alexbrandmeyer@610:     decim *= agc_coeffs_.decimation_.at(stage);
alexbrandmeyer@610:     agc_coeffs_.agc_epsilon_(stage) = 1 - exp((-1 * decim) / (tau * fs));
alexbrandmeyer@610:     FPType n_times = tau * (fs / decim);
alexbrandmeyer@610:     FPType delay = (agc2_scales(stage) - agc1_scales(stage)) / n_times;
alexbrandmeyer@610:     FPType spread_sq = (pow(agc1_scales(stage), 2) +
alexbrandmeyer@610:                         pow(agc2_scales(stage), 2)) / n_times;
alexbrandmeyer@610:     FPType u = 1 + (1 / spread_sq);
alexbrandmeyer@610:     FPType p = u - sqrt(pow(u, 2) - 1);
alexbrandmeyer@610:     FPType dp = delay * (1 - (2 * p) + (p * p)) / 2;
alexbrandmeyer@610:     FPType pole_z1 = p - dp;
alexbrandmeyer@610:     FPType pole_z2 = p + dp;
alexbrandmeyer@610:     agc_coeffs_.agc_pole_z1_(stage) = pole_z1;
alexbrandmeyer@610:     agc_coeffs_.agc_pole_z2_(stage) = pole_z2;
alexbrandmeyer@610:     int n_taps = 0;
alexbrandmeyer@610:     bool fir_ok = false;
alexbrandmeyer@610:     int n_iterations = 1;
alexbrandmeyer@610:     // This section initializes the FIR coeffs settings at each stage.
alexbrandmeyer@610:     FloatArray fir(3);
alexbrandmeyer@610:     while (! fir_ok) {
alexbrandmeyer@610:       switch (n_taps) {
alexbrandmeyer@610:         case 0:
alexbrandmeyer@610:           n_taps = 3;
alexbrandmeyer@610:           break;
alexbrandmeyer@610:         case 3:
alexbrandmeyer@610:           n_taps = 5;
alexbrandmeyer@610:           break;
alexbrandmeyer@610:         case 5:
alexbrandmeyer@610:           n_iterations ++;
alexbrandmeyer@610:           if (n_iterations > 16){
alexbrandmeyer@610:             // This implies too many iterations, so we shoud indicate and error.
alexbrandmeyer@610:             // TODO alexbrandmeyer, proper Google error handling.
alexbrandmeyer@610:           }
alexbrandmeyer@610:           break;
alexbrandmeyer@610:         default:
alexbrandmeyer@610:           // This means a bad n_taps has been provided, so there should again be
alexbrandmeyer@610:           // an error.
alexbrandmeyer@610:           // TODO alexbrandmeyer, proper Google error handling.
alexbrandmeyer@610:           break;
alexbrandmeyer@610:       }
alexbrandmeyer@610:       // Now we can design the FIR coefficients.
alexbrandmeyer@610:       FPType var = spread_sq / n_iterations;
alexbrandmeyer@610:       FPType mn = delay / n_iterations;
alexbrandmeyer@610:       FPType a, b;
alexbrandmeyer@610:       switch (n_taps) {
alexbrandmeyer@610:         case 3:
alexbrandmeyer@610:           a = (var + pow(mn, 2) - mn) / 2;
alexbrandmeyer@610:           b = (var + pow(mn, 2) + mn) / 2;
alexbrandmeyer@610:           fir << a, 1 - a - b, b;
alexbrandmeyer@610:           fir_ok = (fir(2) >= 0.2) ? true : false;
alexbrandmeyer@610:           break;
alexbrandmeyer@610:         case 5:
alexbrandmeyer@610:           a = (((var + pow(mn, 2)) * 2/5) - (mn * 2/3)) / 2;
alexbrandmeyer@610:           b = (((var + pow(mn, 2)) * 2/5) + (mn * 2/3)) / 2;
alexbrandmeyer@610:           fir << a/2, 1 - a - b, b/2;
alexbrandmeyer@610:           fir_ok = (fir(2) >= 0.1) ? true : false;
alexbrandmeyer@610:           break;
alexbrandmeyer@610:         default:
alexbrandmeyer@610:           break;  // Again, we've arrived at a bad n_taps in the design.
alexbrandmeyer@610:           // TODO alexbrandmeyer. Add proper Google error handling.
alexbrandmeyer@610:       }
alexbrandmeyer@610:     }
alexbrandmeyer@610:     // Once we have the FIR design for this stage we can assign it to the
alexbrandmeyer@610:     // appropriate data members.
alexbrandmeyer@610:     agc_coeffs_.agc_spatial_iterations_(stage) = n_iterations;
alexbrandmeyer@610:     agc_coeffs_.agc_spatial_n_taps_(stage) = n_taps;
alexbrandmeyer@610:     // TODO alexbrandmeyer replace using Eigen block method
alexbrandmeyer@610:     agc_coeffs_.agc_spatial_fir_(0, stage) = fir(0);
alexbrandmeyer@610:     agc_coeffs_.agc_spatial_fir_(1, stage) = fir(1);
alexbrandmeyer@610:     agc_coeffs_.agc_spatial_fir_(2, stage) = fir(2);
alexbrandmeyer@610:     total_dc_gain += pow(agc_coeffs_.agc_stage_gain_, (stage));
alexbrandmeyer@610:     agc_coeffs_.agc_mix_coeffs_(stage) =
alexbrandmeyer@610:       (stage == 0) ? 0 : mix_coeff / (tau * (fs /decim));
alexbrandmeyer@610:   }
alexbrandmeyer@610:   agc_coeffs_.agc_gain_ = total_dc_gain;
alexbrandmeyer@610:   agc_coeffs_.detect_scale_ = 1 / total_dc_gain;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::DesignIHC(IHCParams ihc_params, long fs) {
alexbrandmeyer@610:   if (ihc_params.just_hwr_) {
alexbrandmeyer@610:     ihc_coeffs_.just_hwr_ = ihc_params.just_hwr_;
alexbrandmeyer@610:   } else {
alexbrandmeyer@610:     // This section calculates conductance values using two pre-defined scalars.
alexbrandmeyer@610:     FloatArray x(1);
alexbrandmeyer@610:     FPType conduct_at_10, conduct_at_0;
alexbrandmeyer@610:     x << 10;
alexbrandmeyer@610:     x = CARFACDetect(x);
alexbrandmeyer@610:     conduct_at_10 = x(0);
alexbrandmeyer@610:     x << 0;
alexbrandmeyer@610:     x = CARFACDetect(x);
alexbrandmeyer@610:     conduct_at_0 = x(0);
alexbrandmeyer@610:     if (ihc_params.one_cap_) {
alexbrandmeyer@610:       FPType ro = 1 / conduct_at_10 ;
alexbrandmeyer@610:       FPType c = ihc_params.tau1_out_ / ro;
alexbrandmeyer@610:       FPType ri = ihc_params.tau1_in_ / c;
alexbrandmeyer@610:       FPType saturation_output = 1 / ((2 * ro) + ri);
alexbrandmeyer@610:       FPType r0 = 1 / conduct_at_0;
alexbrandmeyer@610:       FPType current = 1 / (ri + r0);
alexbrandmeyer@610:       ihc_coeffs_.cap1_voltage_ = 1 - (current * ri);
alexbrandmeyer@610:       ihc_coeffs_.just_hwr_ = false;
alexbrandmeyer@610:       ihc_coeffs_.lpf_coeff_ = 1 - exp( -1 / (ihc_params.tau_lpf_ * fs));
alexbrandmeyer@610:       ihc_coeffs_.out1_rate_ = ro / (ihc_params.tau1_out_ * fs);
alexbrandmeyer@610:       ihc_coeffs_.in1_rate_ = 1 / (ihc_params.tau1_in_ * fs);
alexbrandmeyer@610:       ihc_coeffs_.one_cap_ = ihc_params.one_cap_;
alexbrandmeyer@610:       ihc_coeffs_.output_gain_ = 1 / (saturation_output - current);
alexbrandmeyer@610:       ihc_coeffs_.rest_output_ = current / (saturation_output - current);
alexbrandmeyer@610:       ihc_coeffs_.rest_cap1_ = ihc_coeffs_.cap1_voltage_;
alexbrandmeyer@610:     } else {
alexbrandmeyer@610:       FPType ro = 1 / conduct_at_10;
alexbrandmeyer@610:       FPType c2 = ihc_params.tau2_out_ / ro;
alexbrandmeyer@610:       FPType r2 = ihc_params.tau2_in_ / c2;
alexbrandmeyer@610:       FPType c1 = ihc_params.tau1_out_ / r2;
alexbrandmeyer@610:       FPType r1 = ihc_params.tau1_in_ / c1;
alexbrandmeyer@610:       FPType saturation_output = 1 / (2 * ro + r2 + r1);
alexbrandmeyer@610:       FPType r0 = 1 / conduct_at_0;
alexbrandmeyer@610:       FPType current = 1 / (r1 + r2 + r0);
alexbrandmeyer@610:       ihc_coeffs_.cap1_voltage_ = 1 - (current * r1);
alexbrandmeyer@610:       ihc_coeffs_.cap2_voltage_ = ihc_coeffs_.cap1_voltage_ - (current * r2);
alexbrandmeyer@610:       ihc_coeffs_.just_hwr_ = false;
alexbrandmeyer@610:       ihc_coeffs_.lpf_coeff_ = 1 - exp(-1 / (ihc_params.tau_lpf_ * fs));
alexbrandmeyer@610:       ihc_coeffs_.out1_rate_ = 1 / (ihc_params.tau1_out_ * fs);
alexbrandmeyer@610:       ihc_coeffs_.in1_rate_ = 1 / (ihc_params.tau1_in_ * fs);
alexbrandmeyer@610:       ihc_coeffs_.out2_rate_ = ro / (ihc_params.tau2_out_ * fs);
alexbrandmeyer@610:       ihc_coeffs_.in2_rate_ = 1 / (ihc_params.tau2_in_ * fs);
alexbrandmeyer@610:       ihc_coeffs_.one_cap_ = ihc_params.one_cap_;
alexbrandmeyer@610:       ihc_coeffs_.output_gain_ = 1 / (saturation_output - current);
alexbrandmeyer@610:       ihc_coeffs_.rest_output_ = current / (saturation_output - current);
alexbrandmeyer@610:       ihc_coeffs_.rest_cap1_ = ihc_coeffs_.cap1_voltage_;
alexbrandmeyer@610:       ihc_coeffs_.rest_cap2_ = ihc_coeffs_.cap2_voltage_;
alexbrandmeyer@610:     }
alexbrandmeyer@610:   }
alexbrandmeyer@610:   ihc_coeffs_.ac_coeff_ = 2 * PI * ihc_params.ac_corner_hz_ / fs;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::InitCARState() {
alexbrandmeyer@610:   car_state_.z1_memory_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   car_state_.z2_memory_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   car_state_.za_memory_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   car_state_.zb_memory_ = car_coeffs_.zr_coeffs_;
alexbrandmeyer@610:   car_state_.dzb_memory_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   car_state_.zy_memory_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   car_state_.g_memory_ = car_coeffs_.g0_coeffs_;
alexbrandmeyer@610:   car_state_.dg_memory_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::InitIHCState() {
alexbrandmeyer@610:   ihc_state_.ihc_accum_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   if (! ihc_coeffs_.just_hwr_) {
alexbrandmeyer@610:     ihc_state_.ac_coupler_ = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:     ihc_state_.lpf1_state_ = ihc_coeffs_.rest_output_ * FloatArray::Ones(n_ch_);
alexbrandmeyer@610:     ihc_state_.lpf2_state_ = ihc_coeffs_.rest_output_ * FloatArray::Ones(n_ch_);
alexbrandmeyer@610:     if (ihc_coeffs_.one_cap_) {
alexbrandmeyer@610:       ihc_state_.cap1_voltage_ = ihc_coeffs_.rest_cap1_ *
alexbrandmeyer@610:                                  FloatArray::Ones(n_ch_);
alexbrandmeyer@610:     } else {
alexbrandmeyer@610:       ihc_state_.cap1_voltage_ = ihc_coeffs_.rest_cap1_ *
alexbrandmeyer@610:                                  FloatArray::Ones(n_ch_);
alexbrandmeyer@610:       ihc_state_.cap2_voltage_ = ihc_coeffs_.rest_cap2_ *
alexbrandmeyer@610:                                  FloatArray::Ones(n_ch_);
alexbrandmeyer@610:     }
alexbrandmeyer@610:   }
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::InitAGCState() {
alexbrandmeyer@610:   agc_state_.n_agc_stages_ = agc_coeffs_.n_agc_stages_;
alexbrandmeyer@610:   agc_state_.agc_memory_.resize(agc_state_.n_agc_stages_);
alexbrandmeyer@610:   agc_state_.input_accum_.resize(agc_state_.n_agc_stages_);
alexbrandmeyer@610:   agc_state_.decim_phase_.resize(agc_state_.n_agc_stages_);
alexbrandmeyer@610:   for (int i = 0; i < agc_state_.n_agc_stages_; i++) {
alexbrandmeyer@610:     agc_state_.decim_phase_.at(i) = 0;
alexbrandmeyer@610:     agc_state_.agc_memory_.at(i) = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:     agc_state_.input_accum_.at(i) = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:   }
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::CARStep(FPType input) {
alexbrandmeyer@609:   FloatArray g(n_ch_);
alexbrandmeyer@609:   FloatArray zb(n_ch_);
alexbrandmeyer@609:   FloatArray za(n_ch_);
alexbrandmeyer@609:   FloatArray v(n_ch_);
alexbrandmeyer@609:   FloatArray nlf(n_ch_);
alexbrandmeyer@609:   FloatArray r(n_ch_);
alexbrandmeyer@609:   FloatArray z1(n_ch_);
alexbrandmeyer@609:   FloatArray z2(n_ch_);
alexbrandmeyer@609:   FloatArray zy(n_ch_);
alexbrandmeyer@609:   FPType in_out;
alexbrandmeyer@610:   // This performs the DOHC stuff.
alexbrandmeyer@610:   g = car_state_.g_memory_ + car_state_.dg_memory_;  // This interpolates g.
alexbrandmeyer@610:   // This calculates the AGC interpolation state.
alexbrandmeyer@610:   zb = 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@609:   za = car_state_.za_memory_;
alexbrandmeyer@609:   v = car_state_.z2_memory_ - za;
alexbrandmeyer@609:   nlf = OHC_NLF(v);
alexbrandmeyer@610:   // Here, zb * nfl is "undamping" delta r.
alexbrandmeyer@610:   r = car_coeffs_.r1_coeffs_ + (zb * nlf);
alexbrandmeyer@609:   za = car_state_.z2_memory_;
alexbrandmeyer@610:   // Here we reduce the CAR state by r and rotate with the fixed cos/sin coeffs.
alexbrandmeyer@609:   z1 = r * ((car_coeffs_.a0_coeffs_ * car_state_.z1_memory_) -
alexbrandmeyer@609:             (car_coeffs_.c0_coeffs_ * car_state_.z2_memory_));
alexbrandmeyer@609:   z2 = r * ((car_coeffs_.c0_coeffs_ * car_state_.z1_memory_) +
alexbrandmeyer@609:             (car_coeffs_.a0_coeffs_ * car_state_.z2_memory_));
alexbrandmeyer@609:   zy = car_coeffs_.h_coeffs_ * z2;
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@609:   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@609:     in_out = g(ch) * (in_out + zy(ch));
alexbrandmeyer@609:     zy(ch) = in_out;
alexbrandmeyer@609:   }
alexbrandmeyer@609:   car_state_.z1_memory_ = z1;
alexbrandmeyer@609:   car_state_.z2_memory_ = z2;
alexbrandmeyer@609:   car_state_.za_memory_ = za;
alexbrandmeyer@609:   car_state_.zb_memory_ = zb;
alexbrandmeyer@609:   car_state_.zy_memory_ = zy;
alexbrandmeyer@609:   car_state_.g_memory_ = g;
alexbrandmeyer@610:   // The output of the CAR is equal to the zy state.
alexbrandmeyer@609:   return zy;
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@610: FloatArray Ear::OHC_NLF(FloatArray velocities) {
alexbrandmeyer@610:   return 1 / (1 +
alexbrandmeyer@610:         (((velocities * car_coeffs_.velocity_scale_) + car_coeffs_.v_offset_) *
alexbrandmeyer@610:          ((velocities * car_coeffs_.velocity_scale_) + car_coeffs_.v_offset_)));
alexbrandmeyer@610: 
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@610: FloatArray Ear::IHCStep(FloatArray car_out) {
alexbrandmeyer@609:   FloatArray ihc_out(n_ch_);
alexbrandmeyer@609:   FloatArray ac_diff(n_ch_);
alexbrandmeyer@609:   FloatArray conductance(n_ch_);
alexbrandmeyer@609:   ac_diff = car_out - ihc_state_.ac_coupler_;
alexbrandmeyer@609:   ihc_state_.ac_coupler_ = ihc_state_.ac_coupler_ +
alexbrandmeyer@609:                            (ihc_coeffs_.ac_coeff_ * ac_diff);
alexbrandmeyer@609:   if (ihc_coeffs_.just_hwr_) {
alexbrandmeyer@610:     for (int ch = 0; ch < n_ch_; ch++) {
alexbrandmeyer@609:       FPType a;
alexbrandmeyer@610:       a = (ac_diff(ch) > 0) ? ac_diff(ch) : 0;
alexbrandmeyer@610:       ihc_out(ch) = (a < 2) ? a : 2;
alexbrandmeyer@609:     }
alexbrandmeyer@609:   } else {
alexbrandmeyer@609:     conductance = CARFACDetect(ac_diff);
alexbrandmeyer@609:     if (ihc_coeffs_.one_cap_) {
alexbrandmeyer@609:       ihc_out = conductance * ihc_state_.cap1_voltage_;
alexbrandmeyer@609:       ihc_state_.cap1_voltage_ = ihc_state_.cap1_voltage_ -
alexbrandmeyer@609:                                  (ihc_out * ihc_coeffs_.out1_rate_) +
alexbrandmeyer@609:                                  ((1 - ihc_state_.cap1_voltage_)
alexbrandmeyer@609:                                   * ihc_coeffs_.in1_rate_);
alexbrandmeyer@609:     } else {
alexbrandmeyer@609:       ihc_out = conductance * ihc_state_.cap2_voltage_;
alexbrandmeyer@609:       ihc_state_.cap1_voltage_ = ihc_state_.cap1_voltage_ -
alexbrandmeyer@609:                       ((ihc_state_.cap1_voltage_ - ihc_state_.cap2_voltage_)
alexbrandmeyer@609:                       * ihc_coeffs_.out1_rate_) +
alexbrandmeyer@609:                       ((1 - ihc_state_.cap1_voltage_) * ihc_coeffs_.in1_rate_);
alexbrandmeyer@609:       ihc_state_.cap2_voltage_ = ihc_state_.cap2_voltage_ -
alexbrandmeyer@609:                           (ihc_out * ihc_coeffs_.out2_rate_) +
alexbrandmeyer@609:                           ((ihc_state_.cap1_voltage_ - ihc_state_.cap2_voltage_)
alexbrandmeyer@609:                           * ihc_coeffs_.in2_rate_);
alexbrandmeyer@609:     }
alexbrandmeyer@610:     // Here we smooth the output twice using a LPF.
alexbrandmeyer@610:     ihc_out *= ihc_coeffs_.output_gain_;
alexbrandmeyer@609:     ihc_state_.lpf1_state_ = ihc_state_.lpf1_state_ +
alexbrandmeyer@609:                   (ihc_coeffs_.lpf_coeff_ * (ihc_out - ihc_state_.lpf1_state_));
alexbrandmeyer@609:     ihc_state_.lpf2_state_ = ihc_state_.lpf2_state_ +
alexbrandmeyer@609:                   (ihc_coeffs_.lpf_coeff_ *
alexbrandmeyer@609:                    (ihc_state_.lpf1_state_ - ihc_state_.lpf2_state_));
alexbrandmeyer@609:     ihc_out = ihc_state_.lpf2_state_ - ihc_coeffs_.rest_output_;
alexbrandmeyer@609:   }
alexbrandmeyer@609:   ihc_state_.ihc_accum_ += ihc_out;
alexbrandmeyer@609:   return ihc_out;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: bool Ear::AGCStep(FloatArray ihc_out) {
alexbrandmeyer@609:   int stage = 0;
alexbrandmeyer@609:   FloatArray agc_in(n_ch_);
alexbrandmeyer@609:   agc_in = agc_coeffs_.detect_scale_ * ihc_out;
alexbrandmeyer@609:   bool updated = AGCRecurse(stage, agc_in);
alexbrandmeyer@609:   return updated;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: bool Ear::AGCRecurse(int stage, FloatArray agc_in) {
alexbrandmeyer@609:   bool updated = true;
alexbrandmeyer@610:   // This is the decim factor for this stage, relative to input or prev. stage:
alexbrandmeyer@610:   int decim = agc_coeffs_.decimation_.at(stage);
alexbrandmeyer@610:   // This is the decim phase of this stage (do work on phase 0 only):
alexbrandmeyer@610:   int decim_phase = agc_state_.decim_phase_.at(stage);
alexbrandmeyer@609:   decim_phase = decim_phase % decim;
alexbrandmeyer@610:   agc_state_.decim_phase_.at(stage) = decim_phase;
alexbrandmeyer@610:   // Here we accumulate input for this stage from the previous stage:
alexbrandmeyer@610:   agc_state_.input_accum_.at(stage) += 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@610:     agc_in = agc_state_.input_accum_.at(stage) / decim;
alexbrandmeyer@610:     // This resets the accumulator.
alexbrandmeyer@610:     agc_state_.input_accum_.at(stage) = FloatArray::Zero(n_ch_);
alexbrandmeyer@610:     if (stage < (agc_coeffs_.decimation_.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@610:       agc_in += (agc_coeffs_.agc_stage_gain_ *
alexbrandmeyer@610:                          agc_state_.agc_memory_.at(stage + 1));
alexbrandmeyer@609:     }
alexbrandmeyer@610:     FloatArray agc_stage_state = agc_state_.agc_memory_.at(stage);
alexbrandmeyer@610:     // This performs a first-order recursive smoothing filter update, in time.
alexbrandmeyer@609:     agc_stage_state = agc_stage_state + (agc_coeffs_.agc_epsilon_(stage) *
alexbrandmeyer@609:                                          (agc_in - agc_stage_state));
alexbrandmeyer@609:     agc_stage_state = AGCSpatialSmooth(stage, agc_stage_state);
alexbrandmeyer@610:     agc_state_.agc_memory_.at(stage) = agc_stage_state;
alexbrandmeyer@609:   } else {
alexbrandmeyer@609:     updated = false;
alexbrandmeyer@609:   }
alexbrandmeyer@609:   return updated;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: FloatArray Ear::AGCSpatialSmooth(int stage, FloatArray stage_state) {
alexbrandmeyer@609:   int n_iterations = agc_coeffs_.agc_spatial_iterations_(stage);
alexbrandmeyer@609:   bool use_fir;
alexbrandmeyer@610:   use_fir = (n_iterations < 4) ? true : false;
alexbrandmeyer@609:   if (use_fir) {
alexbrandmeyer@610:     FloatArray fir_coeffs = agc_coeffs_.agc_spatial_fir_.block(0, stage, 3, 1);
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@609:     int n_taps = agc_coeffs_.agc_spatial_n_taps_(stage);
alexbrandmeyer@610:     // This initializes the first two taps of stage state, which are used for
alexbrandmeyer@610:     // both possible cases.
alexbrandmeyer@609:     ss_tap1(0) = stage_state(0);
alexbrandmeyer@610:     ss_tap1.block(1, 0, n_ch_ - 1, 1) = stage_state.block(0, 0, n_ch_ - 1, 1);
alexbrandmeyer@610:     ss_tap2(n_ch_ - 1) = stage_state(n_ch_ - 1);
alexbrandmeyer@610:     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@609:         stage_state = (fir_coeffs(0) * ss_tap1) +
alexbrandmeyer@609:                       (fir_coeffs(1) * stage_state) +
alexbrandmeyer@609:                       (fir_coeffs(2) * 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@609:         ss_tap3(0) = stage_state(0);
alexbrandmeyer@609:         ss_tap3(1) = stage_state(1);
alexbrandmeyer@610:         ss_tap3.block(2, 0, n_ch_ - 2, 1) =
alexbrandmeyer@610:           stage_state.block(0, 0, n_ch_ - 2, 1);
alexbrandmeyer@610:         ss_tap4(n_ch_ - 2) = stage_state(n_ch_ - 1);
alexbrandmeyer@610:         ss_tap4(n_ch_ - 1) = stage_state(n_ch_ - 2);
alexbrandmeyer@610:         ss_tap4.block(0, 0, n_ch_ - 2, 1) =
alexbrandmeyer@610:           stage_state.block(2, 0, n_ch_ - 2, 1);
alexbrandmeyer@609:         stage_state = (fir_coeffs(0) * (ss_tap3 + ss_tap1)) +
alexbrandmeyer@609:                       (fir_coeffs(1) * stage_state) +
alexbrandmeyer@609:                       (fir_coeffs(2) * (ss_tap2 + ss_tap4));
alexbrandmeyer@609:         break;
alexbrandmeyer@609:       default:
alexbrandmeyer@610:         // TODO alexbrandmeyer: determine proper error handling implementation.
alexbrandmeyer@609:         std::cout << "Error: bad n-taps in AGCSpatialSmooth" << std::endl;
alexbrandmeyer@609:     }
alexbrandmeyer@609:   } else {
alexbrandmeyer@610:     stage_state = AGCSmoothDoubleExponential(stage_state,
alexbrandmeyer@610:                                              agc_coeffs_.agc_pole_z1_(stage),
alexbrandmeyer@610:                                              agc_coeffs_.agc_pole_z2_(stage));
alexbrandmeyer@609:   }
alexbrandmeyer@609:   return stage_state;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@610: FloatArray Ear::AGCSmoothDoubleExponential(FloatArray stage_state,
alexbrandmeyer@610:                                            FPType pole_z1, FPType pole_z2) {
alexbrandmeyer@610:   int n_pts = stage_state.size();
alexbrandmeyer@610:   FPType input;
alexbrandmeyer@610:   FPType state = 0;
alexbrandmeyer@610:   // TODO alexbrandmeyer: I'm assuming one dimensional input for now, but this
alexbrandmeyer@610:   // should be verified with Dick for the final version
alexbrandmeyer@610:   for (int i = n_pts - 11; i < n_pts; i ++){
alexbrandmeyer@610:     input = stage_state(i);
alexbrandmeyer@610:     state = state + (1 - pole_z1) * (input - state);
alexbrandmeyer@610:   }
alexbrandmeyer@610:   for (int i = n_pts - 1; i > -1; i --){
alexbrandmeyer@610:     input = stage_state(i);
alexbrandmeyer@610:     state = state + (1 - pole_z2) * (input - state);
alexbrandmeyer@610:   }
alexbrandmeyer@610:   for (int i = 0; i < n_pts; i ++){
alexbrandmeyer@610:     input = stage_state(i);
alexbrandmeyer@610:     state = state + (1 - pole_z1) * (input - state);
alexbrandmeyer@610:     stage_state(i) = state;
alexbrandmeyer@610:   }
alexbrandmeyer@609:   return stage_state;
alexbrandmeyer@609: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::ReturnZAMemory() {
alexbrandmeyer@610:   return car_state_.za_memory_;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::ReturnZBMemory() {
alexbrandmeyer@610:   return car_state_.zb_memory_;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::ReturnGMemory() {
alexbrandmeyer@610:   return car_state_.g_memory_;
alexbrandmeyer@610: };
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::ReturnZRCoeffs() {
alexbrandmeyer@610:   return car_coeffs_.zr_coeffs_;
alexbrandmeyer@610: };
alexbrandmeyer@610: 
alexbrandmeyer@610: int Ear::ReturnAGCNStages() {
alexbrandmeyer@610:   return agc_coeffs_.n_agc_stages_;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: int Ear::ReturnAGCStateDecimPhase(int stage) {
alexbrandmeyer@610:   return agc_state_.decim_phase_.at(stage);
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FPType Ear::ReturnAGCMixCoeff(int stage) {
alexbrandmeyer@610:   return agc_coeffs_.agc_mix_coeffs_(stage);
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::ReturnAGCStateMemory(int stage) {
alexbrandmeyer@610:   return agc_state_.agc_memory_.at(stage);
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: int Ear::ReturnAGCDecimation(int stage) {
alexbrandmeyer@610:   return agc_coeffs_.decimation_.at(stage);
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::SetAGCStateMemory(int stage, FloatArray new_values) {
alexbrandmeyer@610:   agc_state_.agc_memory_.at(stage) = new_values;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::SetCARStateDZBMemory(FloatArray new_values) {
alexbrandmeyer@610:   car_state_.dzb_memory_ = new_values;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: void Ear::SetCARStateDGMemory(FloatArray new_values) {
alexbrandmeyer@610:   car_state_.dg_memory_ = new_values;
alexbrandmeyer@610: }
alexbrandmeyer@610: 
alexbrandmeyer@610: FloatArray Ear::StageGValue(FloatArray undamping) {
alexbrandmeyer@610:   FloatArray r1 = car_coeffs_.r1_coeffs_;
alexbrandmeyer@610:   FloatArray a0 = car_coeffs_.a0_coeffs_;
alexbrandmeyer@610:   FloatArray c0 = car_coeffs_.c0_coeffs_;
alexbrandmeyer@610:   FloatArray h = car_coeffs_.h_coeffs_;
alexbrandmeyer@610:   FloatArray zr = car_coeffs_.zr_coeffs_;
alexbrandmeyer@610:   FloatArray r = r1 + zr * undamping;
alexbrandmeyer@610:   return (1 - 2 * r * a0 + (r * r)) / (1 - 2 * r * a0 + h * r * c0 + (r * r));
alexbrandmeyer@610: }