#!/usr/bin/perl -w
#
# ORTHO OF BORG main control program
#

use POSIX;
use strict;

use ssc;
use ortho_encoder;
use ortho_sound;
use ortho_ui;

#
# BEHAVIOR VARIABLES
#

# servo variables

my $DRIVE_SERVO = 0;
my $DIRECTION_SERVO = 1;
my $SERVO_DISABLE = 128;

# speed control variables

my $current_output = 128;
my $max_output = 255;
my $min_output = 0;
my $new_output;

my $old_encoder_count;
my $encoder_count;

my $MAX_SPEED = 400;

my $current_speed = 0;
my $desired_speed = 0;
my $speed_error = 0;

my $K_pro = 0.2;

my $T = 1; # samples / second, set to 1 so we simply use #samples for Td and Ti
my $Td = 0; # derivative time in seconds (samples)
my $Ti = 4; # reset time (integral time) in seconds (samples)
my $ek1 = 0.0; # error at Tk-1
my $ek2 = 0.0; # error at Tk-2

my $b0 = $K_pro + ($K_pro * ($T/$Ti)) + ($K_pro * ($Td/$T));
my $b1 = -$K_pro - (2 * $K_pro * ($Td/$T));
my $b2 = $K_pro * ($Td / $T);

# position variables

my $orig_x = 0;
my $orig_y = 0;

my $current_x = 0;
my $current_y = 0;

my $desired_x = 0;
my $desired_y = 0;

# direction control variables

my $DIR_X = 0;
my $DIR_Y = 255;
my $DIR_DISABLE = 128;

my $current_direction = $DIR_DISABLE;
my $desired_direction = $DIR_DISABLE;

my $DSTATE_IDLE = 0;
my $DSTATE_CHANGING = 1;

my $DCHANGE_TIME = 20 * 2; # 2 seconds * 20 iterations/sec

my $direction_control_state = $DSTATE_IDLE;
my $direction_change_time_left = 0;

sub min($$) {
    my $a = shift(@_);
    my $b = shift(@_);
    if ($a < $b) {
	return $a;
    }
    else {
	return $b;
    }
}

#
# BEHAVIOR ROUTINES
#

sub init_speed_control() {
    $old_encoder_count = encoder_read();
}

sub init_direction() {
    $desired_direction = $DIR_DISABLE;
    direction_control();
}

my $encoder_failures = 0;

my $IR_XPLUS = 1;
my $IR_YMINUS = 2;
my $IR_XMINUS = 3;
my $IR_YPLUS = 4;

my $ir_yplus_value = 0;
my $ir_xminus_value = 0;
my $ir_yminus_value = 0;
my $ir_xplus_value = 0;

sub read_sensors() {
    $encoder_count = encoder_read();
    if (!defined($encoder_count)) {
	$encoder_failures++; 
	print "\aTIMEOUT reading encoder: #$encoder_failures\n";
    }
    else {
	$encoder_failures = 0;
#	print "encoder_count=$encoder_count\n";
    }

#    $ir_xminus_value = ir_read($IR_XMINUS);
#    $ir_yminus_value = ir_read($IR_YMINUS);
#    $ir_xplus_value = ir_read($IR_XPLUS);
}

sub calculate_speed() {
    if (defined($encoder_count)) {
	$current_speed = $encoder_count - $old_encoder_count;
	$old_encoder_count = $encoder_count;
    }
    print "speed = $current_speed ";

}

sub update_position() {

    if (defined($encoder_count)) {
	if ($current_direction == $DIR_X) {
	    $current_x = $current_x + $current_speed;
	}
	else {
	    $current_y = $current_y + $current_speed;
	}
    }
#    print "pos = ($current_x, $current_y) ";
}

my $RAMP_MUL = 0.15;
my $RAMP_START_SPEED = $MAX_SPEED / 5;

my $max_desired_speed = 0;

