/*
 *  PSBVLego 2003, Gruppe 3
 *  F. Unger, C. Sommer, M. Stroebl
 *  "Ziel suchen ohne Kamera"
 *
 *  Ziel:
 *    Anfahren einer Lichtquelle jenseits unbekanntem Terrain
 *
 *  Aenderungen:
 *    2002-10-22: Erste Fahrtests
 *                Pledge-Algorithmus
 *    2002-10-29: Bug2-Algorithmus
 *    2002-11-05: Rotationssensor für Drehungen
 *    2002-11-12: Kurskorrekturen beim Fahren zur Lichtquelle
 *                Pledge-Alg falls Lichtquelle nicht ortbar
 *    2002-11-19: Kleinere Korrekturen, variabler Suchbereich
 *                beim findLight, Winkel nun in Grad
 *    2002-11-26: Umfährt U-Lab trotz Hinterfahrens eigener
 *                Position, Drehungsgenauigkeit verbessert,
 *                Lampe für Statusanzeige, LIVELOCK-Erkennung
 *    2002-12-03: Kosmetik am Code
 *    2002-12-10: Light-Shadow-Problem durch zusätzlichen Modus
 *                behoben, Prüfung auf x>0 durch Bereich ersetzt
 *    2003-01-14: Versuch Bug2 durch Curve1 zu ersetzen,Bug2
 *                verwendet, Kosmetik am Code
 */
 

#define fullCircleLeft      (158)
#define fullCircleRight     (168)
#define fullCircle          (180)
#define LIGHTDELTA          3
#define LIVELOCK            600
#define wallCheckInterval   30

// Sensor-Belegung:
#define S_ROT               SENSOR_1
#define S_LIGHT             SENSOR_2
#define S_BUMP              SENSOR_3

// Status:
//   - FAST FLASH:  heading towards light
//   - SLOW FLASH:  avoiding obstacle
//   - ON:          Pledge
#define O_LAMP              OUT_B

int posX        =   0;
int posY        =   0;
int orientation =   0;  // 0..3
int angle       =   0;  // wie Orientation, läuft aber -inf...+inf
int bumped      =   0;  // Bumperkontakt?
int foundLight  =   0;
int noLiveLock  =   0;

void fullStop() {
    Off(OUT_A+OUT_C);
    }

void softStop() {
    Float(OUT_A+OUT_C);
    }

// Drehung links
void turnLeft(int turnangle) {
    fullStop();
    ClearSensor(S_ROT);
    OnRev(OUT_A);
    OnFwd(OUT_C);
    while (abs(S_ROT) < ((turnangle*fullCircleLeft)/fullCircle) ) {}
    fullStop();
    angle -= 1;
    orientation -= 1;
    if (orientation < 0) orientation += 4;
    }

// Drehung rechts
void turnRight(int turnangle) {
    fullStop();
    ClearSensor(S_ROT);
    OnFwd(OUT_A);
    OnRev(OUT_C);
    while (abs(S_ROT) < ((turnangle*fullCircleRight)/fullCircle) ) {}
    fullStop();
    angle += 1;
    orientation += 1;
    if (orientation >= 4) orientation -= 4;
    }

// werte Rotationssensor aus
void countWay() {
    switch (orientation) {
        case 0:
            posY += -S_ROT;
            break;
        case 1:
            posX += -S_ROT;
            break;
        case 2:
            posY -= -S_ROT;
            break;
        case 3:
            posX -= -S_ROT;
            break;
        }
    }

// fahre ein Stueck vorwaerts, melde Hindernis
void tryAhead(int seconds) {
    bumped = 0;
    ClearSensor(S_ROT);

    OnFwd(OUT_A+OUT_C);
    int x;
    for (x=0; x < seconds; x++) {
        Wait(1);
        if (S_BUMP) {
            bumped=1;
            fullStop();
            countWay();
            return;
            }
        }
    fullStop();
    countWay();
    }

// setze ein Stueck zurueck
sub backoff() {
    ClearSensor(S_ROT);
    Wait(5);
    OnRev(OUT_A+OUT_C);
    Wait(20);
    fullStop();
    Wait(5);
    countWay();
    }

