#!/usr/bin/perl

# /* LICENSE:
#   =========================================================================
#     CMPack'03 Source Code Release for OPEN-R SDK v1.0
#     Copyright (C) 2003 Multirobot Lab [Project Head: Manuela Veloso]
#     School of Computer Science, Carnegie Mellon University
#     All rights reserved.
#   ========================================================================= */

use Getopt::Long;

$usage = "usage: setitup
options:
-h|--help                   get help
";

sub present_menu {
  my @menu_items = @_;
  my $valid_choice = 0;
  my $choice;
  my @valid_choices;
  my $choice_params;

  foreach $menu_item (@menu_items) {
    my $first_part;
    my $mi_choice; # menu item choice
    my @mi_params;
    
    ($first_part) = split(/\)/, $menu_item);
    ($mi_choice, @mi_params) = split(/ /,$first_part);
    $mi_choice =~ s/ //g;
    push @valid_choices,$mi_choice;
    push @choice_params,join(' ',@mi_params);
  }

  while(!$valid_choice) {
    print "please enter command from the following options\n";
    foreach $menu_item (@menu_items) {
      print "$menu_item\n";
    }

    my $response,$res_choice,$res_params;
    my $res_choice_end;
    $response = <STDIN>;
    #print "response='$response'\n";
    chomp($response);
    #print "response='$response'\n";
    $res_choice_end = index($response," ");
    if($res_choice_end == -1) {
      $res_choice = $response;
      $res_params = "";
    } else {
      $res_choice = substr $response,0,$res_choice_end;
      $res_params = substr $response,$res_choice_end+1;
    }
    #print "rce=$res_choice_end res_choice='$res_choice' parms='$res_params'\n";

    foreach $valid_choice_item (@valid_choices) {
      if($valid_choice_item eq $res_choice) {
        $valid_choice = 1;
        $choice = $res_choice;
        $choice_params = $res_params;
      }
    }
  }

  #print "choice was '$choice'\n";
  
  return ($choice, $choice_params);
}

sub do_menu {
  my @menu_p_actions = @_;

  my @menu,@actions;
  @menu    = ();
  @actions = ();

  for(my $i=0; $i<=$#menu_p_actions; $i++) {
    if($i % 2 == 0) {
      push @menu,$menu_p_actions[$i];
    } else {
      push @actions,$menu_p_actions[$i];
    }
  }

  my @valid_choices;
  foreach $menu_item (@menu) {
    my $first_part;
    my $mi_choice; # menu item choice
    
    ($first_part) = split(/\)/, $menu_item);
    ($mi_choice) = split(/ /,$first_part);
    $mi_choice =~ s/ //g;
    push @valid_choices,$mi_choice;
  }

  #print join(":",@menu),"\n";
  #print join(":",@actions),"\n";

  my $choice,$params;
  ($choice,$params)=&present_menu(@menu);

  my $i;
  for($i=0; $i<=$#menu; $i++) {
    if($valid_choices[$i] eq $choice) {
      last;
    }
  }
  $i <= $#menu || die "invalid choice got through\n";

  my @param_list=split(/ /,$params);
  &{$actions[$i]}(@param_list);
}

sub print_usage {
  print $usage;
  my $line_used=0;
  foreach $behavior (@behaviors) {
    $line_used+=length($behavior)+4;
    if($line_used >= 80) {
      print "\n";
      $line_used = length($behavior)+4;
    }
    print "'$behavior', ";
  }
  print "\n";
}

my $help=0;

my %opts=
  (
   'help|h' => \$help,
  );

Getopt::Long::Configure("bundling");

GetOptions(%opts) || (&print_usage() && die);

if($help) {
  &print_usage();
  exit(0);
}

$changed=0;
$jersey        ="red";
$goal_to_defend="yellow";
$thresholds    ="compA";
$camera_gain   ="compA";
$role          ="attack";
$side          ="pos";

sub print_cur_params() {
  print "\n";
  print "changed       :$changed\n";
  print "jersey        :$jersey\n";
  print "goal_to_defend:$goal_to_defend\n";
  print "thresholds    :$thresholds\n";
  print "camera_gain   :$camera_gain\n";
  print "role          :$role\n";
  print "side          :$side\n";
  print "\n";
}

