#!/usr/bin/perl

#
# Ohjelma juttelemaan F1EHN EME-ohjaimen kanssa..
#

$asyname = '/dev/ttyS0';

$LastTime = time();

$ospeed = 9600;


@lines = ('','','','','','','');

use POSIX;      # POSIX facilities (for serial port)
use IO;         # Char by char IO
use Term::Cap;  # Terminal capabilities strings
use Math::Trig; # grad2rad()/rad2grad()

select STDOUT; $| = 1;

$terminal = Tgetent Term::Cap { TERM => undef, OSPEED => $ospeed };
$terminal->Trequire(qw/cl cm/);
#$terminal->Tgoto('cm', $col, $row, $FH);
#$terminal->Tputs('dl', $count, $FH);
#$terminal->Tpad($string, $count, $FH);

$CursorVisible = $terminal->Tputs('vs',1); # StandOut cursor
$CursorInvisible = $terminal->Tputs('vi',1); # Invisible cursor
$B1 = $terminal->Tputs('md',1); # Start Bold
$B0 = $terminal->Tputs('me',1); # End Bold (actually any)
$TermHome = "\e[?5h".$terminal->Tputs('cl',1);

$termios = new POSIX::Termios;

$ASY = POSIX::open($asyname,O_RDWR,0)
    or die "ei saa avattua sarjaporttia: ".$asyname;

die "ei saa avattua sarjaporttia: ".$asyname if ($ASY < 0);


$asyio   = IO::Handle->new_from_fd($ASY,'w+')
    or die "IO::Handle:n avaus sarjaporttiin epäonnistui.";

$conio   = IO::Handle->new_from_fd(fileno(STDIN),'r')
    or die "IO::Handle:n avaus STDIN:iin epäonnistui.";

$asyio->autoflush(true);

&asycanon ();

#$sttycmd='stty sane -echo raw istrip speed 2400 -opost evenp inpck';
#$sttycmd='stty speed 2400 evenp inpck istrip cread icanon -echo';
$sttycmd='stty speed 9600 clocal cread pass8 raw min 1';

system($sttycmd.' > /dev/null < '.$asyname);
system($sttycmd.' > /dev/null < '.$asyname);
#system('stty -a < '.$asyname);
#sleep 1;

printf "Sarjaportti auki, kaapelitesti!\n";

POSIX::tcflush($ASY, &POSIX::TCIOFLUSH);
POSIX::tcflow ($ASY, &POSIX::TCION);

system "stty","sane";
system "stty","-icanon","min","1","time","0", "-echo";

&cmdallstop;

while ( &iomux(1,1) ) { };

&cmdallstop;

system "stty","sane";
printf "%s\n",$CursorVisible;

exit 0;

sub asycanon {
    $termios->getattr($ASY) or die "ei saanut getattr() dataa";
    $termios->setispeed(&POSIX::B19200);
    $termios->setospeed(&POSIX::B19200);
    $termios->setcflag( &POSIX::CS8   | &POSIX::CREAD | # 8N1, Rx on
			&POSIX::CLOCAL               ); # ignore modem
    $termios->setiflag( &POSIX::IGNPAR );    # no input parity checking
    $termios->setoflag( 0 );	# No output-flags
    $termios->setlflag( &POSIX::ICANON ); # Pick lines..

    $termios->setattr($ASY, &POSIX::TCSANOW); # Set the attribute set
}


sub komentotulkki {
    local ($key) = @_;

    if ($key eq 'l') {
	cmdAZ(1);
	printf "AZ CW\n";
    }
    if ($key eq 'r') {
	cmdAZ(-1);
	printf "AZ CCW\n";
    }
    if ($key eq 'u') {
	cmdEL(1);
	printf "EL UP\n";
    }
    if ($key eq 'd') {
	cmdEL(-1);
	printf "EL DOWN\n";
    }

    if ($key eq '0') {
	cmdAZ(0);
	cmdEL(0);
	printf "ALL STOP\n";
    }
}

sub sysgetline {
    local $c;
    local $line = '';

    while ($asyio->sysread($c,1) && $c ne "\n") {
	# printf "SYSGETLINE c= 0x%02x = '%s'\n",ord($c),$c;
	$line .= $c;
    }
    return $line;
}

sub asyreadn {
    local $num = shift;

    local $line = '';
    local $c;

    for (;$num > 0; --$num) {
	$asyio->sysread($c,1);
	$line .= $c;
    }
    $line;
}

