//
// robottest.cc
// February 23, 2001
// Copyright (C) 2001 by Wesley H. Huang.  All rights reserved.
//
// This is a simple program that tests out the robot class and the
// simulated laser rangefinder sensor.

#include <stdio.h>
#include "robot.h"

int main()
{
  Robot r("prob1.txt");
  
  const int nc = 3;
  float commands[nc][2] = {
    { 10, 90},
    { sqrt(3.0)*10, 150},
    { 20, -45}};

  PointC2 p;
  float a;
  r.getRealPos(&p, &a);

  printf("Starting at (%f, %f) at heading %f\n",p.x(), p.y(), a);

  for (float k=-90; k <= 90; k+=15) {
    float d, wx, wy, simD;
    r.laserModel(p, a, k, &d, &wx, &wy);
    simD = r.laser(k);
    printf("A laser reading at heading %.0f degrees is %f (%f)\n",
	   k, d, simD);
    printf("  wall slope is dx=%f, dy=%f).\n",wx, wy); 
  }
  return 0;

  for (int k=0; k<nc; k++) {
    printf("moved forwards %f\n", r.forward(commands[k][0]));
    r.getRealPos(&p, &a);
    printf("Now at (%f, %f) heading %f\n",p.x(), p.y(), a);
    printf("turned %f\n", r.turn(commands[k][1]));
    r.getRealPos(&p, &a);
    printf("Now at (%f, %f) heading %f\n",p.x(), p.y(), a);
  }
  printf("I'm done now!\n");
  return 0;
}