//#define debug
#include <debug.h>
#include <makros.h>
#include "Ampel.h"
/*
einfache Ampelanlage für eine Kreuzung mit 2 Strassen und Fussgänger.
 
 Besonderheiten:
 Das Ampelprogramm liegt im FLASH. Die Struktur ist recht einfach, 
 ein Datenblock besteht immer aus 5 byte. Zunächst kommt die Länge des Schrittes in Sekunden, 
 dann die Ampeldaten, abgelegt werden die o.g. Zeichen. Also R für rot,  Y für Gelb... 
 (ACHTUNG. Grossbuchstaben) Ist das Programm zu Ende muss ein 255 als Länge erscheinen.
 
 Tabelle der verschiedenen Ampelphasen:
 
 Ampel    Dauer A1  A2  F1  F2 
 1        10    R   G   G   R    Grün für A2, grün für Fussgänger F1
 2        5     R   G   R   R    F1 Fussgänger auf rot
 3        1     R   Y   R   R    A2 wird gelb
 4        1     R   R   R   R    allet rot
 5        2     O   R   R   R    A1 wird rot/gelb
 6        10    G   R   R   G    Grün für A1 und die Fussgänger F2
 7        5     G   R   R   R    F2 Fussgänger auf rot
 8        1     Y   R   R   R    A1 wird gelb
 9        1     R   R   R   R    allet rot
 10       2     R   O   R   R    A2 wird rot/gelb
 
 R = rot, Y = gelb, G = grün, O = rotgelb (Orange ;-) )
 
 */

const byte STEUERUNG[] = {
  10, 'R', 'G', 'G', 'R' ,
  5, 'R', 'G', 'R', 'R' ,
  1, 'R', 'Y', 'R', 'R' ,
  1, 'R', 'R', 'R', 'R' ,
  2, 'O', 'R', 'R', 'R' ,
  10, 'G', 'R', 'R', 'G' ,
  5, 'G', 'R', 'R', 'R' ,
  1, 'Y', 'R', 'R', 'R' ,
  1, 'R', 'R', 'R', 'R' ,
  2, 'R', 'O', 'R', 'R', 255 
};


// FLASH Adresse
word addr;
byte dauer;
unsigned long time = 0;


void setup() {
  addr = 0;
  dauer = 0;

  for(int i = 0; i < 14; i++) {
    pinMode(i, OUTPUT);
  }
  time = millis();
}

void loop() {
  long delta = (millis() - time) / 1000;
  if (delta >= dauer) {
    dauer = STEUERUNG[addr];
    addr++;
    if (dauer == 255) {
      addr = 0;
      dauer = STEUERUNG[addr];
      addr++;
    } 
    nextStep();
    time = millis();
  }     
  delay(100);
}

void nextStep() {
  char cA1 = STEUERUNG[addr];      
  addr++;
  char cA2 = STEUERUNG[addr];      
  addr++;
  char cF1 = STEUERUNG[addr];      
  addr++;
  char cF2 = STEUERUNG[addr];
  addr++;

  AMPEL a1 = getAmpelState(cA1);
  AMPEL a2 = getAmpelState(cA2);
  AMPEL f1 = getAmpelState(cF1);
  AMPEL f2 = getAmpelState(cF2);

  showAmpel(a1, 0, 1, 2);
  showAmpel(f1, 3, 13, 4);
  showAmpel(a2, 5, 6, 7);
  showAmpel(f2, 8, 13, 9);
}

AMPEL getAmpelState(char cAmpel) {
  switch (cAmpel) {
  case 'R':
    return A_ROT;
  case 'Y':
    return A_GELB;
  case 'O':
    return A_ROT_GELB;
  case 'G':
    return A_GRUEN;
  default:
    return A_ROT;
  }
}

void showAmpel (AMPEL ampel, byte rPin, byte yPin, byte gPin) {
  switch (ampel) {
  case A_ROT: 
    digitalWrite(rPin, HIGH);
    digitalWrite(yPin, LOW);
    digitalWrite(gPin, LOW);
    break;
  case A_ROT_GELB:
    digitalWrite(rPin, HIGH);
    digitalWrite(yPin, HIGH);
    digitalWrite(gPin, LOW);
    break;
  case A_GELB:
    digitalWrite(rPin, LOW);
    digitalWrite(yPin, HIGH);
    digitalWrite(gPin, LOW);
    break;
  case A_GRUEN:
    digitalWrite(rPin, LOW);
    digitalWrite(yPin, LOW);
    digitalWrite(gPin, HIGH);
    break;
  default:
    digitalWrite(rPin, HIGH);
    digitalWrite(yPin, LOW);
    digitalWrite(gPin, LOW);
  }   
}