# filtergen.tcl --
#     Package for digital filters
#     filterButterworth:
#         Generate the coefficients for a low-pass or high-pass Butterworth filter
#     filterChebyshev:
#         Generate the coefficients for a low-pass or high-pass Chebyshev filter
#     filter:
#         Filter an entire series of data
#     filterObject:
#         Class to create filters
#
#     Derived from: https://github.com/adis300/filter-c, which has an MIT license
#     (Note: the original code had a small error, this has been corrected here)
#
#     More information on Chebyshev filters: https://en.wikipedia.org/wiki/Chebyshev_filter
#
#     Note:
#     We can implement band pass/stop filters from the same source.
#
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 (nested 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 || $order % 2 != 0 } {
        return -code error "The order must be an even positive integer"
    }
    if { $samplefreq <= 0.0 || $cutofffreq <= 0.0 } {
        return -code error "The frequencies must be positive"
    }
    if { $samplefreq < 2.0 * $cutofffreq } {
        return -code error "The cutoff frequency must be higher than half the sample frequency"
    }

    set pi     [expr {acos(-1.0)}]

    set a      [expr {tan($pi * $cutofffreq / $samplefreq)}]
    set a2     [expr {$a**2}]

    set s      [expr {1.0 * $samplefreq}]
    set order  [expr {$order / 2}]

    for {set i 0} {$i < $order} {incr i} {
        set r [expr {sin($pi * (2.0 * $i + 1.0) / (4.0 * $order))}]
        set s [expr {$a2 + 2.0 * $a * $r + 1.0}]
        lappend A  [expr {$a2 / $s}]
        lappend d1 [expr {2.0 * (1.0 - $a2) / $s}]
        lappend d2 [expr {-($a2 - 2.0 * $a * $r + 1.0) / $s}]
    }

    set coeffw1 [expr {$lowpass ? 2.0 : -2.0}]
    set scale   [expr {1.0}]

    return [list $coeffw1 $scale $A $d1 $d2]
}

# filterChebyshev --
#     Generate the coefficients for a low-pass/high-pass Chebyshev 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)
#     epsilon              Ripple factor (defaults to 0.1)
#
# Returns:
#     List (nested list) of coefficients for x and y and the scale factor
#
proc ::math::filters::filterChebyshev {lowpass order samplefreq cutofffreq {epsilon 0.1}} {

    ##nagelfar ignore
    if { ![string is integer $order] || $order <= 0 || $order % 2 != 0 } {
        return -code error "The order must be an even positive integer"
    }
    if { $samplefreq <= 0.0 || $cutofffreq <= 0.0 } {
        return -code error "The frequencies must be positive"
    }
    if { $samplefreq < 2.0 * $cutofffreq } {
        return -code error "The cutoff frequency must be higher than half the sample frequency"
    }
    if { $epsilon <= 0.0 } {
        return -code error "The ripple factor should be positive"
    }

    set pi     [expr {acos(-1.0)}]

    set a      [expr {tan($pi * $cutofffreq / $samplefreq)}]
    set a2     [expr {$a**2}]

    set s      [expr {1.0 * $samplefreq}]
    set order  [expr {$order / 2}]

    set u      [expr {log((1.0 + sqrt(1.0 + $epsilon**2)) / $epsilon)}]
    set cu     [expr {cosh($u / (2.0 * $order))}]
    set su     [expr {sinh($u / (2.0 * $order))}]

    for {set i 0} {$i < $order} {incr i} {
        set b [expr {sin($pi * (2.0 * $i + 1.0) / (4.0 * $order)) * $su}]
        set c [expr {cos($pi * (2.0 * $i + 1.0) / (4.0 * $order)) * $cu}]
        set c [expr {$b**2 + $c**2}]
        set s [expr {$a2 * $c + 2.0 * $a * $b + 1.0}]
        lappend A  [expr {$a2 / (4.0 * $s)}]
        lappend d1 [expr {2.0 * (1.0 - $a2 * $c) / $s}]
        lappend d2 [expr {-($a2 * $c - 2.0 * $a * $b + 1.0) / $s}]
    }

    set coeffw1 [expr {$lowpass ? 2.0 : -2.0}]
    set scale   [expr {2.0 / $epsilon}]

    return [list $coeffw1 $scale $A $d1 $d2]
}

# 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 coeffw1 scale list_A list_d1 list_d2

    set filtered {}

    set n       [llength $list_A]
    set list_w0 [lrepeat $n [expr {0.0}]]
    set list_w1 $list_w0
    set list_w2 $list_w0

    for {set j 0} {$j < [llength $data]} {incr j} {
        set x [lindex $data $j]

        set i 0
        foreach A $list_A d1 $list_d1 d2 $list_d2 w1 $list_w1 w2 $list_w2 {
            set  w0 [expr {$d1 * $w1 + $d2 * $w2 + $x}]
            set  x  [expr {$A * ($w0 + $coeffw1 * $w1 + $w2)}]
            lset list_w0 $i $w0
            lset list_w2 $i $w1
            lset list_w1 $i $w0
            incr i
        }

        lappend filtered [expr {$scale * $x}]
    }

    return $filtered
}

# filterObject --
#     Create an object that can filter incoming data
#
# Arguments:
#     coeff           Filter coefficients, as generated by filtergen
#
# Note:
#     Dropping the optional initial 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 as provided by the filter generator
    # - prepare everything
    #
    constructor {coeff} {
        variable coeffw1
        variable scale
        variable list_A
        variable list_d1
        variable list_d2
        variable list_w0
        variable list_w1
        variable list_w2

        lassign $coeff coeffw1 scale list_A list_d1 list_d2

        set n       [llength $list_A]
        set list_w0 [lrepeat $n [expr {0.0}]]
        set list_w1 $list_w0
        set list_w2 $list_w0
    }

    method filter {x} {
        variable coeffw1
        variable scale
        variable list_A
        variable list_d1
        variable list_d2
        variable list_w0
        variable list_w1
        variable list_w2

        set i 0
        foreach A $list_A d1 $list_d1 d2 $list_d2 w1 $list_w1 w2 $list_w2 {
            set  w0 [expr {$d1 * $w1 + $d2 * $w2 + $x}]
            set  x  [expr {$A * ($w0 + $coeffw1 * $w1 + $w2)}]
            lset list_w0 $i $w0
            lset list_w2 $i $w1
            lset list_w1 $i $w0
            incr i
        }

        return [expr {$scale * $x}]
    }

    method reset {} {
        variable list_A
        variable list_w0
        variable list_w1
        variable list_w2

        set n       [llength $list_A]
        set list_w0 [lrepeat $n [expr {0.0}]]
        set list_w1 $list_w0
        set list_w2 $list_w0
    }
}

# Publish the package

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

package provide math::filters 0.4
