#include <BristleBot.h>
 #include <wiring.h>


//     BristleBot::BristleBot()
//     {
//         //Pin fuer linkes Licht vorn.
//         leftLight      = 3;
//         //Pin fuer rechtes Licht vorn.
//         rightLight     = 7 ;
//         //Pin fuer rechtes Licht hinten.
//         backLightRight = 8 ;
//         //Pin fuer linkes Licht hinten.
//         backLightLeft  = 2 ;
//         //Pin fuer blaue LED vorn.
//         ledBlue        = 9;
//         //Pin fuer linken Sensor.
//         sensorLeft     = 3 ;
//         //Pin fuer rechten Sensor.
//         sensorRight    = 2 ;  
//         //Pin fuer rechten Motor (muss PWN Pin sein!!!).
//         motorRight     = 10;
//         //Pin fuer linken Motor (muss PWN Pin sein!!!).
//         motorLeft      = 11;        
//     }
// // //Stopt alle Motoren
// void BristleBot::stop()
// {
//     //Setze rechten Motor auf 0 Volt
//     analogWrite( motorRight, 0 );
//     //Setze linken Motor auf 0 Volt
//     analogWrite( motorLeft, 0 );
// }

// //Schaltet beide Motoren an und faehrt gerade aus
// void BristleBot::fahreGerade()
// {
//     //Setze rechten Motor auf maximal Volt (Darf nicht mehr als 3 Volt sein!!!)
//     analogWrite( motorRight, motOn  );
//     //Setze linken Motor auf maximal Volt (Darf nicht mehr als 3 Volt sein!!!)
//     analogWrite( motorLeft, motOn  ); 
// }

// //Schaltet rechten Motor an, um eine Linkskurve zu fahren.
// void BristleBot::fahreLinks()
// {
//     //Setze linken Motor auf 0 Volt
//     analogWrite( motorLeft, 0 );
//    //Setze rechten Motor auf maximal Volt (Darf nicht mehr als 3 Volt sein!!!)
//     analogWrite( motorRight, motOn  );
// }

// //Schaltet linken Motor an, um eine Rechtskurve zu fahren.
// void BristleBot::fahreRechts()
// {
//     analogWrite( motorRight, 0 );
//     analogWrite( motorLeft, motOn );
    
// }


// //Lese beide Sensorwerte aus und bestimme, welcher der beiden Sensoren mehr Licht 
// //emfaengt bzw. beide Sensoren gleich viel Licht.
// int BristleBot::leseSensor()
// {
//     //Lese rechten Sensorwert ( Wert ist zwischen 0 (kein Licht) und 1024 (ganz hell).
//     int lightRight = analogRead( sensorRight );
//     //Lese linken Sensorwert ( Wert ist zwischen 0 (kein Licht) und 1024 (ganz hell).
//     int lightLeft = analogRead( sensorLeft );

//     //Entscheide, ob rechter Sensor viel mehr Licht empfaengt als linker Sensor.
//     if( ( lightLeft - lightRight ) <  -sensibility )
//     {
//         return RECHTS;
//     } //Entscheide, ob linker Sensor viel mehr Licht empfaengt als rechter Sensor.
//     else if  (( lightLeft - lightRight ) >  sensibility )
//     {
//         return LINKS;
//     } //Ansonsten sind beide Sensorwerte FAST gleich.
//     else
//     {
//         return BEIDE;
//     }       
    
// }

// //Schalte Ruecklicht an oder aus. Wenn das Ruecklicht angeschaltet werden soll, muss fuer "an" true
// //uebergeben werden. Wenn das Licht ausgeschaltet werden soll, muss fuer "an" false uebergeben werden.
// void BristleBot::ruecklicht(bool an )
// {
//     //Schalte rechtes Ruecklicht an oder aus.
//     digitalWrite(backLightRight, !an);
//     //Schalte linkes Ruecklicht an oder aus.
//     digitalWrite(backLightLeft, !an);
// }


// //Schalte linkes Licht an oder aus. Wenn das linke Licht angeschaltet werden soll, muss fuer "an" true
// //uebergeben werden. Wenn das Licht ausgeschaltet werden soll, muss fuer "an" false uebergeben werden.
// void BristleBot::linkesLicht(bool an)
// {
//     //Schalte linkes Licht an oder aus.
//     digitalWrite(leftLight, !an);
// }

// //Schalte rechtes Licht an oder aus. Wenn das rechte Licht angeschaltet werden soll, muss fuer "an" true
// //uebergeben werden. Wenn das Licht ausgeschaltet werden soll, muss fuer "an" false uebergeben werden.
// void BristleBot::rechtesLicht( bool an)
// {
//     //Schalte rechtes Licht an oder aus.
//     digitalWrite(rightLight, !an);
// }

// //Aktiviere Roboter und fuere einige Tests durch
// void BristleBot::init(int _on, int _sens )
// {
//     //Setze Wert fuer eingeschalteten Motor Wert. Muss zwischen 0 und 255 liegen. (Darf nicht mehr als 3 Volt sein!!!
//     //Bei 5 Volt Stromversorgung darf der Wert nicht hoeher als 180 betragen, sonst brutzeln die Motoren durch)
//     motOn = _on;
//     //Differenz zwischen beiden Sensoren, um einen hell/dunkel Uebergang zu erkennen.
//     sensibility = _sens;

//     //Aktiviere Eingangs/Ausgangs Pins des Microcontrollers
//     pinMode(leftLight, OUTPUT);
//     pinMode(rightLight, OUTPUT);
//     pinMode(backLightRight, OUTPUT);
//     pinMode(backLightLeft, OUTPUT);
//     pinMode(ledBlue, OUTPUT );
//     //Schalte blaue LED an
//     digitalWrite(ledBlue, LOW);

//     //Schalte rechtes Licht an.
//     rechtesLicht(AN);
//     //Schalte linkes Licht aus.
//     linkesLicht(AUS);
//     //Schalte Ruecklicht aus.
//     ruecklicht(AUS);
//     //Warte eine halbe Sekunde.
//     delay(500);

//     //Schalte rechtes Licht aus.
//     rechtesLicht(AUS);
//     //Schalte linkes Licht an.
//     linkesLicht(AN);
//     //Schalte Ruecklicht aus.
//     ruecklicht(AUS);
//     //Warte eine halbe Sekunde.
//     delay(500);

//     //Schalte rechtes Licht aus.
//     rechtesLicht(AUS);
//     //Schalte linkes Licht aus.
//     linkesLicht(AUS);
//     //Schalte Ruecklicht an.
//     ruecklicht(AN);
//     //Warte eine halbe Sekunde.
//     delay(500);

//     //Schalte rechtes Licht aus.
//     rechtesLicht(AUS);
//     //Schalte linkes Licht aus.
//     linkesLicht(AUS);
//     //Schalte Ruecklicht aus.
//     ruecklicht(AUS);
//     //Warte eine halbe Sekunde.
//     delay(500);

//     //Fahre Rechtskurve.
//     fahreRechts();
//     //Fahre drei Sekunden lang.
//     delay(3000);
//     //Fahre Linksskurve.
//     fahreLinks();
//     //Warte drei Sekunden.
//     delay(3000);
//     //Fahre Linksskurve.
//     fahreLinks();
//     //Fahre drei Sekunden lang.
//     delay(3000);
// }