## 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} =} inv_distortion (@var{fs}, @var{x}, @var{dist})
##
## Simulate the inverting 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{\alpha}), 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: inverting distortion guitar

function y = inv_distortion(fs, x, dist)

  #### Input arguments

  # Checks

  if (nargin != 3)
    print_usage();
  elseif (!isscalar(fs) || !isnumeric(fs) || !isreal(fs) || fs <= 0)
    error("inv_distortion: FS must be a positive real");
  elseif (isscalar(x) || !isvector(x) || !isnumeric(x) || !isreal(x))
    error("inv_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(["inv_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.001, 0.999, "DIST");

  #### Constants

  # Circuit components

  R1 = 22e3;
  R2 = 12e3;
  C1 = 47e-9;
  C2 = 1e-9;
  Rp = 220e3;
  Cf = 47e-12;
  Rs = 8.163;
  Is = 16.88e-9;
  nD = 9.626;
  VT = 25.85e-3;

  # WDF element values

  wC1_R  = 1 / (2 * C1 * fs);
  wC2_R  = 1 / (2 * C2 * fs);
  wCf_R  = 1 / (2 * Cf * fs);
  wRs_R  = Rs;
  wD_k2  = -2 * nD * VT;
  wD_k4  = 1 / (nD * VT);
  ki     = 1 / (2 * R2);

  #### Non-constants

  # WDF element values

  wRp_R  = (1 - dist) * Rp;
  wp2_kt = wC2_R ./ (wRp_R + wC2_R);
  wp2_R  = (wRp_R * wC2_R) ./ (wRp_R + wC2_R);
  ws2_R  = R2 + wp2_R;
  wp1_kt = R1 ./ (ws2_R + R1);
  wp1_R  = (ws2_R * R1) ./ (ws2_R + R1);
  # ws1_R  = wp1_R + wC1_R;
  ws1_kl = wp1_R ./ (wp1_R + wC1_R);
  ws2_kl = R2 ./ (R2 + wp2_R);

  wI0_R   = dist * Rp;
  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 coefficients

  kIC1x  = 2 * ki * ws1_kl .* ws2_kl;
  kIC2   = -2 * ki * ws2_kl .* (1 - wp2_kt) ...
           .* (1 - wp1_kt .* (1 - ws1_kl));
  kC1x   = 2 * (1 - ws1_kl);
  kC1C1  = 2 * ws1_kl - 1;
  kC1C2  = 2 * wp1_kt .* (1 - wp2_kt) .* (1 - ws1_kl);
  kC2C1x = 2 * ws1_kl .* (1 - ws2_kl);
  kC2C2  = 2 * (1 - wp2_kt) ...
           .* (ws2_kl + wp1_kt .* (1 - ws1_kl) .* (1 - ws2_kl)) - 1;

  #### Filter

  y     = zeros(1, length(x));
  wC1_s = 0;
  wC2_s = 0;
  wCf_s = 0;

  for i = 1:length(x)

    ### Inverted input subcircuit

    ## R1, R2, Rp, C1, and C2 outs
    # wR1_b = 0
    # wR2_b = 0
    # wRp_b = 0
    # wC1_b = wC1_s
    # wC2_b = wC2_s

    ## Parallel junction 2 (upwards)
    # wp2_al = wC2_b = wC2_s
    # wp2_ar = wRp_b = 0
    # wp2_bt = wp2_kt(i) * (wp2_ar - wp2_al) + wp2_al = (1 - wp2_kt(i)) * wC2_s

    ## Series junction 2 (upwards)
    # ws2_al = wR2_b = 0
    # ws2_ar = wp2_bt = (1 - wp2_kt(i)) * wC2_s
    # ws2_bt = -(ws2_al + ws2_ar) = (wp2_kt(i) - 1) * wC2_s

    ## Parallel junction 1 (upwards)
    # wp1_al = wR1_b = 0
    # wp1_ar = ws2_bt = (wp2_kt(i) - 1) * wC2_s
    # wp1_bt = wp1_al + wp1_kt(i) * (wp1_ar - wp1_al) ...
    #        = wp1_kt(i) * (wp2_kt(i) - 1) * wC2_s

    ## Series junction 1 (upwards)
    # ws1_al = wp1_bt = wp1_kt(i) * (wp2_kt(i) - 1) * wC2_s
    # ws1_ar = wC1_b = wC1_s
    # ws1_bt = -(ws1_al + ws1_ar) ...
    #        = wp1_kt(i) * (1 - wp2_kt(i)) * wC2_s - wC1_s

    ## Vin
    # wVin_a = ws1_bt = wp1_kt(i) * (1 - wp2_kt(i)) * wC2_s - wC1_s
    # wVin_b = -2 * x(i) - wVin_a ... (see Errata)
    #        = wC1_s - 2 * x(i) - wp1_kt(i) * (1 - wp2_kt(i)) * wC2_s

    ## Series junction 1 (downwards)
    # ws1_at = wVin_b ...
    #        = wC1_s - 2 * x(i) - wp1_kt(i) * (1 - wp2_kt(i)) * wC2_s
    # ws1_bl = ws1_al + ws1_kl(i) * (ws1_bt - ws1_at) ...
    #        = 2 * ws1_kl(i) * (x(i) - wC1_s) ...
    #          - wp1_kt(i) * (1 - wp2_kt(i)) * (1 - 2 * ws1_kl(i))
    # ws1_br = -(ws1_bl + ws1_at) ...
    #        = 2 * (1 - ws1_kl(i)) * x(i) - (1 - 2 * ws1_kl(i)) * wC1_s ...
    #          + 2 * wp1_kt(i) * (1 - wp2_kt(i)) * (1 - ws1_kl(i)) * wC2_s

    ## Parallel junction 1 (downwards)
    # wp1_at = ws1_bl ...
    #        = 2 * ws1_kl(i) * (x(i) - wC1_s) ...
    #          - wp1_kt(i) * (1 - wp2_kt(i)) * (1 - 2 * ws1_kl(i))
    # wp1_bx = wp1_at + wp1_bt ...
    #        = 2 * (ws1_kl(i) * (x(i) - wC1_s) ...
    #               - wp1_kt(i) * (1 - wp2_kt(i)) * (1 - ws1_kl(i)) * wC2_s)
    # wp1_bl = wp1_bx - wp1_al ...
    #        = 2 * (ws1_kl(i) * (x(i) - wC1_s) ...
    #               - wp1_kt(i) * (1 - wp2_kt(i)) * (1 - ws1_kl(i)) * wC2_s)
    # wp1_br = wp1_bx - wp1_ar ...
    #        = 2 * ws1_kl(i) * (x(i) - wC1_s) ...
    #          + (1 - wp2_kt(i)) * (1 - 2 * wp1_kt(i) * (1 - ws1_kl(i))) * wC2_s

    ## Series junction 2 (downwards)
    # ws2_at = wp1_br ...
    #        = 2 * ws1_kl(i) * (x(i) - wC1_s) ...
    #          + (1 - wp2_kt(i)) * (1 - 2 * wp1_kt(i) * (1 - ws1_kl(i))) * wC2_s
    # ws2_bl = ws2_al + ws2_kl * (ws2_bt - ws2_at) ...
    #        = 2 * ws2_kl(i) * (ws1_kl(i) * (wC1_s - x(i)) ...
    #                           - (1 - wp2_kt(i)) * (1 - wp1_kt(i) ...
    #                              * (1 - ws1_kl(i))) * wC2_s)
    # ws2_br = -(ws2_bl + ws2_at) ...
    #        = 2 * ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #          - (1 - wp2_kt(i)) * (1 - 2 * (ws2_kl(i) + wp1_kt(i) ...
    #                                        * (1 - ws1_kl(i)) ...
    #                                        * (1 - ws2_kl(i)))) * wC2_s

    ## Parallel junction 2 (downwards)
    # wp2_at = ws2_br ...
    #        = 2 * ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #          - (1 - wp2_kt(i)) * (1 - 2 * (ws2_kl(i) + wp1_kt(i) ...
    #                                        * (1 - ws1_kl(i)) ...
    #                                        * (1 - ws2_kl(i)))) * wC2_s
    # wp2_bx = wp2_at + wp2_bt ...
    #        = 2 * (ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #               + (1 - wp2_kt(i)) * (ws2_kl(i) + wp1_kt(i) ...
    #                                    * (1 - ws1_kl(i)) ...
    #                                    * (1 - ws2_kl(i))) * wC2_s)
    # wp2_bl = wp2_bx - wp2_al ...
    #        = 2 * ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #          - (1 - 2 * (1 - wp2_kt(i)) * (ws2_kl(i) + wp1_kt(i) ...
    #                                        * (1 - ws1_kl(i)) ...
    #                                        * (1 - ws2_kl(i)))) * wC2_s
    # wp2_br = wp2_bx - wp2_ar ...
    #        = 2 * (ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #               + (1 - wp2_kt(i)) * (ws2_kl(i) + wp1_kt(i) ...
    #                                    * (1 - ws1_kl(i)) ...
    #                                    * (1 - ws2_kl(i))) * wC2_s)

    ## R2, C1, and C2 in
    # wR2_a  = ws2_bl ...
    #        = 2 * ws2_kl(i) * (ws1_kl(i) * (wC1_s - x(i)) ...
    #                           - (1 - wp2_kt(i)) * (1 - wp1_kt(i) ...
    #                              * (1 - ws1_kl(i))) * wC2_s)
    # wC1_a  = ws1_br ...
    #        = 2 * (1 - ws1_kl(i)) * x(i) - (1 - 2 * ws1_kl(i)) * wC1_s ...
    #          + 2 * wp1_kt(i) * (1 - wp2_kt(i)) * (1 - ws1_kl(i)) * wC2_s
    # wC2_a  = wp2_bl ...
    #        = 2 * ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #          - (1 - 2 * (1 - wp2_kt(i)) * (ws2_kl(i) + wp1_kt(i) ...
    #                                        * (1 - ws1_kl(i)) ...
    #                                        * (1 - ws2_kl(i)))) * wC2_s

    ## Output
    # I = ki * (wR2_a - wR2_b) ...
    #        = 2 * ki * ws2_kl(i) * (ws1_kl(i) * (wC1_s - x(i)) ...
    #                                - (1 - wp2_kt(i)) * (1 - wp1_kt(i) ...
    #                                   * (1 - ws1_kl(i))) * wC2_s)

    ## State updates
    # wC1_s  = wC1_a ...
    #        = 2 * (1 - ws1_kl(i)) * x(i) - (1 - 2 * ws1_kl(i)) * wC1_s ...
    #          + 2 * wp1_kt(i) * (1 - wp2_kt(i)) * (1 - ws1_kl(i)) * wC2_s
    # wC2_s  = wC2_a ...
    #        = 2 * ws1_kl(i) * (1 - ws2_kl(i)) * (wC1_s - x(i)) ...
    #          - (1 - 2 * (1 - wp2_kt(i)) * (ws2_kl(i) + wp1_kt(i) ...
    #                                        * (1 - ws1_kl(i)) ...
    #                                        * (1 - ws2_kl(i)))) * wC2_s

    ## Summing up
    C1x   = wC1_s - x(i);
    I0    = kIC1x(i)  * C1x  + kIC2(i)  * wC2_s;
    wC1_a = kC1x(i)   * x(i) + kC1C1(i) * wC1_s + kC1C2(i) * wC2_s;
    wC2_s = kC2C1x(i) * C1x  + kC2C2(i) * wC2_s;
    wC1_s = wC1_a;

    ### 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;

  endfor

endfunction

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