## Copyright (C) 2015 Stefano D'Angelo
##
## Permission is hereby granted, free of charge, to any person obtaining a copy
## of this software and associated documentation files (the "Software"), to deal
## in the Software without restriction, including without limitation the rights
## to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
## copies of the Software, and to permit persons to whom the Software is
## furnished to do so, subject to the following conditions:
##
## The above copyright notice and this permission notice shall be included in
## all copies or substantial portions of the Software.
##
## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
## AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
## OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
## THE SOFTWARE.

## -*- texinfo -*-
## @deftypefn {Function File} {@var{y} =} noninv_distortion (@var{fs}, @var{x}, @var{dist})
##
## Simulate the noninverting distortion circuit described in
## @quotation
## R. C. D. de Paiva, S. D'Angelo, J. Pakarinen, and V. Valimaki, ``Emulation
## of Operational Amplifiers and Diodes in Audio Distortion Circuits,''
## @emph{IEEE Trans. Circ. Systems-II: Express Briefs}, vol. 59, no. 10,
## pp. 688-692, October 2012.
## @end quotation
##
## It produces the output vector @var{y} from the audio input vector @var{x}.
## @var{fs} is the sample rate (in Hz).
##
## @var{dist} is the normalized distortion setting (@math{R_f/R_{f,max}}), and
## can either be a scalar or a vector whose length is equal to the length of
## @var{x}.
##
## @end deftypefn

## Author: Stefano D'Angelo <zanga.mail@gmail.com>
## Maintainer: Stefano D'Angelo <zanga.mail@gmail.com>
## Version: 1.0.0
## Keywords: noninverting distortion guitar

