#define DIST 30
#define EX_TIME 60000//experiment time
#define REV_POWER 50
#define TURN_ANGLE 139 //turn 30 deg abt left wheel
#define FWD_POWER 50
#define COUNT 2//number of times to turn,formula=
#define FILE_NAME "distance.dat"
#define SPD_FILE_NAME "distFwd.dat"
#define ANG_FILE_NAME "angle.dat"
#define FILE_SIZE 2048

int distFwd=0,angle=0,count=COUNT+1;
byte handle=0,speed_handle=0,distfwd_handle=0,angle_handle=0;

inline void turnleft()
{
  RotateMotor(OUT_C,REV_POWER,TURN_ANGLE);
  angle++;//on every turn, angle increase
  distFwd=0;
  count++;
}

inline void fwd()
{
  RotateMotor(OUT_AC,FWD_POWER,180);//move forward by 0.5*circumference of wheel
  distFwd=1;
}

task main()
{
  //declarations
  SetSensorLowspeed(S4);

  int dist;
  DeleteFile(FILE_NAME);
  DeleteFile(SPD_FILE_NAME);
  DeleteFile(ANG_FILE_NAME);
  CreateFile(FILE_NAME,FILE_SIZE,handle);
  CreateFile(SPD_FILE_NAME,FILE_SIZE,distfwd_handle);
  CreateFile(ANG_FILE_NAME,FILE_SIZE,angle_handle);
  
  //MAIN LOOP
  string s;
  while(CurrentTick() - initTime <= EX_TIME)
  {
    //keeping track of time
  	t = CurrentTick() - initTime;
    dist = SensorUS(S4);
    if (dist >200) dist = 255;//objects that are too far have great inaccuracy, 
                              //so I just standardized their value above a certain threshold
    s = NumToStr(dist);
    WriteLn(handle, s);//save distance sensed
    s = NumToStr(distFwd);
    WriteLn(distfwd_handle, s);//save forward or not from the previous step
    s = NumToStr(angle);
    WriteLn(angle_handle, s);//save angle

    if ((dist <= DIST) && (count < COUNT))
    {
       turnleft();
    }

    else if (dist <= DIST && count >= COUNT)//obstacle after fwd
    {
       count = 0;
       turnleft();
    }

    else if (dist > DIST && count < COUNT)//obstacle disappear for 1st time
    {
        turnleft();
    }

    //the COUNT is there to ensure that the NXT has turn an angle big enough
    //to account for its width

    else if (dist > DIST && count >= COUNT)
  	{
        fwd();
        }
  }
  Off(OUT_AC);
  CloseFile(handle);
  CloseFile(speed_handle);
  CloseFile(angle_handle);
}