/**
 * W/BO Project Wi-Fi over AIBO
 * Author: Gilles RÉANT - 2005/2006
 * GPLv2 licence http://www.gnu.org/licenses/gpl.html
 * see http://macneill.cs.tcd.ie/~reantg/WIBO_Project/
 * or http://www.reant.net/tcd/WIBO_Project/
 * for more informations
 **/

echo "Defining functions & variables";
global.default_noise = wlan.noise;
def max(my_list) {
  maximum = -1;
  index = -1;
  i = -1;
  foreach n in my_list {
    i++;
    if (n > maximum) {
      maximum = n;
      index = i;
    };
  };
  return index;
};
def min(my_list) {
  minimum = 99;
  index = -1;
  i = -1;
  foreach n in my_list {
    i++;
    if (n < minimum) {
      minimum = n;
      index = i;
    };
  };
  return minimum;
};
def standard_deviation(my_list, my_mean) {
  result = 0;
  foreach n in my_list {
    result = result + (n - my_mean) * (n - my_mean);
  };
  result = sqrt(result);
  return result;
};
def checknoise(waiting_time) {
  loop {
    if (wlan.noise != global.default_noise) {
      echo "Wow, the noise is original!! " + wlan.noise + " (instead of default " + global.default_noise + ")!!";
    };
    wait(waiting_time);
  };
};
def measure(number_of_measures, start_waiting_time, waiting_time) {
  ok = false;
  number_of_problems = -1;
  while (!ok) {
    number_of_problems = number_of_problems + 1;
    if (number_of_problems == 0) {
      ledF1 = 0;
      ledF2 = 0;
      ledF13 = 0;
      ledF14 = 0;
      global.ledMode = 0;
    };
    if (number_of_problems == 1) {
      ledF13 = 1;
      speaker.play("wxex.wav");
    };
    if (number_of_problems == 2) {
      ledF14 = 1;
      speaker.play("wxer.wav");
    };
    if (number_of_problems == 3) {
      global.ledMode = 1;
      speaker.play("wxcs.wav");
      echo "/!\ We had measurement problems 3 times in a row, so forward!";
      return -1;
    };
    wait(start_waiting_time);
    mean = 0;
    my_list = [];
    for (i = 0 ; i < number_of_measures ; i++) {
      current_signal = wlan.signal;
      mean = mean + current_signal;
      my_list = my_list + current_signal;
      echo "measure[" + i + "]: signal=" + current_signal + "; mean=" + mean/(i+1);
      if (i < number_of_measures - 1) {
        wait(waiting_time);
      }
    };
    mean = mean / number_of_measures;
    ecart_type = standard_deviation(my_list, mean);
    echo "for information: mean=" + mean + "  standard deviation=" + ecart_type;
    if (ecart_type > 0.1) {
      echo "The standard deviation is too high (>0.1). We are going to make another set of measurements after moving a bit.";
      ok_1 = false;
    } else {
      ok_1 = true;
    };
    if (mean < 0.05) {
      echo "The mean (" + mean + ") is inferior to 0.05. We are going to make another set of measurements after moving a bit.";
      ok_2 = false;
    } else {
      ok_2 = true;
    };
    ok = ok_1 && ok_2;
    if (!ok) {
      speaker.play("drum1.wav");
      walk.turn(30);
      walk.turn(-30);
    };
  };
  return mean;
};
def ears_movement() {
  earR->blend = queue;
  earL->blend = queue;
  loopn(5) {
    earR = 0;
    earL = 0;
    wait(0.1s);
    earR = 1;
    earL = 1;
    wait(0.1s);
  };
};
def recherche(minimum_signal_quality_required, number_of_values_remembered) {
  current_signal_quality = wlan.signal;
  while (current_signal_quality < minimum_signal_quality_required) {
    my_list = [];
    forced_forward = false;
    loopn(number_of_values_remembered) {
      if (!forced_forward) {
        variable = measure(5,2s,0.5s);
        if (variable == -1) {
          echo "forward!";
          forced_forward = true;
        };
        if (!forced_forward) {
          my_list = my_list + variable;
          walk.turn(72);
        };
      };
    };
    if (!forced_forward) {
        echo "Measurements done!";
        echo "List: ";
        echo my_list;
        minimum = min(my_list);
        echo "Minimum: " + minimum;
        variable = measure(5,2s,0.5s);
        if (variable == -1) {
          echo "forward!";
          forced_forward = true;
        };
        if (!forced_forward) {
          while (variable > minimum) {
            echo "The current value (" + variable + ") isn't inferior to the minimum (" + minimum + ").";
            new_list = [];
            i = 0;
            foreach n in my_list {
              if (i != 0) {
                new_list = new_list + n;
              };
              i++;
            };
            new_list = new_list + variable;
            my_list = new_list;
            echo "New list: ";
            echo my_list;
            minimum = min(my_list);
            echo "New minimum: " + minimum;
            walk.turn(72);
            variable = measure(5,2s,0.5s);
            if (variable == -1) {
              echo "forward!";
              forced_forward = true;
            };
          };
        echo "Cool! :D I have a value of " + variable + " inferior to the minimum " + minimum + ".";
      };
    } else {
      echo "Cool! :D I have a value of " + variable + ", so forward.";
    };
    ledF1 = 1;
    ledF2 = 1;
    speaker.play("wxpc.wav");
    walk.go(0.5);
    current_signal_quality = wlan.signal;
  };
  echo "We have reached a quality of signal of " + current_signal_quality + "!";
  echo "End of the program.";
};
def gillesdemo() {
  ledBFC = 1;
  ledBMC = 1;
  ledBRC = 1;
  echo "Gilles start - W/IBO project ; Wi-Fi over AIBO";
  echo "My name on the network: " + wlan.stationName;
  echo "I'm on channel " + wlan.channel;
  echo "WLAN noise (constant value; bad implementation?): " + wlan.noise;
  checknoise(5s),
  echo "I'm connected on the network " + wlan.ssid;
  echo " but I can't tell whether it's an ad-hoc or infrastructure network. :-(";
  motor on;
  echo "Motors on";
  echo "Getting up";
  robot.stand();
  neck = 2 & ears_movement() & tailPan = 0 speed:20;
  speaker.play("wxpc.wav");
  echo "OK, I'm ready";
  timeout (10m) recherche(0.75,5);
};
def quality_of_signal() {
  ledF = 0;
  timeout (2m) loop {
    ledF = 0;
    current_signal_quality = wlan.signal;
    echo "signal quality (0.0-1.0): " + current_signal_quality;
    ledF13 = (current_signal_quality == 0);
    ledF14 = (current_signal_quality == 0);
    ledF1 = (current_signal_quality > 0.08);
    ledF2 = (current_signal_quality > 0.16);
    ledF3 = (current_signal_quality > 0.24);
    ledF5 = (current_signal_quality > 0.32);
    ledF12 = (current_signal_quality > 0.40);
    ledF6 = (current_signal_quality > 0.48);
    ledF4 = (current_signal_quality > 0.56);
    ledF7 = (current_signal_quality > 0.64);
    ledF9 = (current_signal_quality > 0.72);
    ledF11 = (current_signal_quality > 0.80);
    ledF10 = (current_signal_quality > 0.88);
    ledF8 = (current_signal_quality > 0.96);
    speaker.play("beep.wav");
    wait(5s);
  };
};

at((backSensorF > 10) && (backSensorM > 10) && (backSensorR > 10)) {
  gillesdemo();
};