// orientiere Fahrzeug auf hellste Lichtquelle
void findLight(int sweepAngle) {
    int tgAngle = 0;
    int ReadingBefore = 0;
    int Reading = 0;
    int maxReading = 0;
    foundLight = 0;

    if (sweepAngle < fullCircle) turnRight(sweepAngle/2);

    // suche hellste Lichtquelle
    ClearSensor(S_ROT);
    // links drehen
    OnRev(OUT_A);
    OnFwd(OUT_C);
    while (abs(S_ROT) < ((sweepAngle*fullCircleLeft)/fullCircle) ) {
        ReadingBefore = Reading;
        Reading = S_LIGHT;
        // reagiere nur auf Licht-Peak
        if ( ((Reading-ReadingBefore) > LIGHTDELTA) && (ReadingBefore > 0) ) {
            foundLight = 1;
            PlaySound(SOUND_CLICK);
            }
        if ( (Reading > maxReading) && (foundLight)) {
            maxReading = Reading;
            tgAngle = (abs(S_ROT)*fullCircle)/fullCircleLeft;
            }
        }
    fullStop();
    ClearSensor(S_ROT);
    // richte Roboter auf Lichtquelle aus
    if (foundLight) {
        if ( ((sweepAngle-tgAngle) < (tgAngle)) || (sweepAngle < fullCircle) ) {
            turnRight(sweepAngle-tgAngle);
            } else {
            turnLeft(tgAngle);
            }
        } else {
        if (sweepAngle < fullCircle) turnRight(sweepAngle/2);
        }
    fullStop();
    }

// folge Hindernis auf andere Seite, Bug2
void avoidObstacle() {
    orientation = 0;
    posX = 0;
    posY = 0;

    start slowFlash;
    ClearTimer(0);
    backoff();
    turnLeft(fullCircle/4);
    while (((posX <= 0) || (posX > 120) || (posY <= 0)) && ((Timer(0) < LIVELOCK) || noLiveLock)) {
        followWall();
        }
    if ((Timer(0) >= LIVELOCK) && (noLiveLock == 0)) PlaySound(SOUND_DOWN);
    turnLeft(fullCircle/4);
    stop slowFlash;
    }

// Kernroutine zum Hindernisumfahren, fährt ein Stück die Wand entlang
void followWall() {
    tryAhead(wallCheckInterval);
    if (bumped) {
        // Innenwinkel
        backoff();
        turnLeft(fullCircle/4);
        } else {
        turnRight(fullCircle/4);
        tryAhead(30);
        if (bumped) {
            // Wand noch da
            backoff();
            turnLeft(fullCircle/4);
            } else {
            // Aussenwinkel
            }
        }
    }

// durchfahre Parcours, bis Licht auftaucht - Pledge
void searchLight() {
    int i = 0;
    
    On(O_LAMP);

    while (1) {
        // Fahre vorwaerts bis Hindernis:
        while (1) {
            tryAhead(120);
            if (bumped) break;
            backoff();
            findLight(fullCircle);
            if (foundLight) {
                Off(O_LAMP);
                return;
                }
            }

        // umfahre Hindernis:
        backoff();
        angle=0;
        turnLeft(fullCircle/4);
        do {
            followWall();
            i++;
            if (i > 2) {
                i=0;
                backoff();
                findLight(fullCircle);
                if (foundLight) {
                    Off(O_LAMP);
                    return;
                    }
                }
            } while (angle != 0);
        }
    }

// fahre zum Licht, bricht ab bei Bumperkontakt
void driveToLight() {
    int i;
    
    start fastFlash;
    
    while (1) {
        tryAhead(90);
        if (bumped) {
            stop fastFlash;
            return;
            }
        findLight(fullCircle/4);
        }
    }

task main() {
    int t;

    // Sensor-Eingaenge einstellen
    SetSensor(S_LIGHT, SENSOR_LIGHT);
    SetSensor(S_BUMP, SENSOR_TOUCH);
    SetSensor(S_ROT, SENSOR_ROTATION);

    // Bumper bei Programmstart gedrueckt ==> Vollkreis drehen
    for (t=0; t < 50; t++) {
        Wait(1);
        if (S_BUMP) {
            noLiveLock = 1;
            turnLeft(fullCircle);
            Wait(500);
            turnRight(fullCircle);
            Wait(500);
            break;
            }
        }

    // Task für Abbruchbedingung starten
    start taskLight;

    // Hauptprogramm
    findLight(fullCircle);
    while (1) {
        if (foundLight) {
            // Bug2-Algorithmus
            driveToLight();
            avoidObstacle();
            findLight(fullCircle/2);
            } else {
            // Pledge-Algorithmus
            searchLight();
            }
        }
    }

// hält Ausführung an, wenn Ziel gefunden
task taskLight() {
    while (S_LIGHT < 100) {};
    stop main;
    softStop();
    PlaySound(SOUND_UP);
    StopAllTasks();
    }

// Status-Lampe
task slowFlash() {
    while (1) {
        On(O_LAMP);
        Wait(20);
        Off(O_LAMP);
        Wait(20);
        }
    }
    
    
// Status-Lampe
task fastFlash() {
    while (1) {
        On(O_LAMP);
        Wait(8);
        Off(O_LAMP);
        Wait(8);
        }
    }