sub cmdAZ {
    local $dir = shift; # -1 / 0 / +1  (CCW / stop / CW)
    local $cmdbyte = 0; # Stop

    if ($dir == 0) {  # STOP
	$cmdbyte = 0;
	$AZdir = 0;
    } elsif ($dir > 0) {  # CW
	$cmdbyte = 1;
	$AZdir = 1;
    } elsif ($dir < 0) {  # CCW
	$cmdbyte = 2;
	$AZdir = -1;
    }

    # Read all trash away...

    $asyio->blocking(0);
    while (asyreadn(1) ne '') {
    }
    $asyio->blocking(1);
    
    # Write the command

    # Std preamble
    $asyio->printf("%c%c%c%c%c%c",150,128,160,149,192,224);
    # CmdAz
    $asyio->printf("%c",$cmdbyte);
    $asyio->flush;

    # Read all trash away...

    $asyio->blocking(0);
    while (asyreadn(1) ne '') {
    }
    $asyio->blocking(1);
}

sub cmdEL {
    local $dir = shift; # -1 / 0 / +1  (down / stop / up)
    local $cmdbyte;

    if ($dir == 0) {  # STOP
	$cmdbyte = 16;
	$ELdir = 0;
    } elsif ($dir > 0) {  # UP
	$cmdbyte = 18;
	$ELdir = 1;
    } elsif ($dir < 0) {  # DOWN
	$cmdbyte = 17;
	$ELdir = -1;
    }
    
    # Read all trash away...

    $asyio->blocking(0);
    while (asyreadn(1) ne '') {
    }
    $asyio->blocking(1);
    
    # Write the command

    # Std preamble
    $asyio->printf("%c%c%c%c%c%c",150,128,160,149,192,224);
    # StopEl
    $asyio->printf("%c",$cmdbyte);
    $asyio->flush;

    # Read all trash away...

    $asyio->blocking(0);
    while (asyreadn(1) ne '') {
    }
    $asyio->blocking(1);
}

sub cmdallstop {
    cmdAZ(0);
    cmdEL(0);
}

sub readazel {

    # Read all trash away...

    $asyio->blocking(0);
    while (asyreadn(1) ne '') {
    }
    $asyio->blocking(1);
    
    # Write the command

    $asyio->printf("%c%c%c%c%c%c",150,128,160,149,192,224);
    $asyio->flush;

    local $line = asyreadn(6);

    local $AZ = ord(substr($line,1,1))*256+ord(substr($line,2,1));
    local $EL = ord(substr($line,4,1))*256+ord(substr($line,5,1));



    $AZ = 360.0/65536.0 * $AZ;
    $EL = 360.0/65536.0 * $EL;
    if ($EL > 180.0) {
	$EL -= 360.0;
    }

    printf "AzEl: ";
    printf(" %7.3f  %7.3f", $AZ, $EL);
    printf "\n";

    return ($AZ, $EL);
}

sub iomux {
    local ($timeout,$keyinput) = @_;
    local $rin,$rout;
    local $ELdir = 0;
    local $AZdir = 0;

#printf "IOMUX(%.2f,%d) ",$timeout,$keyinput;

    $rin = '';
    if ($keyinput) {
	vec($rin,fileno(STDIN),1) = 1;
    }

    local ($AZ, $EL) = & readazel;

    if ($ELdir < 0.0 && $EL < 0.0) {
	& cmdELstop;
	$ELdir = 0;
    }
    if ($ELdir > 0 && $EL > 85.0) {
	& cmdELstop;
	$ELdir = 0;
    }


    local($nfound,$timeleft) = select($rout=$rin, undef, undef, $timeout);

    if ($keyinput) {
	printf "%s",$CursorInvisible;
    }

    if ($nfound == 0) {
#	printf "IOMUX-TIMEOUT!";
	return 1;
    }


    while (vec($rout,fileno(STDIN),1)) {

	local $line;

	$conio->blocking(0);
	$conio->sysread($line, 1);
	$conio->blocking(1);

printf("CONread len=%d line='%s'\n", length($line), $line);

	if ($line eq 'X' or $line eq 'x') {
	    return 0;
	}

	&komentotulkki($line);
	system "stty","-icanon","min","1","time","0";

	last;
    }

    return 1;
}



#printf "'%s'\n", &within_viestikallio($x,$y);

