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@609: void Ear::InitEar(long fs, CARParams car_p, IHCParams ihc_p, AGCParams agc_p){
alexbrandmeyer@609:   car_params_ = car_p;
alexbrandmeyer@609:   ihc_params_ = ihc_p;
alexbrandmeyer@609:   agc_params_ = agc_p;
alexbrandmeyer@609:   n_ch_ = 0;
alexbrandmeyer@609:   FPType pole_hz = car_params_.first_pole_theta_ * fs / (2 * PI);
alexbrandmeyer@609:   while (pole_hz > car_params_.min_pole_hz_) {
alexbrandmeyer@609:     n_ch_++;
alexbrandmeyer@609:     pole_hz = pole_hz - car_params_.erb_per_step_ *
alexbrandmeyer@609:     ERBHz(pole_hz, car_params_.erb_break_freq_, car_params_.erb_q_);
alexbrandmeyer@609:   }
alexbrandmeyer@609:   FloatArray pole_freqs(n_ch_);
alexbrandmeyer@609:   pole_hz = car_params_.first_pole_theta_ * fs / (2 * PI);
alexbrandmeyer@609:   for(int ch=0;ch < n_ch_; ch++){
alexbrandmeyer@609:     pole_freqs(ch) = pole_hz;
alexbrandmeyer@609:     pole_hz = pole_hz - car_params_.erb_per_step_ *
alexbrandmeyer@609:     ERBHz(pole_hz, car_params_.erb_break_freq_, car_params_.erb_q_);
alexbrandmeyer@609:   }
alexbrandmeyer@609:   max_channels_per_octave_ = log(2) / log(pole_freqs(0) / pole_freqs(1));
alexbrandmeyer@609:   car_coeffs_.DesignFilters(car_params_, fs, &pole_freqs);
alexbrandmeyer@609:   agc_coeffs_.DesignAGC(agc_params_, fs, n_ch_);
alexbrandmeyer@609:   ihc_coeffs_.DesignIHC(ihc_params_, fs, n_ch_);
alexbrandmeyer@609:   car_state_.InitCARState(car_coeffs_);
alexbrandmeyer@609:   agc_state_.InitAGCState(agc_coeffs_);
alexbrandmeyer@609:   ihc_state_.InitIHCState(ihc_coeffs_);
alexbrandmeyer@609: 
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@609: 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@609:   
alexbrandmeyer@609:   // do the DOHC stuff:
alexbrandmeyer@609:   g = car_state_.g_memory_ + car_state_.dg_memory_; //interp g
alexbrandmeyer@609:   zb = car_state_.zb_memory_ + car_state_.dzb_memory_; //AGC interpolation state
alexbrandmeyer@609:   // update the nonlinear function of "velocity", and zA (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@609:   r = car_coeffs_.r1_coeffs_ + (zb * nlf); // zB * nfl is "undamping" delta r
alexbrandmeyer@609:   za = car_state_.z2_memory_;
alexbrandmeyer@609:   // now reduce 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@609:   // Ripple input-output path, instead of parallel, to avoid delay...
alexbrandmeyer@609:   // this is the only part that doesn't get computed "in parallel":
alexbrandmeyer@609:   in_out = input;
alexbrandmeyer@609:   for (int ch = 0; ch < n_ch_; ch++){
alexbrandmeyer@609:     z1(ch) = z1(ch) + in_out;
alexbrandmeyer@609:     // ripple, saving 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@609:   // car_out is equal to zy state;
alexbrandmeyer@609:   return zy;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@609: // start with a quadratic nonlinear function, and limit it via a
alexbrandmeyer@609: // rational function; make the result go to zero at high
alexbrandmeyer@609: // absolute velocities, so it will do nothing there.
alexbrandmeyer@609: FloatArray Ear::OHC_NLF(FloatArray velocities){
alexbrandmeyer@609:   FloatArray nlf(n_ch_);
alexbrandmeyer@609:   nlf = 1 / ((velocities * car_coeffs_.velocity_scale_) +
alexbrandmeyer@609:              (car_coeffs_.v_offset_ * car_coeffs_.v_offset_));
alexbrandmeyer@609:   return nlf;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@609: // One sample-time update of inner-hair-cell (IHC) model, including the
alexbrandmeyer@609: // detection nonlinearity and one or two capacitor state variables.
alexbrandmeyer@609: 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@609:     //TODO Figure out best implementation with Eigen max/min methods
alexbrandmeyer@609:     for (int ch = 0; ch < n_ch_; ch++){
alexbrandmeyer@609:       FPType a;
alexbrandmeyer@609:       if (ac_diff(ch) > 0){
alexbrandmeyer@609:         a = ac_diff(ch);
alexbrandmeyer@609:       } else {
alexbrandmeyer@609:         a = 0;
alexbrandmeyer@609:       }
alexbrandmeyer@609:       if (a < 2){
alexbrandmeyer@609:         ihc_out(ch) = a;
alexbrandmeyer@609:       } else {
alexbrandmeyer@609:         ihc_out(ch) = 2;
alexbrandmeyer@609:       }
alexbrandmeyer@609:     }
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@609:     // smooth it twice with LPF:
alexbrandmeyer@609:     ihc_out = 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@609: 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@609: bool Ear::AGCRecurse(int stage, FloatArray agc_in){
alexbrandmeyer@609:   bool updated = true;
alexbrandmeyer@609:   // decim factor for this stage, relative to input or prev. stage:
alexbrandmeyer@609:   int decim = agc_coeffs_.decimation_(stage);
alexbrandmeyer@609:   // decim phase of this stage (do work on phase 0 only):
alexbrandmeyer@609:   //TODO FIX MODULO
alexbrandmeyer@609:   
alexbrandmeyer@609:   int decim_phase = agc_state_.decim_phase_(stage);
alexbrandmeyer@609:   decim_phase = decim_phase % decim;
alexbrandmeyer@609:   agc_state_.decim_phase_(stage) = decim_phase;
alexbrandmeyer@609:   // accumulate input for this stage from detect or previous stage:
alexbrandmeyer@609:   agc_state_.input_accum_.block(0,stage,n_ch_,1) =
alexbrandmeyer@609:                         agc_state_.input_accum_.block(0,stage,n_ch_,1) + agc_in;
alexbrandmeyer@609:   
alexbrandmeyer@609:   // nothing else to do if it's not the right decim_phase
alexbrandmeyer@609:   if (decim_phase == 0){
alexbrandmeyer@609:     // do lots of work, at decimated rate.
alexbrandmeyer@609:     // decimated inputs for this stage, and to be decimated more for next:
alexbrandmeyer@609:     agc_in = agc_state_.input_accum_.block(0,stage,n_ch_,1) / decim;
alexbrandmeyer@609:     // reset accumulator:
alexbrandmeyer@609:     agc_state_.input_accum_.block(0,stage,n_ch_,1) = FloatArray::Zero(n_ch_);
alexbrandmeyer@609:     
alexbrandmeyer@609:     if (stage < (agc_coeffs_.decimation_.size() - 1)){
alexbrandmeyer@609:       // recurse to evaluate next stage(s)
alexbrandmeyer@609:       updated = AGCRecurse(stage+1, agc_in);
alexbrandmeyer@609:       // and add its output to this stage input, whether it updated or not:
alexbrandmeyer@609:       agc_in = agc_in + (agc_coeffs_.agc_stage_gain_ *
alexbrandmeyer@609:                          agc_state_.agc_memory_.block(0,stage+1,n_ch_,1));
alexbrandmeyer@609:     }
alexbrandmeyer@609:     FloatArray agc_stage_state = agc_state_.agc_memory_.block(0,stage,n_ch_,1);
alexbrandmeyer@609:     // 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@609:     agc_state_.agc_memory_.block(0,stage,n_ch_,1) = agc_stage_state;
alexbrandmeyer@609:   } else {
alexbrandmeyer@609:     updated = false;
alexbrandmeyer@609:   }
alexbrandmeyer@609:   return updated;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@609: 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@609:   if (n_iterations < 4){
alexbrandmeyer@609:     use_fir = true;
alexbrandmeyer@609:   } else {
alexbrandmeyer@609:     use_fir = false;
alexbrandmeyer@609:   }
alexbrandmeyer@609:   
alexbrandmeyer@609:   if (use_fir) {
alexbrandmeyer@609:     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@609:     //Initialize first two taps of stage state, used for both cases
alexbrandmeyer@609:     ss_tap1(0) = stage_state(0);
alexbrandmeyer@609:     ss_tap1.block(1,0,n_ch_-1,1) = stage_state.block(0,0,n_ch_-1,1);
alexbrandmeyer@609:     ss_tap2(n_ch_-1) = stage_state(n_ch_-1);
alexbrandmeyer@609:     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@609:         //Initialize last two taps of stage state, used for 5-tap case
alexbrandmeyer@609:         ss_tap3(0) = stage_state(0);
alexbrandmeyer@609:         ss_tap3(1) = stage_state(1);
alexbrandmeyer@609:         ss_tap3.block(2,0,n_ch_-2,1) = stage_state.block(0,0,n_ch_-2,1);
alexbrandmeyer@609:         ss_tap4(n_ch_-2) = stage_state(n_ch_-1);
alexbrandmeyer@609:         ss_tap4(n_ch_-1) = stage_state(n_ch_-2);
alexbrandmeyer@609:         ss_tap4.block(0,0,n_ch_-2,1) = stage_state.block(2,0,n_ch_-2,1);
alexbrandmeyer@609:         
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@609:         //TODO Throw Error
alexbrandmeyer@609:         std::cout << "Error: bad n-taps in AGCSpatialSmooth" << std::endl;
alexbrandmeyer@609:     }
alexbrandmeyer@609:     
alexbrandmeyer@609:   } else {
alexbrandmeyer@609:     stage_state = AGCSmoothDoubleExponential(stage_state);
alexbrandmeyer@609:   }
alexbrandmeyer@609:   return stage_state;
alexbrandmeyer@609: }
alexbrandmeyer@609: 
alexbrandmeyer@609: FloatArray Ear::AGCSmoothDoubleExponential(FloatArray stage_state){
alexbrandmeyer@609:   return stage_state;
alexbrandmeyer@609: }