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

//===================================================================
//Konstruktor des Roboters. Ein neuer Roboter wird erzeugt.
//===================================================================
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.
    WARTE(500);

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

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

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

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