sub within_viestikallio {
    local ($x,$y) = @_; # Easting, Northing

# Data pairs: Easting, Northing
    local (@poly) = ( 613.69, 548.36,
		       669.00, 510.00,
		       593.27, 440.77,
		       398.07, 497.69,
		       425.46, 593.46,
		       517.44, 533.63 );
    
    #@poly = ( 0, 0,  100, 0, 100, 100, 0, 100 );

# Number of numbers in the poly array
    local ($N) = 1+$#poly;

# Separate the corner coordinate vector into X and Y components
    local (@x)=();
    local (@y)=();
    local ($i,$ii);
    for ($i = 0; $i < $N; $i += 2) {
	$ii = $i/2;
	$x[$ii] = $poly[$i+0];
	$y[$ii] = $poly[$i+1];
    }

    $N = $N / 2;

    local ($allwithin)  = 1;
    local ($alloutside) = 1;

    local ($smallestinsidedist)  = 999999999999;
    local ($smallestoutsidedist) = 999999999999;
    local ($smallest_s1)         = 999999999999;

    local ($lowIsLeft)  =  99999999999;
    local ($highIsLeft) = -99999999999;

    local ($i1, $i2, $isLeft);

    for ($i1 = 0; $i1 < $N; ++$i1) {
	$i2 = ($i1 + 1) % $N;

	$isLeft = ( ($x[$i2] - $x[$i1]) * ($y - $y[$i1]) -
		    ($y[$i2] - $y[$i1]) * ($x - $x[$i1]) );

	$lowIsLeft  = $isLeft if ($lowIsLeft  > $isLeft);
	$highIsLeft = $isLeft if ($highIsLeft < $isLeft);

	printf "Vector %d  isLeft: %.3f\n", $i1, $isLeft;

	local ($l2) = ( ($x[$i1] - $x[$i2]) * ($x[$i1] - $x[$i2]) +
			($y[$i1] - $y[$i2]) * ($y[$i1] - $y[$i2]) );

	local ($u)  = ( ( ($x - $x[$i1]) * ($x[$i2] - $x[$i1]) +
			  ($y - $y[$i1]) * ($y[$i2] - $y[$i1]) ) / $l2 );

	local ($x_) = $x[$i1] + $u * ($x[$i2] - $x[$i1]);
	local ($y_) = $y[$i1] + $u * ($y[$i2] - $y[$i1]);

	local ($s)  = vdist( $x, $y, $x_, $y_);
	local ($s1) = vdist( $x, $y, $x[$i1], $y[$i1] );

	local ($within) = ( iswithin($x[$i1], $x_, $x[$i2]) &&
			    iswithin($y[$i1], $y_, $y[$i2]) );

	$smallest_s1 = $s1 if ($smallest_s1 > $s1);

	if ($within) {
	    $alloutside = 0;
	    $smallestinsidedist  = $s if ($smallestinsidedist > $s);
	} else {
	    $allwithin  = 0;
	    #$smallestoutsidedist = $s if ($smallestoutsidedist > $s);
	}


	printf("Edge: %d xsect: %9.3f/%9.3f is%s within this edge; dist: %.3f\n",
	       $i1, $x_, $y_,
	       $within ? "   " : "n't",
	       $s);

    }

    printf "OUTSIDE\n" if ($alloutside);
    printf "INSIDE\n"  if ($allwithin);

    printf "IsLeft low=%.1f high=%.1f\n", $lowIsLeft, $highIsLeft;

    printf("Smallest cross-sect distance to within an edge: %.3f\n",
	   $smallestinsidedist);
    printf("Smallest cross-sect distance to edge extensions: %.3f\n",
	   $smallestoutsidedist);
    printf("Smallest corner distance: %.3f\n", $smallest_s1);
    
    if (($lowIsLeft >= 0 && $highIsLeft >= 0) ||
	($lowIsLeft <= 0 && $highIsLeft <= 0) ) {
	printf "INSIDE\n";
	$inside = 1;
    } else {
	printf "OUTSIDE\n";
	$inside = 0;
    }

    $dpydist = $smallestinsidedist;
    if ($dpydist > $smallest_s1) {
	$dpydist = $smallest_s1;
	$inside = 2;
    }

    $dpydist = 99.9 if ($dpydist > 99.9);

    if ($dpydist < 10) {
	return sprintf("5.9. %d %4.2f", $inside, $dpydist);
    } else {
	return sprintf("5.9. %d %4.1f", $inside, $dpydist);
    }
}


sub iswithin {
    local ($low, $test, $high) = @_;

    return 1 if ($low <= $test && $test <= $high);
    # If the values are in reverse order..
    return 1 if ($low >= $test && $test >= $high);
    # Not within the end points
    return 0;
}

sub vdist {
    local ($x1, $y1, $x2, $y2) = @_;

    return sqrt( ($x1 - $x2) * ($x1 - $x2) +
		 ($y1 - $y2) * ($y1 - $y2) );
}
