// vehicle.cc     for traffic simulation
//                includes route class for vehicle

#include "vehicle.h"
#include <cstdio>

route::route( int Aroute_count, direction Amoving, route * route_list)
{
  intersection_count = Aroute_count;
  moving = Amoving;
  next = route_list;
}

void route::add_route( int Aroute_count, direction Amoving)
{
  next = new route( intersection_count, moving, next);
  intersection_count = Aroute_count;
  moving = Amoving;
}

direction route::get_moving(void)
{
  return moving;
}

int route::get_count(void)
{
  return intersection_count;
}

route * route::get_next(void)
{
  return next;
}

// vehicle.cc

vehicle::vehicle( float Alength,
	      float Aweight,
	      float Aaccel,
	      float Adecell,
	      float Aposition,
	      float Aspeed,
	      float Aspeed_limit,
	      float Ax_base,
	      float Ay_base,
	      direction Amoving,
	      float Asignal_pos,
              signal_state Asignal,
	      vehicle *Anext_ptr)
{
     id = unique_id();
     length = Alength;
     weight = Aweight;
     accel =  Aaccel;
     decell = Adecell;
     position = Aposition;
     position_last = Aposition;
     speed = Aspeed;
     speed_limit = Aspeed_limit;
     time_last_move = (float)clock()/CLK_PER_SEC;
     x_base = Ax_base;
     y_base = Ay_base;
     moving = Amoving;
     signal = Asignal;
     signal_pos = Asignal_pos;
     next_ptr = Anext_ptr;
}

void vehicle::move(void)
{
  float delta_time;
  float separation = 16.0/10.0; // 16 feet per 10 feet per second
  float speed_temp;
  float position_temp;
  float stopping_distance;

  delta_time = (float)clock()/CLK_PER_SEC - time_last_move;
  time_last_move = (float)clock()/CLK_PER_SEC;
  position_last = position;
  speed_temp = speed;

  if( position < signal_pos )
  {
    // attempt normal movement, accelerate to speed limit
    if (speed_temp < speed_limit)      // try to accelerate
    {
      speed_temp = speed + accel*delta_time;
      if (speed_temp > speed_limit)
      {
	speed_temp = speed_limit;
      }
    }
    position_temp = position + speed_temp*delta_time;

    // limit speed if necessary to keep distance from vehicle in front
    if (next_ptr != 0)          // check what is in front ( if any )
    {
      if( position_temp + separation*speed_temp >
	 next_ptr->position)
      {
	speed_temp    = speed;
	position_temp = position + speed_temp*delta_time;
	if (position_temp + separation*speed_temp >
	   next_ptr->position)
	{
	  // must slow down to keep distance from vehicle in front
	  if (speed > 0.0)     // can not back up
	  {
	    speed_temp = speed - decell*delta_time;
	    if (speed_temp < 0.0)
	    {
	      speed_temp = 0.0;
	    }
	  }
	  position_temp = position + speed_temp*delta_time;
	}
      }
    }

    // if signal is red, yellow or stop  in range, must decelerate
    switch (signal)
    {
      case red:
      case yellow:
      case stop:
      if (speed_temp >= speed)
      {
	stopping_distance = 0.5 * speed_temp * speed_temp / decell;
	if (stopping_distance > signal_pos-position_temp)
	{
	  if (speed > 0.0)      // can not back up
	  {
	    speed_temp = speed - decell*delta_time;
	    if (speed_temp < 0.0)
	    {
	      speed_temp = 0.0;
	    }
	  }
	  position_temp = position + speed_temp*delta_time;
	  if( speed_temp == 0.0)  // force to exit
	  {
	    position_temp = signal_pos;
	  }
	}
      }
    }

    position = position_temp;
    speed  = speed_temp;
  }
  draw();
} // end move

vehicle *vehicle::next(void)
{
    return next_ptr;
}

void vehicle::new_next( vehicle *a_next )
{
       next_ptr = a_next;
}

bool vehicle::exiting( void )
{
  if ( this == 0 )
  {
    return false;
  }
  if( position >= signal_pos && signal != red && signal != yellow )
  {
    if( route_list != 0 )
    {
      route_count = route_count+1;
      if( route_now->get_count() >= route_count)
      {
        moving = route_now->get_moving(); // go this way!
        route_count = 0;
        route_now = route_now->get_next();
        if( route_now->get_moving() == reset)
        {
          route_now = route_list; // reset
        }
      }
    }
    return true;
  }
  else
  {
    return false; // (position >= signal_pos);
  }
}

void vehicle::set_new_lane_data(float Aspeed_limit,
		       float Ax_base,
		       float Ay_base,
		       direction Amoving,
		       float Asignal_pos,
                       signal_state Asignal)
{
    position_last = position;
    draw(true); // erase id in old position
    position = position-signal_pos; // retain increment beyond end
    if(position < 0.0) position = 0.0;
    speed_limit = Aspeed_limit;
    x_base = Ax_base;
    y_base = Ay_base;
    moving = Amoving;
    signal_pos = Asignal_pos;
    signal = Asignal;
}

void vehicle::set_signal(signal_state Asignal)
{
    signal = Asignal;
}

void vehicle::draw(bool erase_only)
{
    float x_offset;
    float y_offset;
    char any_string[4];
    sprintf(any_string,"%d",id);
    get_offsets(moving, x_offset, y_offset);
    WRITE_TEXT(x_base+x_offset*position_last/my_scale,
               y_base+y_offset*position_last/my_scale+0.5,
	       any_string,
	       CLEAR_OUTLINE);
    if(erase_only) return;
    WRITE_TEXT(x_base+x_offset*position/my_scale,
	       y_base+y_offset*position/my_scale+0.5,
	       any_string,
	       OUTLINE);
} //  end draw
// end vehicle.cc