#!/usr/bin/perl

use IO::Socket;
use IO::Select;
use IO::File;

$host = "127.0.0.1";
$port = "2947";

$checkinterval = 10;
$ataltitude = 0;
$radius = 3963.1;

$safety = 17000;
$maxdistance = 100;

sub do_command {
  my @ready, $s, $buf;
  my($handle, $command) = @_;
  my $read_set = new IO::Select($handle);
  print $handle "$command\n";
  while (1) {
    if (@ready = $read_set->can_read(2)) {
      foreach $s (@ready) {
        $buf = <$s>;
        if ($buf =~ m/GPSD/) {
          return $buf;
        }
      }
    }
    else {
      return 0;
    }
  }
}

sub do_admon {
  my @ready, $s, $buf, $repcount;
  my($handle, $command) = @_;
  my $read_set = new IO::Select($handle);
  while ($repcount < 3) {
    print $handle "$command";
    if (@ready = $read_set->can_read(2)) {
      foreach $s (@ready) {
        $buf = <$s>;
        if ($buf =~ m/\d\s+\d/) {
          return 1;
        }
      }
    }
    $repcount++;
  }
  return 0;
}
    
sub acos {
  my($x) = @_;
  if ((1 - $x**2) <= 0) {
    return atan2(0, $x);
  }
  else {
    return atan2(sqrt(1 - $x**2), $x);
  }
}
                                                                                  
while ((! $gpsd) && ($opencount < 30)) {
  $gpsd = new IO::Socket::INET
            (PeerAddr => $host,
             PeerPort => $port,
             Proto    => 'tcp',);
  $opencount++;
  sleep 1;
}
die "Could not create socket: $!\n" unless $gpsd;
$gpsd->autoflush(1);

$opencount  = 0;

while ((! $admon) && ($opencount < 30)) {
  $admon = new IO::Socket::INET
             (PeerAddr => '127.0.0.1',
              PeerPort => '7070',
              Proto    => 'tcp',);
  $opencount++;
  sleep 1;
}
die "Could not create socket: $!\n" unless $admon;
$admon->autoflush(1);
                                               
$| = 1;

$pi = atan2(1, 1) * 4;

while (! $gps_valid) {
  do_command($gpsd, "s") =~ m/GPSD,S=([01])/;
  $gps_valid = $1;
  sleep 1;
}

do_admon($admon, "R1 1\n");
sleep 1;
do_admon($admon, "R1 0\n");

do_command($gpsd, "p") =~ m/GPSD,P=(\S+)\s+(\S+)/;
$startlat = $1 * ($pi / 180);
$startlon = $2 * ($pi / 180);

do_command($gpsd, "a") =~ m/GPSD,A=(\S+)/;
$startalt = $1 * 3.28;

while (1) {
  do_command($gpsd, "s") =~ m/GPSD,S=([01])/;
  $gps_valid = $1;
  if ($gps_valid) {
    do_command($gpsd, "p") =~ m/GPSD,P=(\S+)\s+(\S+)/;
    $lat = $1 * ($pi / 180);
    $lon = $2 * ($pi / 180);
    $distance = &acos(cos($startlat)*cos($startlon)*cos($lat)*cos($lon) + cos($startlat)*sin($startlon)*cos($lat)*sin($lon) + sin($startlat)*sin($lat)) * $radius;
    do_command($gpsd, "a") =~ m/GPSD,A=(\S+)/;
    $alt = int($1 * 3.28);
    do_command($gpsd, "v") =~ m/GPSD,V=(\S+)/;
    $speed = $1 * 1.1507771555;
    if ((! $ataltitude) && ($alt > $safety)) {
      $ataltitude = 1;
    }
    if ((($ataltitude) && ($alt < $safety)) || ($speed >= 80)) {
      if (! $relaystat) {
        do_admon($admon, "R2 1\n");
        $relaystat = 1;
      }
      do_admon($admon, "R1 1\n");
      sleep 2;
      do_admon($admon, "R1 0\n");
    }
    if ($relaystat && ($speed < 80)) {
      $relaystat = 0;
      do_admon($admon, "R2 0\n");
    }
    if (($distance > $maxdistance) && (! $didcut)) {
      do_admon($admon, "R3 1\n");
      sleep 20;    
      do_admon($admon, "R3 0\n");
      $didcut = 1;
    }
    if (open(CUTDOWN, "/tmp/cutdown") && (! $didcut)) {
      close CUTDOWN;
      system("/bin/rm /tmp/cutdown");
      do_admon($admon, "R1 1\n");
      sleep 1;
      do_admon($admon, "R1 0\n");
      do_admon($admon, "R3 1\n");
      sleep 20;
      do_admon($admon, "R3 0\n");
      $didcut = 1;
    }
  }          
  sleep $checkinterval;
}