/*
 * Pseudo code for tableSweeper final project
 * description: we have most of the infrastructure in place to create
 */

#use "explego.icb"
#use "use-sonar.ic"

void main() {
    // start main behaviors (as threads)

    mainDeliberator();
    avoidObstacles();
    determinePosition();
}

void mainDeliberator() {
    // begin initial behavior
    buildMap();
    while (shouldKeepWandering) {

        // run detector in background... when conditions are correct start next
behavior (do not terminate buildMap)
    }
    // initialize behavior to cover as much area of generated map as possible
    coverMap();
}

// initializes a bunch of sensor reading processes
void avoidObstacles() {
    // start new threads to poll sensors
    watchForBumps()
      watchForTape()
      scanSonar()
      openGrippers()
      while (true)  {
        if (obstacleDetected) {
            closeGrippers()
              performEvasiveAction
              openGrippers()
          }
    }
}

void buildMap() {
    while (true) {
        Check for sensor readings
          update map appropriately
          updateOdds(oldOdds, sonarModel, xPos, yPos)
          }
          }

          void updateOdds(oldOdds, sonarModel, xPos, yPos) {
            // use evidence grids to update our map
        }

        void scanSonar() {
            while (true) {
                move
                  pass sonar readings to buildMap
                  if (obstacleDetected) {
                    pass signal to avoidObstacles()
                  }
            }
        }

        void watchForTape() {
            while (true) {
                pass light sensor readings to buildMap
                  if (obstacleDetected) {
                    pass signal to avoidObstacles()
                  }
            }
        }
        void watchForBumps() {
            while (true) {
                pass bump readings to buildMap
                  if (obstacleDetected) {
                    pass signal to avoidObstacles()
                  }
            }
        }

        void coverMap() {
            while (territoryUnexplored) {
                planPath();
                executePath();
            }
        }

        void planPath() {
            // use map and covered territory to plan next move
        }

        void executePath() {
            // given map and path, generate motor commands to accomplish it
        }

        void determinePosition() {
            // poll encoders to determine heading and x, y position
        }
        void closeGrippers() {

        }

        void openGrippers() {

        }