Strategy Mars Rover Hank

  while(!shutdown_requested()) {

    if (lake_detected) {
      rover_state(LAKE_DETECTED);
    } else if (edge_detected) {
      rover_state(EDGE_DETECTED);
    }

    // display rover state
    lcd_int(rover_current_state);

    /***********************************
         SCANNING FOR LAKES
    ***********************************/
    if (rover_current_state == SCANNING_FOR_LAKES) {

      // just started scanning?
      if (rover_scanning_angle == 0) {

        // reset trigger
        sem_wait(&sem_com);
        TRIGGER2 = 0;
        TRIGGER3 = 0;
        sem_post(&sem_com);

        // sleep to wait for coordinates
        msleep(SCANNING_WAIT_TIME);

      }

      // if other brick received coordinates
      if (TRIGGER2 != 0) {

        // point mars rover to the found lake
        rover_state(DIRECTING_TO_LAKE);

      } else {

        // rotate a step and stay in scanning mode
        drive_rotate(LAKE_SCANNING_STEP_ANGLE);

        // sleep to wait for coordinates
        msleep(SCANNING_WAIT_TIME);

        // increase angle
        rover_scanning_angle += LAKE_SCANNING_STEP_ANGLE;

      }

      // when turned 360 degrees and still no lake is found
      if (rover_scanning_angle >= 360) {

        // go to exploratory driving
        rover_state(DRIVING);

      }

    }

    /***********************************
         DIRECTING TO LAKE
    ***********************************/
    else if (rover_current_state == DIRECTING_TO_LAKE) {

      // when no coordinate is present
      if (TRIGGER2 == 0) {

        // no coordinate, no point in directing, just drive
        rover_state(DRIVING);

      } else {

        // convert camera coordinates to distance and angle
        lake_angle = 0;
        lake_distance = 0;
        conv_coord(TRIGGER2, TRIGGER3, &lake_distance, &lake_angle);

        // when error is low enough, drive ahead
        if (absolute(lake_angle) < DIRECTING_THRESHOLD_ANGLE) {

          // drive to the lake
          rover_state(DRIVING);

        } else {

          // rotate towards the lake and stay in directing mode
          drive_rotate(lake_angle);

        }

        // forget current coordinate
        sem_wait(&sem_com);
        TRIGGER2 = 0;
        TRIGGER3 = 0;
        sem_post(&sem_com);


      }

    }

    /***********************************
         DRIVING STRAIGHT
    ***********************************/
    else if (rover_current_state == DRIVING) {

      // if driving end time is not set
      if (rover_driving_start_time == 0) {

        // start driving
        drive_forward();

        // set driving end time
        rover_driving_start_time = get_system_up_time();

      }

      // when the rover is not driving to a lake or the directed driving time is exceeded
      if (rover_previous_state != DIRECTING_TO_LAKE || get_system_up_time() > rover_driving_start_time + DIRECTED_DRIVING_TIME) {

        // when a coordinate is received
        if (TRIGGER2 != 0) {

          // brake
          drive_brake();

          // direct towards lake
          rover_state(DIRECTING_TO_LAKE);

        }
      }

      // when the rover is not driving to a lake and the exploratory driving time is exceeded
      if (rover_previous_state != DIRECTING_TO_LAKE && get_system_up_time() > rover_driving_start_time + EXPLORATORY_DRIVING_TIME)  {

                // brake
                drive_brake();

                // scan for lakes
                rover_state(SCANNING_FOR_LAKES);

      }

    }

    /***********************************
         LAKE DETECTED
    ***********************************/
    else if (rover_current_state == LAKE_DETECTED) {

      // brake
      drive_brake();

      // reset communication trigger
      sem_wait(&sem_com);
      TRIGGER1 = 0;
      sem_post(&sem_com);

      // set max measurement end time
      temp_measurement_end_time = get_system_up_time()+2*TEMP_ACTUATION_TIME+TEMP_WAIT_TIME+2000;

      // ask brick 1 to do temperature measurement
      com_send(ID,1,1,&message);

      // wait for brick 2 to complete temp measurement or max time exceeding
      while (TRIGGER1 == 0 && temp_measurement_end_time > get_system_up_time()) {
        msleep(50);
      }

      // reset lake light sensor
      light_reset_average(1);

      // we have dealt with this lake
      lake_detected = 0;

      // act like edge is detected
      rover_state(EDGE_DETECTED);

    }

    /***********************************
         EDGE DETECTED
    ***********************************/
    else if (rover_current_state == EDGE_DETECTED) {

      // drive backwards
      drive_backward();
      msleep(EDGE_BACKWARDS_TIME);

      // reset light averages
      light_reset_average(0);
      light_reset_average(2);

      // we have dealt with this edge
      edge_detected = 0;

      // if a lake was detected, we are away from it now
      lake_detected = 0;

      // rotate plus or minus pseudo randomly
      if (get_system_up_time() % 2 > 0) {
         drive_rotate(-1*EDGE_ANGLE);
      } else {
         drive_rotate(EDGE_ANGLE);
      }

      // drive a while
      rover_state(DRIVING);

    }

    // end of loop
    msleep(100);

  }