/* Autor:      Felix Stahlberg                                               */
/* E-Mail:     stahli36@googlemail.com                                       */
/* URL:        http://www.dorfdesign.pytalhost.de                            */
/* This program is free software; you can redistribute it and/or modify it   */
/* under the terms of the GNU General Public License as published by the     */
/* Free Software Foundation; either version 2 of the License, or (at your    */
/* option) any later version.                                                */
#define O_LEFT OUT_A
#define O_RIGHT OUT_B
#define O_TOWER OUT_C
#define S_LIGHT SENSOR_1
#define S_FRONT SENSOR_3//Sensor zum Programmabbruch vorne als Stoßsensor (vorlaeufig)

#define MIDDLE_TOWER 16
/*Zeit, die der Tower nach
der Berührung mit den Sensor braucht, genau mittig zu sein*/
#define TOWER_PRESSED SENSOR_2==1

#define STRAIGHT 0//für View
#define RIGHT 3
#define LEFT 1

int View,LightDir,Brightest,OldLightDir,WaitTime;

task TurnTower(){
/*TowerState:
0: Blick gerade nach vorn
1: Blick irgendwo links
2: Blick gerade nach hinten
3: Blick irgendwo rechts(praktischerweise Startposition): nächste Position:0*/
     while(1==1){
          if(TOWER_PRESSED){//View managen
               View++;
               if(View>=4)
                    View=STRAIGHT;
               while(TOWER_PRESSED)Wait(10);
          }
          if(S_LIGHT<Brightest){//LightDir managen
               Brightest=S_LIGHT;
               LightDir=View;
          }
     }
          
}

void SearchLight(){
     Brightest=1023;//max dunkel
     start TurnTower;
     On(O_TOWER);
     while(View==STRAIGHT);
     while(View!=STRAIGHT);
     Wait(MIDDLE_TOWER);
     Off(O_TOWER);
     stop TurnTower;
}

task MainProc()
{//eigentliche main-Task, nur wegen Überprüfung des Stoßsensors in eigene Task verpackt
     while(0==0){
          SearchLight();
          if((OldLightDir!=0)&&(LightDir!=0)&&(OldLightDir!=LightDir)){//Wechsel von rechts nach links oder umgekehrt
          //Zu weit gedreht->kleinere Schritte
               if(WaitTime>=10)
                    WaitTime/=2;
               else
                    LightDir=STRAIGHT;//Zu geringe Abweichung, losfahren
         }
         OldLightDir=LightDir;
          switch(LightDir){
               case STRAIGHT://Geradeaus
                    OnFwd (O_RIGHT+O_LEFT);
                    Wait(150);
                    WaitTime=10;//Nur noch kleinere Änderung nötig
                    break;
               case LEFT://Nach Links
                    OnFwd(O_RIGHT);
                    OnRev(O_LEFT);
                    Wait(WaitTime);
                    break;
               case 2://Umdrehen (Als "nach Links" Auffassen, doch stark gegenlenken
                    OnFwd(O_RIGHT);
                    OnRev(O_LEFT);
                    Wait(150);
                    break;
               case RIGHT://Nach Rechts
                    OnRev(O_RIGHT);
                    OnFwd(O_LEFT);
                    Wait(WaitTime);
                    break;
          }
          Off(O_RIGHT+O_LEFT);
     }
}

task main(){
     SetSensorType(SENSOR_2, SENSOR_TYPE_TOUCH);//Setttings
     SetSensorMode(SENSOR_2,SENSOR_MODE_BOOL);
     SetSensorType(S_FRONT, SENSOR_TYPE_TOUCH);
     SetSensorMode(S_FRONT,SENSOR_MODE_PERCENT);
     SetSensorType(S_LIGHT,SENSOR_TYPE_LIGHT);
     SetSensorMode(S_LIGHT,SENSOR_MODE_RAW);
     WaitTime=40;//Initalizisations
     OldLightDir=0;
     View=STRAIGHT;//Guckt std.mäßig geradeaus
     
          start MainProc;//eigentlicher Programmstart
          
     while(S_FRONT==0);
     stop MainProc;
     start TurnTower;//Am Hinderniss angelangt: Turm gerade drehen
     On(O_TOWER);
     while(View!=0);
     Wait(MIDDLE_TOWER);
     Off(O_TOWER);
     stop TurnTower;
}