sub speed_ramp() {
    if ($current_direction == $DIR_X) {
	$max_desired_speed = min(abs($desired_x - $current_x) * $RAMP_MUL, 
				 abs($orig_x - $current_x) * $RAMP_MUL
				 + $RAMP_START_SPEED);
    }
    elsif ($current_direction == $DIR_Y) {
	$max_desired_speed = min(abs($desired_y - $current_y) * $RAMP_MUL,
				 abs($orig_y - $current_y) * $RAMP_MUL
				 + $RAMP_START_SPEED);
    }

    if ($desired_speed > $max_desired_speed) {
	print "clamp speed to $max_desired_speed"; #if $current_speed > $max_desired_speed;
	$desired_speed = $max_desired_speed;
    }
    elsif ($desired_speed < -$max_desired_speed) {
	print "clamp speed to $max_desired_speed"; #if $current_speed > $max_desired_speed;
	$desired_speed = -$max_desired_speed;
    }

}

my $AVOID_THRESHOLD = 180;

sub ir_avoid() {

    if ($current_direction = $DIR_Y) {
	if ($desired_y > $current_y) {
	    if ($ir_yplus_value > $AVOID_THRESHOLD) {
		$desired_y = $current_y;
	    }
	}
	else {
	    if ($ir_yminus_value > $AVOID_THRESHOLD) {
		$desired_y = $current_y;
	    }
	}
    }
    else {
	if ($desired_x > $current_x) {
	    if ($ir_xplus_value > $AVOID_THRESHOLD) {
		$desired_x = $current_x;
	    }
	}
	else {
	    if ($ir_xminus_value > $AVOID_THRESHOLD) {
		$desired_x = $current_x;
	    }
	}
    }
}

sub speed_control() {

    if (!defined($encoder_count)) { return };

    if (($desired_speed > 0) 
	or (($desired_speed == 0) and ($current_speed > 0))) {
	$min_output = $SERVO_DISABLE;
	$max_output = $SERVO_DISABLE + ui_max_output();
    }
    elsif (($desired_speed < 0) 
	   or (($desired_speed == 0) and ($current_speed < 0))) {
	$min_output = $SERVO_DISABLE - ui_max_output();
	$max_output = $SERVO_DISABLE;
    }

    if ($desired_direction != $current_direction) {
#	print "desired_direction != current_direction, zeroing speed\n";
	$desired_speed = 0;
    };
#    if (abs($current_speed) < 100 and $desired_speed != 0) {
#	sound_play("carma/scrub2.wav");
#    }
#    elsif ($desired_speed == 0 and abs($current_speed) > 100 ) {
#	sound_play("carma/skid3.wav");
#    };

#    print "desired=$desired_speed ";

    $speed_error = int($desired_speed - $current_speed);
#    print "err=$speed_error ";

    $new_output = ($b0 * $speed_error) 
	+ ($b1 * $ek1) 
	    + ($b2 * $ek2) 
		+ $current_output;

    $new_output = $max_output if $new_output > $max_output;
    $new_output = $min_output if $new_output < $min_output;

# stop "singing"
    if ($desired_speed == 0) {
	$new_output = $SERVO_DISABLE;
    };

    if ($desired_direction != $current_direction) {
	$new_output = $SERVO_DISABLE;
    };

#    print "output=$new_output";

    ssc_set_servo($DRIVE_SERVO, $new_output);
    $current_output = $new_output;
    $ek2 = $ek1;
    $ek1 = $speed_error;
}

my $Y_TARGET_THRESHOLD = 200;
my $X_TARGET_THRESHOLD = 200;

sub at_target() {
    if ($current_direction == $DIR_Y) {
	return (abs($current_y - $desired_y) <= $Y_TARGET_THRESHOLD);
    }
    else {
	return (abs($current_x - $desired_x) <= $X_TARGET_THRESHOLD);
    }
}
	    