function y = noninv_distortion(fs, x, dist)

  #### Input arguments

  # Checks

  if (nargin != 3)
    print_usage();
  elseif (!isscalar(fs) || !isnumeric(fs) || !isreal(fs) || fs <= 0)
    error("noninv_distortion: FS must be a positive real");
  elseif (isscalar(x) || !isvector(x) || !isnumeric(x) || !isreal(x))
    error("noninv_distortion: X must be a vector of real values");
  elseif (!isnumeric(dist) || !isreal(dist)
          || (!isscalar(dist) && !isvector(dist))
          || (!isscalar(dist) && isvector(dist)
              && length(dist) != length(x)))
    error(["noninv_distortion: DIST must be a scalar or a vector of real " ...
           "values whose length is equal to the length of X"]);
  endif

  # Convert scalar parameters to vectors

  if (isscalar(dist))
    dist = repmat(dist, 1, length(x));
  endif

  # Enforce bounds

  dist = limit(dist, 0, 1, "DIST");

  #### Constants

  # Circuit components

  Rn    = 10e3;
  Cn    = 1e-6;
  Ri    = 4.7e3;
  Ci    = 4.7e-9;
  Rfmax = 500e3;
  Cf    = 51e-12;
  Rs    = 0.568;
  Is    = 2.52e-9;
  nD    = 1.752;
  VT    = 25.86e-3;

  # WDF element values

  wCn_R  = 1 / (2 * Cn * fs);
  # wVn_k  = 2 * wCn_R / (wCn_R + Rn);
  wVn_kx = wCn_R / (wCn_R + Rn); # = 0.5 * wVn_k
  wCi_R  = 1 / (2 * Ci * fs);
  wVi_k  = 2 * wCi_R / (wCi_R + Ri);
  ki     = 1 / (2 * wCi_R);
  wCf_R  = 1 / (2 * Cf * fs);
  wRs_R  = Rs;
  wD_k2  = -2 * nD * VT;
  wD_k4  = 1 / (nD * VT);

  #### Non-constants

  wI0_R   = Rfmax * dist;
  wpf_kt  = wI0_R ./ (wI0_R + wCf_R);
  wpf_R   = (wCf_R * wI0_R) ./ (wCf_R + wI0_R);
  # wsD_kr  = wpf_R ./ (wRs_R + wpf_R);
  wsD_krx = 0.5 * wpf_R ./ (wRs_R + wpf_R); # = 0.5 * wsD_kr
  wsD_R   = wpf_R + wRs_R;
  wD_k1   = 2 * wsD_R * Is;
  wD_k3   = wsD_R * Is / (nD * VT);
  wD_k3x  = wD_k3 + log(wD_k3);

  #### Filter

  y     = zeros(1, length(x));
  wCn_s = 0;
  wCi_s = 0;
  wCf_s = 0;

  for i = 1:length(x)

    ### Non-inverted input subcircuit

    ## Cn out
    # wCn_b = wCn_s

    ## Series junction (upwards)
    # wsn_ab =  wCn_b  =  wCn_s
    # wsn_bt = -wsn_ab = -wCn_s

    ## Vn
    # wVn_a = wsn_bt = -wCn_s
    # wVn_b = wVn_a + wVn_k * (x(i) - wVn_a) = wVn_k * (x(i) + wCn_s) - wCn_s

    ## Series junction (downwards)
    # wsn_at =  wVn_b  = wVn_k * (x(i) + wCn_s) - wCn_s
    # wsn_bb = -wsn_at = wCn_s - wVn_k * (x(i) + wCn_s)

    ## Cn in
    # wCn_a = wsn_bb = wCn_s - wVn_k * (x(i) + wCn_s)

    ## Output (see Errata)
    # Vp = x(i) + 0.5 * (wCn_a + wCn_b) ...
    #    = x(i) + wCn_s - 0.5 * wVn_k * (x(i) + wCn_s)

    ## State update
    # wCn_s = wCn_a = wCn_s - wVn_k * (x(i) + wCn_s)

    ## Summing up
    s     = x(i) + wCn_s;
    p     = wVn_kx * s;
    Vp    = s - p;
    wCn_s = wCn_s - (p + p);

    ### Inverted input subcircuit

    ## Ci out
    # wCi_b = wCi_s

    ## Series junction (upwards)
    # wsi_ab =  wCi_b  =  wCi_s
    # wsi_bt = -wsi_ab = -wCi_s

    ## Vi
    # wVi_a = wsi_bt = -wCi_s
    # wVi_b = wVi_a + wVi_k * (Vp - wVi_a) = wVi_k * (Vp + wCi_s) - wCi_s

    ## Series junction (downwards)
    # wsi_at =  wVi_b  = wVi_k * (Vp + wCi_s) - wCi_s
    # wsi_bb = -wsi_at = wCi_s - wVi_k * (Vp + wCi_s)

    ## Ci in
    # wCi_a = wsi_bb = wCi_s - wVi_k * (Vp + wCi_s)

    ## Output
    # I0 = ki * (wVi_a - wVi_b) = ki * wVi_k * (Vp + wCi_s)

    ## State update
    # wCi_s = wCi_a = wCi_s - wVi_k * (Vp + wCi_s)

    ## Summing up
    p     = wVi_k * (Vp + wCi_s);
    I0    = ki * p;
    wCi_s = wCi_s - p;

    ### Feedback subcircuit

    ## I0 and Cf outs
    # wI0_b = wI0_R(i) * I0
    # wCf_b = wCf_s

    ## Parallel junction (upwards)
    # wpf_al = wI0_b = wI0_R(i) * I0
    # wpf_ar = wCf_b = wCf_s
    # wpf_ax = wpf_ar - wpf_al = wCf_s - wI0_R(i) * I0
    # wpf_kx = wpf_kt(i) * wpf_ax = wpf_kt(i) * (wCf_s - wI0_R(i) * I0)
    # wpf_bt = wpf_kx + wpf_al ...
    #        = wpf_kt(i) * (wCf_s - wI0_R(i) * I0) + wI0_R(i) * I0

    ## Antiparallel diodes

    # Rs out
    # wRs_b = 0

    # Series junction (upwards)
    # wsD_al = wRs_b = 0
    # wsD_ar = wpf_bt = wpf_kt(i) * (wCf_s - wI0_R(i) * I0) + wI0_R(i) * I0
    # wsD_bt = -(wsD_al + wsD_ar) ...
    #        = wpf_kt(i) * (wI0_R(i) * I0 - wCf_s) - wI0_R(i) * I0

    # Nonlinearity
    # wD_a = wsD_bt = wpf_kt(i) * (wI0_R(i) * I0 - wCf_s) - wI0_R(i) * I0
    # wD_b = ...
    #   if (wD_a >= 0)
    #     wD_b = wD_a + wD_k1(i) ...
    #            + wD_k2 * lambertw(wD_k3(i) * exp(wD_k3(i) + wD_k4 * wD_a))
    #   else
    #     wD_b = wD_a - wD_k1(i) ...
    #            - wD_k2 * lambertw(wD_k3(i) * exp(wD_k3(i) - wD_k4 * wD_a))
    #   endif = ...
    #   if (wD_a >= 0)
    #     wD_b = wD_a + wD_k1(i) ...
    #            + wD_k2 * lambertw(exp(wD_k3(i) + log(wD_k3(i)) ...
    #                                   + wD_k4 * wD_a))
    #   else
    #     wD_b = wD_a - wD_k1(i) ...
    #            - wD_k2 * lambertw(exp(wD_k3(i) + log(wD_k3(i)) ...
    #                                   - wD_k4 * wD_a))
    #   endif

    # Series junction (downwards)
    # wsD_at = wD_b
    # wsD_bl unused
    # wsD_br = wsD_ar + wsD_kr(i) * (wsD_bt - wsD_at) ...
    #        = wsD_kr(i) * (wD_a - wD_b) - wD_a

    # Rs in
    # wRs_a unused

    ## Parallel junction (downwards)
    # wpf_at = wsD_br = wsD_kr(i) * (wD_a - wD_b) - wD_a
    # wpf_bl = wpf_kx + wpf_at ...
    #        = wsD_kr(i) * (wD_a - wD_b) - 2 * wD_a - wI0_R(i) * I0
    # wpf_br = wpf_bl - wpf_ax ...
    #        = wsD_kr(i) * (wD_a - wD_b) - 2 * wD_a - wCf_s

    ## I0 and Cf ins
    # wI0_a = wpf_bl = wsD_kr(i) * (wD_a - wD_b) - 2 * wD_a - wI0_R(i) * I0
    # wCf_a = wpf_br = wsD_kr(i) * (wD_a - wD_b) - 2 * wD_a - wCf_s

    ## Output
    # wI0_b = wI0_R(i) * I0
    # Vf = 0.5 * (wI0_a + wI0_b) = 0.5 * wsD_kr(i) * (wD_a - wD_b) - wD_a

    ## State update
    # wCf_s = wCf_a = wsD_kr(i) * (wD_a - wD_b) - 2 * wD_a - wCf_s ...
    #       = 2 * Vf - wCf_s

    ## Summing up
    p     = wI0_R(i) * I0;
    wD_a  = wpf_kt(i) * (p - wCf_s) - p;
    if (wD_a >= 0)
      wD_b = wD_a + wD_k1(i) + wD_k2 * lambertw(exp(wD_k3x(i) + wD_k4 * wD_a));
    else
      wD_b = wD_a - wD_k1(i) - wD_k2 * lambertw(exp(wD_k3x(i) - wD_k4 * wD_a));
    endif
    Vf    = wsD_krx(i) * (wD_a - wD_b) - wD_a;
    wCf_s = Vf + Vf - wCf_s;

    ### Global output

    y(i)   = Vf + Vp;

  endfor

endfunction

function y = limit(x, low, up, name)
  y = x;
  if (min(y) < low || max(y) > up)
    warning(["noninv_distortion: limiting " name " to [" num2str(low) ", " ...
             num2str(up) "]"]);
    y(y < low) = low;
    y(y > up)  = up;
  endif
endfunction
