class Vector
{
  float a,m;
  
  Vector() {}
  Vector(float $a, float $m)
  {
    a = $a;
    m = $m;
  }
  Vector(Vector v)
  {
    a = v.a;
    m = v.m;
  }
  
  void set(float $a, float $m)
  {
    a = $a;
    m = $m;
  }
  void set(Vector v)
  {
    a = v.a;
    m = v.m;
  }
  void set(Position p, Position p2)
  {
    m = dist(p, p2);
    a = normalize(atan2(p, p2));
  }
  
  void add(Vector v)
  {
    float x = cos(a)*m + cos(v.a)*v.m;
    float y = sin(a)*m + sin(v.a)*v.m;
    m = dist(0,0,x,y);
    a = normalize(atan2(y,x));
  }
  void add(float $m, float $a)
  {
    float x = cos(a)*m + cos($a)*$m;
    float y = sin(a)*m + sin($a)*$m;
    m = dist(0,0,x,y);
    a = normalize(atan2(y,x));
  }
  
  void subtract(Vector v)
  {
    float x = cos(a)*m - cos(v.a)*v.m;
    float y = sin(a)*m - sin(v.a)*v.m;
    m = dist(0,0,x,y);
    a = atan2(y,x);
  }
  
  void turn_to(Vector v, float amt)
  {
    if (a > v.a) { a -= amt; }
    if (a < v.a) { a += amt; }
  }
  void turn_to(float $a, float amt)
  {
    if (a > $a) { a -= amt; }
    if (a < $a) { a += amt; }
  }
}


float normalize(float ang)                        //normalizes a heading between 0 and 360
{
  while(ang > TWO_PI) ang -= TWO_PI;
  while(ang < 0) ang += TWO_PI;
  return ang;
}