sub direction_control() {
    if ($direction_control_state == $DSTATE_IDLE) {
	if ($desired_speed == 0 and $current_speed == 0 and 
	    $desired_direction != $current_direction) {
	    print "changing direction to ", 
	        $desired_direction == $DIR_X ? "X\n" : "Y\n"; 
	    ssc_set_servo($DIRECTION_SERVO, $desired_direction);
	    $direction_control_state = $DSTATE_CHANGING;
	    $direction_change_time_left = $DCHANGE_TIME;
	};
    }
    elsif ($direction_control_state == $DSTATE_CHANGING) {
	if (--$direction_change_time_left <= 0) {
	    $current_direction = $desired_direction;
	    $direction_control_state = $DSTATE_IDLE;
	    print "finished direction change\n";
	}
	else {
	    ssc_set_servo($DIRECTION_SERVO, $desired_direction);
	}
    }
    else {
	print "Invalid direction control state: $direction_control_state\n";
    };
}

sub start_change_to_y() {
    $desired_direction = $DIR_Y;
    return 0;
}

sub change_to_y() {
    return $current_direction == $DIR_Y;
}

sub start_change_to_x() {
    $desired_direction = $DIR_X;
    return 0;
}

sub change_to_x() {
    return $current_direction == $DIR_X;
}

sub start_line_follow() {
    print "\nLine following not yet implemented.\a\n";
    select undef,undef,undef,1.0;
    return 1;
}

sub line_follow() {
    return 1;
}

sub dummy_behavior() {
    return 1;
}

my $start_count = 5;

my $QT_IDLE = 0;
my $QT_FWD_STARTING = 1;
my $QT_FWD = 2;
my $QT_FWD_STOPPING = 3;
my $QT_BACK = 4;
my $QT_BACK_STOPPING = 5;

my $qt_state = $QT_IDLE;
my $qt_orig_y = 0;

sub start_quick_trip() {
    $start_count = 5;
    $orig_y = $qt_orig_y = $current_y;
    $desired_y = $current_y + inches_to_encoder_counts(ui_y_inches());
    $desired_direction = $DIR_Y;
    direction_control();
    $qt_state = $QT_FWD_STARTING;
    return ir_start($IR_YPLUS);
}

