#!/usr/bin/perl 
# 
# dartsfilter.pl
# 
# The DARTS filtering and prediction algorithm testbed. 
#
# Copyright (c) 2003 Jeffrey M. Laughlin, n1ywb@arrl.org
# 
# This program is free software; you can redistribute it and/or modify     
# it under the terms of the GNU General Public License as published by     
# the Free Software Foundation; either version 2 of the License, or        
# (at your option) any later version.                                      
#                                                                          
# This program is distributed in the hope that it will be useful, 
# but WITHOUT ANY WARRANTY; without even the implied warranty of           
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            
# GNU General Public License for more details.                             
# 
# You should have received a copy of the GNU General Public License        
# along with this program; if not, write to the Free Software              
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

use Curses;
use Time::HiRes qw( ualarm gettimeofday );

$SIG{INT}  = \&sighndl;
$SIG{TERM} = \&sighndl;

# curses initialization
initscr;
cbreak;
noecho;
clear;
refresh;

refresh();

sysopen(DARTS, "/dev/usb/darts-usb0", 0) || (
    cleanup() && 
    die "couln't open darts device");

while (1)
{
    my @strength = (0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
    my @antinoise = (0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
    my $buff;
    my @vals;
    my $total;
    my $last_bearing;
    my $steady;
    my @confidence;

    # This strangeness allows us to read data constantly from the device, but
    # once every second stop for a moment to process the data from the previous
    # second.
    eval {
        my $siglast;
        # Local anonymous signal handler functions
        local $SIG{ALRM} = sub { 
            local($sig) = @_;
            $siglast = $sig;
        };
        local $SIG{INT}  = sub { die "int\n" };
        local $SIG{TERM} = sub { die "term\n" };
        ualarm (1000000,0); # Set an alarm 
        $last_bearing = -1;
        $steady = 0;
        while (1)
        {
            # Read only the exact amount of data
            if ( sysread(DARTS, $buff, 1 + 4 + 4) < 1 + 4 + 4 )
            {
                cleanup();
                die ("read too little data");
            }
            # Decode the data structure
            # DARTS returns a data structure consisting of a char and two longs
            @vals = unpack("Cll", $buff);

            ++ $strength[$vals[0]];
            ++ $total;

            # See if this bearing was also the last bearing
            # This determines the noise level
            if ( $vals[0] == $last_bearing )
            {
                ++ $steady;
            }

            # If the alarm went off, get ready to exit the loop
            if ($siglast) { 
                last; 
                $vals[0] = -1;
            }

            # If the bearing changed, calculate the antinoise
            if ( $vals[0] != $last_bearing )
            {
                $antinoise[$last_bearing] += $steady**2;
                $steady = 0;
            }

            $last_bearing = $vals[0];
        }
    };

    # See if the loop exit was caused by a signal
    if ($@)
    {
        if ( $@ eq "alarm\n") {
          # Alarm signal, do nothing
        }
        elsif ( $@ eq "int\n" || $@ eq "term\n" ) {
            cleanup();
            die;
        }
        else {
            cleanup();
            die "Unexpected error '$@'";
        }
    }

    # Process data from the previous second 
    clear();
    move(0,0);

    # Normalize input values to 1
    for ($i = 0; $i < @strength; ++ $i)
    {
        if ($strength[$i] == 0) {
            $antinoise[$i] = 1; }
        else {
            $antinoise[$i] /= $strength[$i] ** 2; }

        $strength[$i] /= $total;
    }
    
    @confidence = fuzzyfilter($total, \@strength, \@antinoise);
    
    # The bearing with the highest confidence wins. This is dumb, it should use a
    # smarter algorithm. The hard part is that the function is cylindrical, not
    # linear.
    $greatest = 0;
    for ($i = 0; $i < @confidence; ++ $i)
    {
        printw("%2d : %.4f : %.4f : %1.3f\n", $i, $strength[$i], $antinoise[$i], $confidence[$i]); 
        if ($confidence[$i] > $confidence[$greatest])
        {
            $greatest = $i;
        }
    }

    printw("total - %4d\n", $total);

    draw_compass($greatest);
    
    refresh();

#    sleep(5);
    if ($sigtermed) {
        last;
    }
}

cleanup();

# Release curses and close the device file.
sub cleanup {
    standend;
    clear;
    refresh;
    endwin;
    close (DARTS);
}

# Signal handler for main loop
sub sighndl {
    local($sig) = @_;
    $sigtermed = $sig;
}

# Draws the compass on the screen, with the argument as the highlighted
# point.
sub draw_compass {
    my $led;
    my @x;
    my @y;
    my $i;
    my $offset = 35;

    $led = shift();
    @x = (15, 20, 24, 28, 30, 28, 24, 20, 15, 10,  6,  2,  0,  2,  6, 10, 15);
    @y = ( 0,  0,  1,  3,  6,  9, 11, 12, 12, 12, 11,  9,  6,  3,  1,  0,  6);
    
    for( $i = 0; $i <= 16; ++ $i )
    {
        addch($y[$i], $offset + $x[$i] + 1, '*');
        if ($i == $led) {
            addch($y[$i], $offset + $x[$i] - 1 + 1, '(');
            addch($y[$i], $offset + $x[$i] + 1 + 1, ')');
        }
    }
}

# Fuzzy logic filtering function.
sub fuzzyfilter {
    my $total = shift();
    my $strength = shift();
    my $antinoise = shift();
    my $maxval = 0;
    my $maxbearing = -1;
    my $i;
    my @confidence;

    for ( $i = 0; $i < @$strength; ++ $i ) {
        my $s1; my $s3; my $s5; my $s7; my $s9;
        my $n1; my $n3; my $n5; my $n7; my $n9;
        my $c1; my $c3; my $c5; my $c7; my $c9;

        # Variable names are like an "s-meter"
        # Compute input memberships. 
        $s1 = membership($$strength[$i], -1.0, 0.0, 0.3333);
        $s3 = membership($$strength[$i], 0.0, 0.3333, 0.6666);
        $s5 = membership($$strength[$i], 0.3333, 0.5, 0.75);
        $s7 = membership($$strength[$i], 0.5, 0.75, 1.0);
        $s9 = membership($$strength[$i], 0.75, 1.0, 2.0);

        $n1 = membership($$antinoise[$i], -1.0, 0.0, 0.2);
        $n3 = membership($$antinoise[$i], 0.1, 0.2, 0.3);
        $n5 = membership($$antinoise[$i], 0.2, 0.3, 0.5);
        $n7 = membership($$antinoise[$i], 0.4, 0.6, 0.8);
        $n9 = membership($$antinoise[$i], 0.5, 0.9, 1.0, 2.0);

        # Compute output memberships.
        $c1 = fuzzy_or($s1, $n1); 
        $c3 = fuzzy_or($s3, $n3);
        $c5 = fuzzy_or($s5, $n5);
        $c7 = fuzzy_or($s7, $n7);
        $c9 = fuzzy_and($s9, $n9); 

        # Compute overall level of confidence
        # This is sort of a lazy way to do this, but it works. Doing something
        # like finding the center of mass would be better.
        $confidence[$i] = which_is_biggest($c1, $c3, $c5, $c7, $c9) / 4.0; 
    }

    return @confidence;
}

# Takes a list, returns the index of the largest item in the list.
sub which_is_biggest {
    my $biggest = 0;
    my $bigval = 0;
    my $i;

    for ($i = 0; $i < @_; ++ $i) {
        if ( $_[$i] > $bigval )
        {
            $bigval = $_[$i];
            $biggest = $i;
        }
    }

    return $biggest;
}

# Computes membership in a fuzzy function. Takes an X value, and either two or
# three points. In the case of three points, the function is triangular, with
# points y(0) and y(2) = 0, and point y(1) = 1. In the case of four points, 
# the function is a trapazoid, with points y(0) and y(3) = 0, and points y(1)
# and y(2) = 1.
sub membership {
    my $xval = shift();
    my $yval;
    my @p = @_;

    if ($xval >= $p[0] && $xval <= $p[1]) {
        $yval = ( $xval - $p[0] ) * ( 1 / ( $p[1] - $p[0] ) ); }
    
    if ( @p == 4 ) {
        if ($xval >= $p[1] && $xval <= $p[2]) {
            $yval = 1;
        }
        shift(@p);
    }

    if ($xval >= $p[1] && $xval <= $p[2]) {
        $yval = ( $p[2] - ( $xval - $p[1] ) ) * ( 1 / ( $p[2] - $p[1] ) ); }
   
    return $yval;
} 

# Returns the fuzzy-and of a list of inputs
sub fuzzy_and {
    my $min = shift();
    my $v;
    foreach $v (@_) {
        $min = $v if $min > $v;
    }
    return $min;
}

# Returns the fuzzy-or of a list of inputs
sub fuzzy_or {
    my $max = shift();
    my $v;
    foreach $v (@_) {
        $max = $v if $max < $v;
    }
    return $max;
 }