sub write_behave_cfg() {
  my @behave_cfg_lines=();

  open(BEHAVE_CFG,"</memstick/config/behave.cfg");
  while($behave_cfg_line=<BEHAVE_CFG>) {
    push @behave_cfg_lines,$behave_cfg_line;
  }
  close(BEHAVE_CFG);

  for(my $i=0; $i<=$#behave_cfg_lines; $i++) {
    $behave_cfg_lines[$i] =~ /^(.*teamColor\s*=\s*")[^"]*(".*)$/ && do { # " keep emacs semi-happy
      $behave_cfg_lines[$i] = "$1$jersey$2\n";
    };
    $behave_cfg_lines[$i] =~ /^(.*defendGoal\s*=\s*")[^"]*(".*)$/ && do { # " keep emacs semi-happy
      $behave_cfg_lines[$i] = "$1$goal_to_defend$2\n";
    };
    $behave_cfg_lines[$i] =~ /^(.*role\s*=\s*")[^"]*(".*)$/ && do { # " keep emacs semi-happy
      $behave_cfg_lines[$i] = "$1$role$2\n";
    };
    $behave_cfg_lines[$i] =~ /^(.*fieldSide\s*=\s*")[^"]*(".*)$/ && do { # " keep emacs semi-happy
      $behave_cfg_lines[$i] = "$1$side$2\n";
    };
  }

  open(BEHAVE_CFG,">/memstick/config/behave.cfg");
  for(my $i=0; $i<=$#behave_cfg_lines; $i++) {
    print BEHAVE_CFG $behave_cfg_lines[$i];
  }
  close(BEHAVE_CFG);
}

sub write_thresholds() {
  if($thresholds eq "compA") {
    system("cp /usr0/slenser/projects/dogs/dog_images_2001/seattle/conf/compA/08-03/thresh.tm /memstick/config/thresh.tm");
  } elsif($thresholds eq "practice") {
    system("cp /usr0/slenser/projects/dogs/dog_images_2001/seattle/conf/practice/08-02/thresh.tm /memstick/config/thresh.tm");
  } elsif($thresholds eq "lab") {
    system("cp /usr0/slenser/projects/dogs/dog_images_2001/lab/09-04/thresh.tm /memstick/config/thresh.tm");
  } elsif($thresholds eq "unknown") {
    print "thresholds unknown so leaving untouched\n";
  } else {
    print "ATTEMPT TO WRITE INVALID THRESHOLDS '$thresholds'\n";
  }
}

sub write_camera_gain() {
  if($camera_gain eq "compA") {
    system("cp /usr0/slenser/projects/dogs/code01r/agent/config/camera.cfg /memstick/config/camera.cfg");
  } elsif($camera_gain eq "practice") {
    system("cp /usr0/slenser/projects/dogs/code01r/agent/config/camera_practice.cfg /memstick/config/camera.cfg");
  } elsif($camera_gain eq "lab") {
    system("cp /usr0/slenser/projects/dogs/code01/agent/config/camera.cfg /memstick/config/camera.cfg");
  } elsif($camera_gain eq "unknown") {
    print "camera gain unknown so leaving untouched\n";
  } else {
    print "ATTEMPT TO WRITE INVALID CAMERA GAINS '$camera_gain'\n";
  }
}

sub write_cur_params() {
  print "WRITING THIS CONFIGURATION TO MEMORY STICK!\n";
  &print_cur_params();

  &write_behave_cfg();
  &write_thresholds();
  &write_camera_gain();

  $changed = 0;
}

sub read_behave_cfg() {
  my @behave_cfg_lines=();

  open(BEHAVE_CFG,"</memstick/config/behave.cfg");
  while($behave_cfg_line=<BEHAVE_CFG>) {
    push @behave_cfg_lines,$behave_cfg_line;
  }
  close(BEHAVE_CFG);

  for(my $i=0; $i<=$#behave_cfg_lines; $i++) {
    $behave_cfg_lines[$i] =~ /^(.*teamColor\s*=\s*")([^"]*)(".*)$/ && do { # " keep emacs semi-happy
      $jersey = $2;
    };
    $behave_cfg_lines[$i] =~ /^(.*defendGoal\s*=\s*")([^"]*)(".*)$/ && do { # " keep emacs semi-happy
      $goal_to_defend = $2;
    };
    $behave_cfg_lines[$i] =~ /^(.*role\s*=\s*")([^"]*)(".*)$/ && do { # " keep emacs semi-happy
      $role = $2;
    };
    $behave_cfg_lines[$i] =~ /^(.*fieldSide\s*=\s*")([^"]*)(".*)$/ && do { # " keep emacs semi-happy
      $side = $2;
    };
  }
}

sub read_thresholds() {
  my $ret_val;

  $thresholds = "unknown";

  $ret_val = system("diff -q /usr0/slenser/projects/dogs/dog_images_2001/seattle/conf/compA/08-03/thresh.tm /memstick/config/thresh.tm >/dev/null");
  $ret_val /= 256;
  if($ret_val == 0) {
    $thresholds = "compA";
  }

  $ret_val = system("diff -q /usr0/slenser/projects/dogs/dog_images_2001/seattle/conf/practice/08-02/thresh.tm /memstick/config/thresh.tm >/dev/null");
  $ret_val /= 256;
  if($ret_val == 0) {
    $thresholds = "practice";
  }

  $ret_val = system("diff -q /usr0/slenser/projects/dogs/dog_images_2001/lab/09-04/thresh.tm /memstick/config/thresh.tm >/dev/null");
  $ret_val /= 256;
  if($ret_val == 0) {
    $thresholds = "lab";
  }
}

