% This is the module for recovering from bumps. 
% The two important outputs are RECOVERY_START, RECOVERY_END. 

	module BUMP: 

	input TOUCH_1, TOUCH_3; 
	input CRUISE_SPEED(integer); 
        output MOTOR_A_SPEED(integer), MOTOR_C_SPEED(integer),  
               MOTOR_A_DIR(integer), MOTOR_C_DIR(integer), 
               CPUTS(string),
	       RECOVERY_START, RECOVERY_END; 
        constant MOTOR_FWD, MOTOR_REV, MAX_SPEED : integer; 
	    var t : integer in 
	       every [TOUCH_1 or TOUCH_3] do 
		  emit CPUTS("bump"); 
		  emit RECOVERY_START;
		  present TOUCH_1 then 
		     t:=1; 
		  else 
		     t:=3; 
		  end present; 
		  emit MOTOR_A_DIR(MOTOR_REV); 
		  emit MOTOR_C_DIR(MOTOR_REV); 
		  emit MOTOR_A_SPEED(?CRUISE_SPEED); 
		  emit MOTOR_C_SPEED(?CRUISE_SPEED); 
		  await 50 tick;        % 1 tick = 1 ms 
		  if t=1 then 
		     emit MOTOR_A_DIR(MOTOR_FWD); 
		  else 
		     emit MOTOR_C_DIR(MOTOR_FWD); 
		  end if; 
		  await 50 tick;  % 1 tick = 1 ms 
		  % Go forward again 
		  emit MOTOR_A_DIR(MOTOR_FWD); 
		  emit MOTOR_C_DIR(MOTOR_FWD); 
		  emit CPUTS ("ok"); 
		  % Drives forward a bit, to avoid being in the same
		  % situation due to a light behind the obstacle. 
		  await 50 tick; 
		  % and say that we have finished the recovery
		  emit RECOVERY_END
              end every 
           end var
	end module 

% This module wiggles left and right by a certain amount, trying to
% optimize the amount of light. 

	module WIGGLE: 

	input CRUISE_SPEED(integer); 
        output MOTOR_A_SPEED(integer), MOTOR_C_SPEED(integer),  
               MOTOR_A_DIR(integer), MOTOR_C_DIR(integer), 
               CPUTS(string);
	sensor LIGHT_2_VALUE : integer; 
        constant MOTOR_FWD, MOTOR_REV, MAX_SPEED : integer; 
	
	      loop
	         var light_l: integer, 
		     light_r: integer, 
		     light_c: integer,
		     full_speed: integer, 
		     half_speed: integer, 
		     delta: integer, 
		     max_light: integer in 

		    full_speed := ?CRUISE_SPEED; 
		    half_speed := ?CRUISE_SPEED / 2; 
		    % half_speed := 0; 
		    delta := 20; % Delta-time for wiggling. 
		    % Goes just a little bit straight. 
		    await 3 * delta tick;
		    %
		    % Samples light 
		    %
		    % Samples light in the center. 
		    light_c := ?LIGHT_2_VALUE; 
		    % Samples light on the left. 
		    emit MOTOR_A_DIR(MOTOR_REV); 
		    emit MOTOR_C_DIR(MOTOR_FWD);
		    emit MOTOR_A_SPEED(half_speed); 
		    emit MOTOR_C_SPEED(full_speed); 
		    await delta tick; 
		    light_l := ?LIGHT_2_VALUE; 
		    % Samples light on the right
		    emit MOTOR_A_DIR(MOTOR_FWD); 
		    emit MOTOR_C_DIR(MOTOR_REV);
		    emit MOTOR_A_SPEED(full_speed); 
		    emit MOTOR_C_SPEED(half_speed); 
		    await 2*delta tick; 
		    light_r := ?LIGHT_2_VALUE; 
		    %
		    % Decides where to go 
		    %
		    % Computes max light 
		    max_light := 0; 
		    if light_r > max_light then 
		       max_light := light_r
		    end if;
		    if light_l > max_light then 
		       max_light := light_l
		    end if;
		    if light_c > max_light then 
		       max_light := light_c
		    end if;
		    % Chooses the max light, giving preference to 
		    % going straight. 
		    if light_c = max_light then 
		       % goes straight
		       emit CPUTS("strgt"); 
		       emit MOTOR_A_DIR(MOTOR_REV); 
		       emit MOTOR_C_DIR(MOTOR_FWD);
		       emit MOTOR_A_SPEED(half_speed); 
		       emit MOTOR_C_SPEED(full_speed); 
		       await delta tick; 
		       emit MOTOR_A_DIR(MOTOR_FWD); 
		       emit MOTOR_C_DIR(MOTOR_FWD);
		       emit MOTOR_A_SPEED(full_speed); 
		       emit MOTOR_C_SPEED(full_speed);
		    elsif light_r = max_light then  
		       % goes right, that is, straight from 
		       % current orientation. 
		       emit CPUTS("rght"); 
		       emit MOTOR_A_DIR(MOTOR_FWD); 
		       emit MOTOR_C_DIR(MOTOR_FWD);
		       emit MOTOR_A_SPEED(full_speed); 
		       emit MOTOR_C_SPEED(full_speed); 
		    else 
		       % goes left 
		       emit CPUTS("left"); 
		       emit MOTOR_A_DIR(MOTOR_REV); 
		       emit MOTOR_C_DIR(MOTOR_FWD);
		       emit MOTOR_A_SPEED(half_speed); 
		       emit MOTOR_C_SPEED(full_speed); 
		       await 2*delta tick; 
		       emit MOTOR_A_DIR(MOTOR_FWD); 
		       emit MOTOR_C_DIR(MOTOR_FWD);
		       emit MOTOR_A_SPEED(full_speed); 
		       emit MOTOR_C_SPEED(full_speed); 
		    end if
		 end var
	      end loop 
       end module

