# filtergen.tcl --
#     Package for digital filters
#     filterButterworth:
#         Generate the coefficients for a low-pass or high-pass Butterworth filter
#     filter:
#         Filter an entire series of data
#     filterObject:
#         Class to create filters
#
#     Derived from: https://www.meme.net.au/butterworth.html
#
#     Here is the license notice from this webpage:
#
#       @licstart  The following is the entire license notice for the
#       JavaScript code in this page.
#
#       Copyright (C) 2013 Glenn McIntosh
#
#       The JavaScript code in this page is free software: you can
#       redistribute it and/or modify it under the terms of the GNU
#       General Public License (GNU GPL) as published by the Free Software
#       Foundation, either version 3 of the License, or (at your option)
#       any later version.  The code is distributed WITHOUT ANY WARRANTY;
#       without even the implied warranty of MERCHANTABILITY or FITNESS
#       FOR A PARTICULAR PURPOSE.  See the GNU GPL for more details.
#
#       As additional permission under GNU GPL version 3 section 7, you
#       may distribute non-source (e.g., minimized or compacted) forms of
#       that code without the copy of the GNU GPL normally required by
#       section 4, provided you include this license notice and a URL
#       through which recipients can access the Corresponding Source.
#
#       @licend  The above is the entire license notice
#       for the JavaScript code in this page.
#
package require Tcl 8.6 9
package require TclOO

namespace eval ::math::filters {}

# filterButterworth --
#     Generate the coefficients for a low-pass/high-pass Butterworth filter
#
# Arguments:
#     lowpass              Low-pass if 1, high-pass if 0
#     order                Order of the filter
#     samplefreq           Sample frequency
#     cutofffreq           Cut-off frequency (3 dB point)
#
# Returns:
#     List (nexted list) of coefficients for x and y and the scale factor
#
proc ::math::filters::filterButterworth {lowpass order samplefreq cutofffreq} {

    ##nagelfar ignore
    if { ![string is integer $order] || $order <= 0 } {
        return -code error "The order must be a positive integer"
    }
    if { $samplefreq <= 0.0 || $cutofffreq <= 0.0 } {
        return -code error "The frequencies must be positive"
    }
    if { $samplefreq < $cutofffreq } {
        return -code error "The cutoff frequency must be lower than the sample frequency"
    }

    set pi     [expr {acos(-1.0)}]
    set cutoff [expr {-$cutofffreq / double($samplefreq) * 2.0 * $pi}]

    set yf0    [lrepeat [expr {$order+1}] 0.0]
    set yf1    $yf0
    set xf     $yf0

    lset yf0   0 -1.0
    lset yf1   0  0.0
    lset xf    0  1.0

    set scale  1.0
    set invert [expr {$lowpass == 1? 1.0 : -1.0}]

    for {set i 1} {$i <= $order} {incr i} {
        set angle  [expr {($i-0.5) / $order * $pi}]
        set sinsin [expr {1.0 - sin($cutoff) * sin($angle)}]
        set rcof0  [expr {-cos($cutoff) / $sinsin}]
        set rcof1  [expr { sin($cutoff) * cos($angle) / $sinsin}]

        lset yf0 $i 0.0
        lset yf1 $i 0.0

        for {set j $i} {$j > 0} {incr j -1} {
            set yf0jm1 [lindex $yf0 [expr {$j-1}]]
            set yf1jm1 [lindex $yf1 [expr {$j-1}]]
            set yf0j   [lindex $yf0 $j]
            set yf1j   [lindex $yf1 $j]

            lset yf0 $j [expr {$yf0j + $rcof0 * $yf0jm1 + $rcof1 * $yf1jm1}]
            lset yf1 $j [expr {$yf1j + $rcof0 * $yf1jm1 - $rcof1 * $yf0jm1}]
        }

        set  scale [expr {$scale * $sinsin * 2.0 / (1.0 - cos($cutoff) * $invert)}]
        set  xfim1 [lindex $xf [expr {$i-1}]]
        lset xf $i [expr {$xfim1 * $invert * ($order-$i+1)/double($i)}]
    }

    set scale [expr {sqrt($scale)}]

    for {set i 1} {$i <= $order} {incr i} {
        set  yf0i   [lindex $yf0 $i]
        lset yf0 $i [expr {$yf0i * $scale}]
    }

    return [list $xf [lrange $yf0 1 end] $scale]
}

# filter --
#     Filter the data series based on the given coefficients
#
# Arguments:
#     coeff           Filter coefficients, as generated by filtergen
#     data            Data to be filtered
#
# Returns:
#     The filtered data
#
# Note:
#     The initial part of the filtered data is a list of zeroes
#
proc ::math::filters::filter {coeff data} {
    lassign $coeff xcoeff ycoeff scale

    set filtered {}

    set yv [lrepeat [llength $ycoeff] [expr {0.0}]]

    set noxcoeff [llength $xcoeff]
    set xcoeff   [lreverse $xcoeff]
    set ycoeff   [lreverse $ycoeff]

    for {set i 0} {$i <= [llength $data]-$noxcoeff} {incr i} {
        set xv [lrange $data $i [expr {$i+$noxcoeff-1}]]

        set f  [expr {0.0}]

        foreach x $xv c $xcoeff {
            set f [expr {$f + $c * $x}]
        }

        foreach y $yv c $ycoeff {
            set f [expr {$f + $c * $y}]
        }

        set f [expr {$f / $scale}]

        lappend filtered $f

        set yv [concat [lrange $yv 1 end] $f]
    }

    return $filtered
}

# filterObject --
#     Create an object that can filter incoming data
#
# Arguments:
#     coeff           Filter coefficients, as generated by filtergen
#     yinit           (Optional) initial y-values
#
::oo::class create ::math::filters::filterObject {
    variable xcoeff
    variable ycoeff
    variable yv
    variable xv
    variable xv_org
    variable yv_org

    #
    # Constructor:
    # - the arguments coeff and, optionally, yinit
    # - prepare everything
    #
    constructor {coeff {yinit {}}} {
        variable xcoeff
        variable ycoeff
        variable scale
        variable yv
        variable xv
        variable yv_org
        variable xv_org

        lassign $coeff xcoeff ycoeff scale

        set xcoeff   [lreverse $xcoeff]
        set ycoeff   [lreverse $ycoeff]

        if { $yinit eq {} } {
            set yv [lrepeat [llength $ycoeff] [expr {0.0}]]
        } else {
            if { [llength $yinit] != [llength $ycoeff] } {
                return -code error "Length of initial y-values must be equal to the number of y coefficients"
            }
            set yv $yinit
        }
        set xv [lrepeat [llength $xcoeff] [expr {0.0}]]

        set xv_org $xv
        set yv_org $yv
    }

    method filter {x} {
        variable xcoeff
        variable ycoeff
        variable scale
        variable yv
        variable xv

        set xv [concat [lrange $xv 1 end] $x]
        set f  [expr {0.0}]

        foreach x $xv c $xcoeff {
            set f [expr {$f + $c * $x}]
        }

        foreach y $yv c $ycoeff {
            set f [expr {$f + $c * $y}]
        }

        set f  [expr {$f / $scale}]

        set yv [concat [lrange $yv 1 end] $f]

        return $f
    }

    method reset {} {
        variable yv
        variable xv
        variable xv_org
        variable yv_org

        set xv $xv_org
        set yv $yv_org
    }
}

# Publish the package

namespace eval ::math::filters {
    namespace export filterButterworth filter filterObject
}

package provide math::filters 0.3