#************************************************************************
# avoid.m - Copyright (C) 2010 - Paul Osmialowski
#   This program is free software; you can redistribute it and/or modify
#   it under the terms of the GNU General Public License as published by
#   the Free Software Foundation; either version 2, or (at your option)
#   any later version.
#
#   This program is distributed in the hope that it will be useful,
#   but WITHOUT ANY WARRANTY; without even the implied warranty of
#   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#   GNU General Public License for more details.
#***********************************************************************

r.robot = client_create("127.0.0.1", 6665);
r.index = 0;
ranger = proxy_create("ranger", "r", r);
r.index = 0;
position = proxy_create("position", "a", r);
position_set_motor_state(1, position);

epsilon = 0.000001;
dtor = pi / 180.0;

yaw = ranger_yaw(ranger);
pitch = ranger_pitch(ranger);
roll = ranger_roll(ranger);
if ((length(yaw) != 1) || (length(pitch) != 1) || (length(roll) != 1))
  poses = yaw / dtor;
  left = (poses >= 0.0) .* (poses < 55.0) .* (abs(pitch / dtor) < epsilon);
  right = (poses > -55.0) .* (poses <= 0.0) .* (abs(pitch / dtor) < epsilon);
  both = (poses > -55.0) .* (poses < 55.0) .* (abs(pitch / dtor) < epsilon);
else
  if ((abs(yaw(1)) > epsilon) || (abs(pitch(1)) > epsilon) || (abs(roll(1)) > epsilon))
    error("incompatible ranger pose\n");
  end
  poses = ranger_min_angle(ranger):ranger_angular_res(ranger):ranger_max_angle(ranger);
  poses /= dtor;
  left = (poses >= 0.0) .* (poses < 55.0);
  right = (poses > -55.0) .* (poses <= 0.0);
  both = (poses > -55.0) .* (poses < 55.0);
end

masklen = length(left);
if (length(right) != masklen)
  error("internal error: something went wrong\n");
end
if (length(both) != masklen)
  error("internal error: something went wrong\n");
end

turn_time = time();
while (1)
  do
    if (client_read(r))
      error("client_read returned an error!\n");
    end
    scans = ranger_val(ranger);
    scanlen = length(scans);
  until (scanlen > 0)
  if (scanlen != masklen)
    error("invalid number of scans!\n");
  end
  left_scans = scans .* left;
  right_scans = scans .* right;
  both_scans = scans .* both;
  maxdist = max(both_scans);
  if (maxdist > 0.7)
    maxdist = 0.7;
  end
  speed = 0.0;
  turn = 0.0;
  if (any((left_scans > epsilon) .* (left_scans < maxdist)))
    speed = 0.0;
    turn = -1.0;
  elseif (any((right_scans > epsilon) .* (right_scans < maxdist)))
    if (abs(time() - turn_time) > 1)
      speed = 0.0;
      turn = -1.0;
    else
      speed = 0.0;
      turn = 1.0;
    end
  else
    speed = maxdist;
    turn = 0.0;
  end
  if (abs(turn) < epsilon)
    turn_time = time();
  end
  position_setspeed(speed, 0.0, turn, position);
  usleep(1000);
end
position_setspeed(0.0, 0.0, 0.0, position);
position_set_motor_state(0, position);
client_destroy(r);