sub quick_trip() {

    countdown();
    if ($start_count >= -1) { return 0; };

    if ($qt_state == $QT_FWD_STARTING) {
	if ($current_direction == $DIR_Y) {
	    $desired_speed = $MAX_SPEED;
	    $qt_state = $QT_FWD;
	}
	else {
	    $desired_direction = $DIR_Y;
	    $desired_speed = 0;
	}
    }
    if ($qt_state == $QT_FWD) {
	if (at_target()) {
	    $desired_speed = 0;
	    $qt_state = $QT_FWD_STOPPING;
	}
	else {
	    $desired_speed = $MAX_SPEED;
	}
    }
    elsif ($qt_state == $QT_FWD_STOPPING) {
	if ($current_speed == 0) {
	    sound_play("carma/chkpoint.wav");
	    $qt_state = $QT_BACK;
	    $desired_speed = -$MAX_SPEED;
	    $orig_y = $current_y;
	    $desired_y = $qt_orig_y;
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($qt_state == $QT_BACK) {
	if (at_target()) {
	    $qt_state = $QT_BACK_STOPPING;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($qt_state == $QT_BACK_STOPPING) {
	if ($current_speed == 0) {
	    sound_play("carma/racecplt.wav");
	    $qt_state = $QT_IDLE;
	}
	else {
	    $desired_speed = 0;
	}
    }

    return $qt_state == $QT_IDLE;
}

my $TT_IDLE = 0;
my $TT_FWD_STARTING = 1;
my $TT_FWD = 2;
my $TT_FWD_STOPPING = 3;
my $TT_FWD_CKPT = 4;
my $TT_BACK_HALF = 5;
my $TT_BACK_HALF_STOPPING = 6;
my $TT_LEFT_STARTING = 7;
my $TT_LEFT = 8;
my $TT_LEFT_STOPPING = 9;
my $TT_LEFT_CKPT = 10;
my $TT_RIGHT = 11;
my $TT_RIGHT_STOPPING = 12;
my $TT_BACK_HOME_STARTING = 13;
my $TT_BACK_HOME = 14;
my $TT_BACK_HOME_STOPPING = 15;
my $TT_APPLAUSE = 16;

my $tt_state = $TT_IDLE;
my $tt_fwd_dist = 0;
my $tt_left_dist = 0;

sub start_t_time($$) {
    $start_count = 5;
    $tt_fwd_dist = inches_to_encoder_counts(ui_x_inches());
    $tt_left_dist = inches_to_encoder_counts(ui_y_inches());
    $desired_direction = $DIR_X;
    direction_control();
    $tt_state = $TT_FWD_STARTING;
    return ir_start($IR_XPLUS);
}

sub t_time() {
    countdown();
    if ($start_count >= -1) { return 0; };
    if ($tt_state == $TT_IDLE) {
#	$desired_sound = "carma/diesel.wav";
	return 1;
    }
    elsif ($tt_state == $TT_FWD_STARTING) {
	if ($current_direction == $DIR_X) {
	    $orig_x = $current_x;
	    $desired_x = $current_x + $tt_fwd_dist;
	    $desired_speed = $MAX_SPEED;
	    $tt_state = $TT_FWD;
	}
	else {
	    $desired_direction = $DIR_X;
	    $desired_speed = 0;
	}
    }
    elsif ($tt_state == $TT_FWD) {
	if (at_target()) {
	    $desired_speed = 0;
	    $tt_state = $TT_FWD_STOPPING;
	}
	else {
	    $desired_speed = $MAX_SPEED;
	}
    }
    elsif ($tt_state == $TT_FWD_STOPPING) {
	if ($current_speed == 0) {
	    $tt_state = $TT_FWD_CKPT;
	    sound_play("carma/chkpoint.wav");
	}
    }
    elsif ($tt_state == $TT_FWD_CKPT) {
	    $tt_state = $TT_BACK_HALF;
	    $orig_x = $current_x;
	    $desired_x = $current_x - ($tt_fwd_dist * 0.58);
	    $desired_speed = -$MAX_SPEED;
    }
    elsif ($tt_state == $TT_BACK_HALF) {
	if (at_target()) {
	    $tt_state = $TT_BACK_HALF_STOPPING;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($tt_state == $TT_BACK_HALF_STOPPING) {
	if ($current_speed == 0) {
	    $tt_state = $TT_LEFT_STARTING;
	    $desired_direction = $DIR_Y;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($tt_state == $TT_LEFT_STARTING) {
	if ($current_direction == $DIR_Y) {
	    $tt_state = $TT_LEFT;
	    $orig_y = $current_y;
	    $desired_y = $current_y + $tt_left_dist;
	    $desired_speed = $MAX_SPEED;
	}
	else {
	    $desired_speed = 0;
	    $desired_direction = $DIR_Y;
	}
    }
    elsif ($tt_state == $TT_LEFT) {
	if (at_target()) {
	    $tt_state = $TT_LEFT_STOPPING;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = $MAX_SPEED;
	}
    }
    elsif ($tt_state == $TT_LEFT_STOPPING) {
	if ($current_speed == 0) {
	    $tt_state = $TT_LEFT_CKPT;
	    sound_play("carma/chkpoint.wav");
	}
    }
    elsif ($tt_state == $TT_LEFT_CKPT) {
	$tt_state = $TT_RIGHT;
	$orig_y = $current_y;
	$desired_y = $current_y - $tt_left_dist;
	$desired_speed = -$MAX_SPEED;
    }
    elsif ($tt_state == $TT_RIGHT) {
	if (at_target()) {
	    $tt_state = $TT_RIGHT_STOPPING;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($tt_state == $TT_RIGHT_STOPPING) {
	if ($current_speed == 0) {
	    $tt_state = $TT_BACK_HOME_STARTING;
	    $desired_direction = $DIR_X;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($tt_state == $TT_BACK_HOME_STARTING) {
	if ($current_direction == $DIR_X and $current_speed == 0) {
	    $tt_state = $TT_BACK_HOME;
	    $orig_x = $current_x;
	    $desired_x = $current_x - ($tt_fwd_dist * 0.42);
	    $desired_speed = -$MAX_SPEED;
	}
	else {
	    $desired_direction = $DIR_X;
	    $desired_speed = 0;
	}
    }
    elsif ($tt_state == $TT_BACK_HOME) {
	if (at_target()) {
	    $tt_state = $TT_BACK_HOME_STOPPING;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($tt_state == $TT_BACK_HOME_STOPPING) {
	if ($current_speed == 0) {
	    $tt_state = $TT_APPLAUSE;
	    sound_play("carma/racecplt.wav");
	    sound_control();
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($tt_state == $TT_APPLAUSE) {
	$tt_state = $TT_IDLE;
	sound_play("carma/theclap.wav");
	return 1;
    }
    return 0;
}

my @countdown_sounds = ( "carma/go!.wav", "carma/one.wav", "carma/two.wav", "carma/three.wav", "carma/four.wav", "carma/five.wav");

sub countdown() {
    if (sound_idle() and $start_count >= 0) {
#	print "countdown: playing sound $start_count\n";
	sound_play($countdown_sounds[$start_count--]);
    }
    else {
#	print "sound not idle: ", sound_idle(), " $start_count\n";
    }

    if ($start_count == -1) {
	if (sound_idle() and length(sound_queued()) == 0) {
	    $start_count--;
	}
    }
}

my $SQ_IDLE = 0;
my $SQ_FWD_STARTING = 1;
my $SQ_FWD = 2;
my $SQ_FWD_STOPPING = 3;
my $SQ_LEFT_STARTING = 4;
my $SQ_LEFT = 5;
my $SQ_LEFT_STOPPING = 6;
my $SQ_BACK_STARTING = 7;
my $SQ_BACK = 8;
my $SQ_BACK_STOPPING = 9;
my $SQ_RIGHT_STARTING = 10;
my $SQ_RIGHT = 11;
my $SQ_RIGHT_STOPPING = 12;

my $sq_state = $SQ_IDLE;
my $sq_y_length = 0;
my $sq_x_length = 0;
my $sq_orig_x = 0;
my $sq_orig_y = 0;

sub start_square() {
    $sq_y_length = inches_to_encoder_counts(ui_y_inches());
    $sq_x_length = inches_to_encoder_counts(ui_x_inches());
    $sq_state = $SQ_FWD_STARTING;
    $sq_orig_x = $orig_x = $current_x;
    $sq_orig_y = $orig_y = $current_y;
    $desired_y = $sq_orig_y + $sq_y_length;
}

sub square() {
#    print "sq_state = $sq_state\n";

    if ($sq_state == $SQ_FWD_STARTING) {
	if ($current_direction == $DIR_Y) {
	    $sq_state = $SQ_FWD;
	    $desired_speed = $MAX_SPEED;
	}
	else {
	    $desired_direction = $DIR_Y;
	    $desired_speed = 0;
	}
    }
    elsif ($sq_state == $SQ_FWD) {
	if (at_target()) {
	    $desired_speed = 0;
	    $sq_state = $SQ_FWD_STOPPING;
	}
	else {
	    $desired_speed = $MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_FWD_STOPPING) {
	if ($current_speed == 0) {
	    print "overshot FWD by ", $current_y - $desired_y;
	    $sq_state = $SQ_LEFT_STARTING;
	    $desired_direction = $DIR_X;
	    $orig_x = $current_x;
	    $desired_x = $sq_orig_x - $sq_x_length;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($sq_state == $SQ_LEFT_STARTING) {
	if ($current_direction == $DIR_X) {
	    $sq_state = $SQ_LEFT;
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_LEFT) {
	if (at_target()) {
	    $desired_speed = 0;
	    $sq_state = $SQ_LEFT_STOPPING;
	}
	else {
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_LEFT_STOPPING) {
	if ($current_speed == 0) {
	    print "overshot LEFT by ", $desired_x - $current_x;
	    $sq_state = $SQ_BACK_STARTING;
	    $desired_direction = $DIR_Y;
	    $orig_y = $current_y;
	    $desired_y = $sq_orig_y;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($sq_state == $SQ_BACK_STARTING) {
	if ($current_direction == $DIR_Y) {
	    $sq_state = $SQ_BACK;
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_BACK) {
	if (at_target()) {
	    $sq_state = $SQ_BACK_STOPPING;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = -$MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_BACK_STOPPING) {
	if ($current_speed == 0) {
	    print "overshot BACK by ", $desired_y - $current_y;
	    $sq_state = $SQ_RIGHT_STARTING;
	    $desired_direction = $DIR_X;
	    $orig_x = $current_x;
	    $desired_x = $sq_orig_x;
	    $desired_speed = 0;
	}
	else {
	    $desired_speed = 0;
	}
    }
    elsif ($sq_state == $SQ_RIGHT_STARTING) {
	if ($current_direction == $DIR_X) {
	    $sq_state = $SQ_RIGHT;
	    $desired_speed = $MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_RIGHT) {
	if (at_target()) {
	    $desired_speed = 0;
	    $sq_state = $SQ_RIGHT_STOPPING;
	}
	else {
	    $desired_speed = $MAX_SPEED;
	}
    }
    elsif ($sq_state == $SQ_RIGHT_STOPPING) {
	if ($current_speed == 0) {
	    print "overshot RIGHT by ", $current_x - $desired_x;
	    if (ui_repeat_mode()) {
		$sq_state = $SQ_FWD_STARTING;
		$desired_direction = $DIR_Y;
		$orig_y = $current_y;
		$desired_y = $sq_orig_y + $sq_y_length;
		$desired_speed = 0;
	    }
	    else {
		$sq_state = $SQ_IDLE;
		$desired_speed = 0;
	    }
	}
	else {
	    $desired_speed = 0;
	}
    }

    return $sq_state == $SQ_IDLE;
}

sub start_jog_minus() {
    if ($current_direction == $DIR_Y) {
	$orig_y = $current_y;
	$desired_y = $current_y - inches_to_encoder_counts(ui_y_inches());
    }
    else {
	$orig_x = $current_x;
	$desired_x = $current_x - inches_to_encoder_counts(ui_x_inches());
    }
    $desired_speed = -$MAX_SPEED;
}

sub start_jog_plus() {
    if ($current_direction == $DIR_Y) {
	$orig_y = $current_y;
	$desired_y = $current_y + inches_to_encoder_counts(ui_y_inches());
    }
    else {
	$orig_x = $current_x;
	$desired_x = $current_x + inches_to_encoder_counts(ui_x_inches());
    }
    $desired_speed = $MAX_SPEED;
}

sub jog_minus() {
    if ($current_direction == $DIR_Y) {
	if (at_target()) {
	    $desired_speed = 0;
	    return 1;
	}
    }
    else {
	if (at_target()) {
	    $desired_speed = 0;
	    return 1;
	}
    }
    $desired_speed = -$MAX_SPEED;
    return 0;
}

sub jog_plus() {
    if ($current_direction == $DIR_Y) {
	if (at_target()) {
	    $desired_speed = 0;
	    return 1;
	}
    }
    else {
	if (at_target()) {
	    $desired_speed = 0;
	    return 1;
	}
    }
    $desired_speed = $MAX_SPEED;
    return 0;
}

my $mode;

my %mode_start_routines = (
			   l => \&start_line_follow,
			   q => \&start_quick_trip,
			   t => \&start_t_time,
			   c => \&start_can_can,
			   s => \&start_square,
			   o => \&ui_set_max_output,
			   r => \&ui_toggle_repeat_mode,
			   x => \&ui_set_x,
			   y => \&ui_set_y,
			   '-' => \&start_change_to_y,
			   '+' => \&start_change_to_x,
			   '4' => \&start_jog_minus,
			   '6' => \&start_jog_plus,
			   );

my %mode_behaviors = (
		      l => \&line_follow,
		      q => \&quick_trip,
		      t => \&t_time,
		      c => \&can_can,
		      s => \&square,
		      o => \&dummy_behavior,
		      r => \&dummy_behavior,
		      x => \&dummy_behavior,
		      y => \&dummy_behavior,
		      '-' => \&change_to_y,
		      '+' => \&change_to_x,
		      '4' => \&jog_minus,
		      '6' => \&jog_plus,
		      );

sub mode_start() {
    if ($mode_start_routines{$mode}) {
	return $mode_start_routines{$mode}->();
    }
    else {
	print "unknown mode: $mode\n";
	return 1;
    }
}

sub mode_based_behavior() {
    if ($mode_behaviors{$mode}) {
	return $mode_behaviors{$mode}->();
    }
    else {
	print "unknown mode: $mode\n";
	return 1;
    }
}

sub control_loop() {

    my $done;
    my $rin = '';
    my $rout;
    my $keypress = 0;

    vec($rin, fileno(STDIN), 1) = 1;

    #
    # wait 30 milliseconds, or until keypress; if keypress, STOP!
    #
    until ($done or ($keypress = select($rout=$rin, undef, undef, 0.01) == 1)) {

	#
	# input phase
	#
	read_sensors();
	calculate_speed();
	update_position();

	#
	# decision phase
	#
	$done = mode_based_behavior();

	#
	# output phase
	#
#	ir_avoid();
	speed_ramp();
	speed_control();
	direction_control();
	sound_control();

	if ($encoder_failures > 5) { $done = 1; }
	print ".\n";
    }

#
# make sure we're stopped
#
    $desired_speed = 0;
    ssc_set_servo($DRIVE_SERVO, $SERVO_DISABLE);
    if ($keypress) {
	ssc_set_servo($DIRECTION_SERVO, $SERVO_DISABLE);
	getc();
	print "\a\a\aOPERATION ABORTED!  "
    }
}

my $IR_DETECT_THRESHOLD = 140; 

sub ir_start($) {

    my $sensor_number = shift(@_);
    my $reading = 0;
    my $c;
    my $rin = '';
    my $rout;
    my $keypress = 0;

    vec($rin, fileno(STDIN), 1) = 1;

    print "Awaiting flag start (sensor $sensor_number)\n";
    print "Wave flag or press G to Go, any other key to Abort:";

    do {
	sound_play("carma/diesel.wav");
	sound_control();
	$reading = ir_read($sensor_number);
    } until ($reading >= $IR_DETECT_THRESHOLD 
	     or ($keypress = select($rout=$rin, undef, undef, 0.0) == 1));

    if ($keypress) { 
	$c = getc();
	return ($c !~ /[gG]/); 
    }
    else {
	print "\n\aFlag Detected!\n";
	sound_play("");
	return 0;
    }
}

#
# MAIN PROGRAM
#
ssc_open();
encoder_open();
sound_init();
ui_open();

init_speed_control();
init_direction();

#start_quick_trip_1(10);
#start_square(24);

#my $f = shift(@ARGV);
#my $l = shift(@ARGV);

#start_ttime($f, $l);

while (($mode = ui_get_menu_choice()) !~ /e/) {
    if (mode_start()) {
	print "\n\aStart aborted\a\n";
    }
    else {
	control_loop();
    };
}

ui_close();
print "\nOrtho is off-line.\n";