// mov2goal.cpp
//
// by Dave Hershberger - heavily modified version of Floyd Henning's
//                       move_to_goal.cc

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mov2goal.h"
#include "vector.h"

MoveToGoal::MoveToGoal(float x, float y, float mag)
{
  goal_x = x;
  goal_y = y;
  magnitude = mag;
  done = 0;
}

void MoveToGoal::read_from_console()
{
  printf("   New goal x > ");
  scanf("%f", &goal_x);
  printf("   New goal y > ");
  scanf("%f", &goal_y);
  printf("   New magnitude > ");
  scanf("%f", &magnitude);
  done = 0;
}

// vector returns a vector pointing from the robot to the goal.
// robot_x and robot_y describe the current position of the robot.
Vector MoveToGoal::vector(float robot_x, float robot_y)
{
  Vector retvec;
  float dx, dy;

  dx = goal_x - robot_x;
  dy = goal_y - robot_y;

  retvec.dir = atan2(dy, dx);

  if (fabs(dx) < 1.0 && fabs(dy) < 1.0){
    done = 1;
    retvec.mag = 0;
  }
  else{
    done = 0;
    retvec.mag = magnitude;
  }
  return retvec;
}