#!/usr/bin/perl
# -*- perl -*-

$zsafe = 0.3;
$zup = 0.1;
$zdown = -0.15;

$fastdownfeed = 20;
$downfeed = 20;
$upfeed = 20;
$fastupfeed = 20;

$infile = shift;
$outfile = shift;

open IN, $infile;

sub end_file {
    if ($old_file) {
	print "%\n";
	close OUT;
    }
}

sub start_file {
    my ($diam) = @_;
    end_file ();
    $rad = $diam / 2;
    $diam =~ s/\./_/;
    $old_file = 1;
    open OUT, ">$outfile.$diam.ngc";
    select OUT;
    print "%\n";
    print "G17 G20 G40 G49\n";
    print "G54 G80 G90 G94\n";
    print "F10\n";
    print "G10 L1 T1 P1 R$rad X0 Y0 Z0\n";
    print "#100 = $fastdownfeed\n";
    print "#101 = $downfeed\n";
    print "#102 = $upfeed\n";
    print "#103 = $fastupfeed\n";
    print "F#101\n";
    print "G1 Z$zsafe F#102\n";
    print "G98\n";
}

$tool = 1;

while (<IN>) {
    s/[\r\n]+$//g;
    if (/^T(.*)C(.*)/) {
	$diam{$1} = $2;
    }

    if (/^T([0-9]*)$/) {
	$diam = $diam{$1};
    }

    if (/^X([0-9]*)Y([0-9]*)/) {
	$x = $1;
	$y = $2;
	$i = 0 + $hits{$diam}{count};
	$hits{$diam}{x}[$i] = $x / 10000;
	$hits{$diam}{y}[$i] = $y / 10000;
	$hits{$diam}{count} = $i + 1;
    }
}

$odiam = -1;


sub getxy {
    my ($i) = @_;
    return ($hits{$diam}{x}[$i], $hits{$diam}{y}[$i]);
}

sub swap {
    my ($i,$j) = @_;
    my ($tmp) = $hits{$diam}{x}[$i];
    $hits{$diam}{x}[$i] = $hits{$diam}{x}[$j];
    $hits{$diam}{x}[$j] = $tmp;
    $tmp = $hits{$diam}{y}[$i];
    $hits{$diam}{y}[$i] = $hits{$diam}{y}[$j];
    $hits{$diam}{y}[$j] = $tmp;
}

sub dist {
    my ($i, $j) = @_;
    my ($xi, $yi, $xj, $yj, $dx, $dy);
    ($xi, $yi) = &getxy ($i);
    ($xj, $yj) = &getxy ($j);
    $dx = $xi - $xj;
    $dy = $yi - $yj;
    return sqrt ($dx*$dx + $dy*$dy);
}

sub distxy {
    my ($xi, $yi, $j) = @_;
    my ($xj, $yj, $rv, $dx, $dy);
    ($xj, $yj) = &getxy ($j);
    $dx = $xi - $xj;
    $dy = $yi - $yj;
    return sqrt ($dx*$dx + $dy*$dy);
}


for $diam (sort keys %hits) {
    &start_file ($diam);

    @seen = ();


    $ox = 0;
    $oy = 0;
    for ($h=0; $h<$hits{$diam}{count}-1; $h++) {
	$best = 999999;
	$besti = -1;
	for ($i=$h; $i<$hits{$diam}{count}; $i++) {
	    # drill
	    $dist = &distxy ($ox, $oy, $i);
	    if ($dist < $best) {
		$best = $dist;
		$besti = $i;
	    }
	}
	print STDERR "swapping $besti and $h\n";
	swap ($besti, $h);
	($ox, $oy) = &getxy($h);
	printf STDERR "x,y now %f,%f\n", $ox, $oy;
    }

    $did_something = 1;
    while ($did_something) {
	$did_something = 0;
	for ($i=1; $i<$hits{$diam}{count}-2; $i++) {

	    if (&dist($i-1, $i) + &dist($i, $i+1) + &dist($i+1, $i+2)
		> &dist($i-1, $i+1) + &dist($i+1, $i) + &dist($i, $i+2)) {
		print STDERR "swapping at $i\n";
		&swap ($i,$i+1);
		$did_something = 1;
	    }
	}
    }

    for ($i=0; $i<$hits{$diam}{count}; $i++) {
	$x = $hits{$diam}{x}[$i];
	$y = $hits{$diam}{y}[$i];

	print "G81 X$x Y$y Z$zdown R$zup\n";
#	print "G0 X$x Y$y\n";
#	print "G1 Z$zup F#100\n";
#	print "G1 Z$zdown F#101\n";
#	print "G1 Z$zup F#102\n";
#	print "G1 Z$zsafe F103\n";
    }
}
print "%\n";
end_file ();