#!/usr/bin/perl

use strict;
use Device::SerialPort;

my $noat = 0;
if ($ARGV[0] eq "-x")
{
	$noat = 1;
	shift;
}

my $dev = (glob("/dev/tty.usb*"))[0] || die "no device!\n";
if ($ARGV[0] =~ /tty/)
{
	$dev = shift;
}

my $cmd = shift || "ATI0";

print STDERR "Using $dev\n";
my $port = new Device::SerialPort($dev) || die "$! $@";
$port->baudrate(57600);
$port->databits(8);
$port->parity("none");
$port->stopbits(1);
$port->stty_echo(0);

# mavlink heartbeat packet
my $heartbeat = pack("CCCLCC", 2, 1, 4, 0, 4, 1);
my $mavlink = pack("", 0xFE, 9, int(rand(2**8)), 0, 0, 0, $heartbeat, 0);
my $c;

if ($noat)
{
	$|++;
	my $inc = 0;
	while (1)
	{
		print "\r$inc";
		$port->write("$inc\r\n");
		$inc++;
		my $c = $port->lookfor();
		print "\r$c\n" if length($c);
		#$port->write("testtesttest\r\n");
		#sleep(1);
		select(undef, undef, undef, 0.1);
	}
}
#$c = get("ATI1", '\d\.\d');
#print "! $c\n";

if (!$c)
{
	unless ($noat)
	{
		sleep(1);
$SIG{INT} = sub
{
	$port->write("\r\nATZ\r\n");
	$port->write("\r\nATZ\r\n");
	print "\nATZ\n";
	exit;
};

		$c = get("+++", "OK", 2);
		print "! $c\n";
	}

while (1) { 
	$c = get($cmd);
	print "! $c\n";
	}

	$c = get("ATI1", '\d\.\d');
	print "! $c\n";
}

$c = get("ATI5", '\d\.\d');
print "! $c\n";


sub get
{
	my ($out, $back, $timeout) = @_;
	$timeout ||= 5;

	my $time = time();
	$port->write($out eq "+++" ? "+++" : "\r\n$out\r\n");
	msleep(200);
	my $c = "";
	while (1)
	{
		$c .= $port->lookfor();
		$c =~ s/\r/\n/g;
		if (
			(length($back) && $c =~ /\Q$out.*$back/s) ||
		  (!length($back) && $c =~ /$out/) ||
		  #(!length($back) && length($c)) ||
		  ($timeout && time() - $time >= $timeout)
		)
		{
		print length($back) . "\n";
			$c =~ s/^(?:\+\+\+)//;
			$c =~ s/^\Q$out//;
			msleep(200);
			return $c;
		}

		msleep(100);
	}

	print STDERR "weird: $c\n";
	$@ = $c;
	return;
}

sub msleep
{
	select(undef, undef, undef, $_[0] / 1000);
}