sub read_camera_gain() {
  my $camera_cfg_line;
  my $camera_compA=1;
  my $camera_practice=1;
  my $camera_lab=1;

  open(CAMERA_CFG,"</memstick/config/camera.cfg");
  while($camera_cfg_line=<CAMERA_CFG>) {
    print "camcfg $camera_cfg_line";
    $camera_cfg_line =~ /^(.*whiteBalance\s*=\s*)([a-zA-Z]*)/ && do {
      my $wb=$2;
      print "wb '$wb'\n";
      $camera_lab=0      if($wb ne "indoor"     );
      $camera_compA=0    if($wb ne "flourescent");
      $camera_practice=0 if($wb ne "flourescent");
    };
    $camera_cfg_line =~ /^(.*gain\s*=\s*)([a-zA-Z]*)/ && do {
      my $gain=$2;
      print "gain '$gain'\n";
      $camera_lab=0      if($gain ne "mid");
      $camera_compA=0    if($gain ne "low");
      $camera_practice=0 if($gain ne "low");
    };
    $camera_cfg_line =~ /^(.*shutterSpeed\s*=\s*)([a-zA-Z]*)/ && do {
      my $shutter=$2;
      print "shutter '$shutter'\n";
      $camera_lab=0      if($shutter ne "mid");
      $camera_compA=0    if($shutter ne "fast");
      $camera_practice=0 if($shutter ne "mid" );
    };
    print "compA $camera_compA practice $camera_practice\n";
  }
  close(CAMERA_CFG);

  if($camera_practice) {
    $camera_gain="practice";
  } elsif($camera_compA) {
    $camera_gain="compA";
  } elsif($camera_lab) {
    $camera_gain="lab";
  } else {
    $camera_gain="unknown";
  }
}

sub read_cur_params() {
  &read_behave_cfg();
  &read_thresholds();
  &read_camera_gain();
}

sub set_jersey() {
  my $param_str = shift @_;
  my @params = split(/ /,$param_str);

  if($params[0] eq "red") {
    $jersey = "red";
    $goal_to_defend = "yellow";
    $changed = 1;
  } elsif($params[0] eq "blue") {
    $jersey = "blue";
    $goal_to_defend = "cyan";
    $changed = 1;
  } else {
    print "invalid jersey color '$params[0]'\n";
  }
}

sub set_thresholds() {
  my $param_str = shift @_;
  my @params = split(/ /,$param_str);

  if($params[0] eq "compA") {
    $thresholds  = "compA";
    $camera_gain = "compA";
    $changed = 1;
  } elsif($params[0] eq "practice") {
    $thresholds  = "practice";
    $camera_gain = "practice";
    $changed = 1;
  } elsif($params[0] eq "lab") {
    $thresholds  = "lab";
    $camera_gain = "lab";
    $changed = 1;
  } else {
    print "invalid thresholds '$params[0]'\n";
  }
}

sub set_role() {
  my $param_str = shift @_;
  my @params = split(/ /,$param_str);

  if($params[0] eq "attack") {
    $role = "attack";
    $changed = 1;
  } elsif($params[0] eq "goalie") {
    $role = "goalie";
    $changed = 1;
  } else {
    print "invalid role '$params[0]'\n";
  }
}

sub set_side() {
  my $param_str = shift @_;
  my @params = split(/ /,$param_str);

  if($params[0] eq "pos") {
    $side = "pos";
    $changed = 1;
  } elsif($params[0] eq "neg") {
    $side = "neg";
    $changed = 1;
  } else {
    print "invalid role '$params[0]'\n";
  }
}

sub quit {
  my $override = 0;

  if($changed) {
    print "*************************************************\n";
    print "* Warning: you have not saved these settings!!! *\n";
    print "*************************************************\n";

    print "proceed anyway (y/n)\n";
    my $val;
    do {
      $val=<STDIN>;
      chomp($val);
    } while($val !~ /^y/ && $val !~ /^n/);
    if($val =~ /^y/) {
      $override=1;
    }
  }

  if(!$changed ||
     ($changed && $override)) {
    exit(0);
  }
}

@main_menu =
  ( "j <color> ) set jersey color <red/blue>", 
    "set_jersey",
    "t <loc>   ) set thresholds <compA/practice>",
    "set_thresholds",
    "r <role>  ) set role <attack/goalie>",
    "set_role",
    "s <side>  ) set side <pos/neg>",
    "set_side",
    "m         ) grab configuration from stick",
    "read_cur_params",
    "w         ) write configuration to stick",
    "write_cur_params",
    "q         ) quit/exit",
    "quit",
    "x         ) quit/exit",
    "quit"
  );

$cur_menu = \@main_menu;

&read_cur_params();
while(1) {
  &print_cur_params();
  &do_menu(@{$cur_menu});
}