% This code generates a sustained signal. 
       module GEN_SIGNAL: 
       input START, END; 
       output HAPPENING; 
       % Note the immediate below: otherwise, HAPPENING is 
       % generated with one unit of delay. 
       every immediate START do 
          abort 
	     sustain HAPPENING
	  % If we used "immediate END", the signal HAPPENING would 
	  % become inactive at the same time when END is generated, 
	  % which in this case is not what we want. 
	  % What would happen if we used a weak abort? 
	  when END
       end every 
       end module 

% This module does a full-circle sweep. 
	module SWEEP: 

	input CRUISE_SPEED(integer);
        output MOTOR_A_SPEED(integer), MOTOR_C_SPEED(integer),  
               MOTOR_A_DIR(integer), MOTOR_C_DIR(integer), 
               CPUTS(string), DO_SWEEP, SWEEP_DONE;
	sensor LIGHT_2_VALUE : integer; 
        constant MOTOR_FWD, MOTOR_REV, MAX_SPEED : integer; 

	loop 
	   emit DO_SWEEP; 
	   % This pretends to do the sweep. 
	   emit MOTOR_A_SPEED(0); 
           emit MOTOR_C_SPEED(0); 
	   emit CPUTS ("sweep"); 
	   await 200 tick; 
	   % end of sweep 
	   emit SWEEP_DONE; 
	   emit CPUTS ("swok"); 
	   % Inter-sweep time 
	   await 400 tick; 
	end loop 
	end module 


% Main lego module 

        module lego : 
        input TOUCH_1, TOUCH_3; 
        output MOTOR_A_SPEED(integer), MOTOR_C_SPEED(integer),  
               MOTOR_A_DIR(integer), MOTOR_C_DIR(integer), 
               CPUTS(string);
        relation TOUCH_1 # TOUCH_3; 
	sensor LIGHT_2_VALUE : integer; 

        constant MOTOR_FWD, MOTOR_REV, MAX_SPEED : integer; 

	signal RECOVERY_START, RECOVERY_END, RECOVERING, 
	       DO_SWEEP, SWEEP_DONE, SWEEPING, 
	       CRUISE_SPEED(integer) in 
	    % This is the code to recover from bumping. 
	    run BUMP
	||
	    % Broadcasts the cruise speed
	    sustain CRUISE_SPEED (MAX_SPEED/2); 
	||
	   % This is the code that generates the signal RECOVERING. 
	   % Put this into a module!! 
	   run gen1 / GEN_SIGNAL [ signal RECOVERY_START / START, 
					  RECOVERY_END / END, 
					  RECOVERING / HAPPENING ]
	||
	   % This is the code that generates the signal SWEEPING. 
	   % Put this into a module!! 
	   run gen2 / GEN_SIGNAL [ signal DO_SWEEP / START, 
					  SWEEP_DONE / END, 
					  SWEEPING / HAPPENING ]
	||
	   % This wiggles towards the light. 
	   loop 
	      abort 
	         run WIGGLE; 
	      when immediate [RECOVERY_START or DO_SWEEP]; 
	      % Think: would an immediate await be fine here? 
	      await [not RECOVERING and not SWEEPING] 
	   end loop 
	|| 
	   % This does a full sweep. 
	   loop 
	      abort 
	         run SWEEP; 
	      when immediate RECOVERY_START; 
	      await RECOVERY_END
	   end loop 
	end signal
	end module