import processing.video.*;
import processing.serial.*;
import java.io.*;
import java.net.*;

String kameraName = "Logitech HD Webcam C270";  //Bei Alissa(mac): "USB-Kamera" Bei Theresa(windows): "Logitech HD Webcam C270"

final int test = 0; //Debug Modus zur ausführlichen Ausgabe
final int tafelbreite = 175; //in cm
final int tafelhoehe = 114; //in cm
final int high = 480; //Kamerahöhe
final int wide = 640; //Kamerabreite
final float schwammbreite = 16;
final float radumfang = 19.3;
final float robumfang = 2*16.5*PI;
final float schwammabstand = 24; //ausgleichen vom ueberstehen des schwamm
final float punktabstand = 11; //Abstandzwischen den farbeigen Punkten (Mittelpunkte)

Capture video;
Serial port;
Socket clientSocket;
DataOutputStream outToServer;
InputStream inFromServer;

boolean schwarz;
boolean angekommen;

int maus = 0; //Variable zum Hochzählen bei Mausklick, um verschieden Zustände zu unterscheiden 
int a = 50; //Seite a des ausgewählten Rechtecks per Mausklick
int b = 50; //Seite b des ausgewählten Rechtecks per Mausklick
int x_1, y_1, x_2, y_2; //Mauspositionen bei Klick 1 und 2

int threshold = 130; //Grenzwert, ob Pixel schwarz oder weiß wird
color black = color(0);
color white = color(255);
color red = color(255, 0, 0);
color blue = color(0, 0, 255);
int numPixels;
float rot_xPos = 0;
float rot_yPos = 0;
float blau_xPos = 0;
float blau_yPos = 0;

char val = ' '; //Befehlsvariable für Arduino

int actualState = 0; //aktueller Modus des Arduino/Roboters
int programState = 0; //Sollmodus des Arduino/Roboters

float schwammumdrehungen = schwammbreite / radumfang;
float punktdistanz = 0;
float schwammdistanz = 0;

void setup() 
{
  size(640, 580);
  strokeWeight(5);
  videoStart();
  smooth();
  /*println(Serial.list());
   port = new Serial(this, Serial.list()[0], 9600);*/

  //Wlan einrichten
  try
  {
    String serverAddress="192.168.0.100";
    int serverPort=8000;
    clientSocket = new Socket(serverAddress, serverPort);
    outToServer = new DataOutputStream(clientSocket.getOutputStream());
    inFromServer = clientSocket.getInputStream();
  }
  catch(Exception e)
  {
    print("Verbindung aufbauen setup");
    println(e);
  }
  if (video.available()) 
    Bildfaerbung(); //Kamerabild lesen und Bild einfärben
  servo(); //Servo hochfahren
  delay(2000);
  punktdistanz = dist(rot_xPos, rot_yPos, blau_xPos, blau_yPos); //Umrechnungsfaktor Pixel <-> Zentimeter in echt
  schwammdistanz = (punktdistanz/punktabstand) * schwammabstand; //Umrechnung in cm
}


void draw() {
  float winkel  = 0;
  char input = ' ';
  if (antwortArduino() == 'f') //Wenn Arduino sagt, er ist fertig...
    programState++; //...Sollmodus um 1 erhöhen -> in den nächsten Zustand gehen

  if (video.available())
  {
    Bildfaerbung(); //Bild aufnehmen und einfärben

    //Bereich auswaehlen
    if (maus == 1) //Erster Klick: Erste Ecke des Rechtecks speichern
    {
      x_1 = mouseX;
      y_1 = mouseY;
      maus = 2;
    }
    if (maus == 2) //Rechteck mit erster Ecke und Maus malen
    {
      stroke(255, 0, 0);
      line(x_1, y_1, x_1, mouseY);
      line(x_1, y_1, mouseX, y_1);
      line(x_1, mouseY, mouseX, mouseY);
      line(mouseX, y_1, mouseX, mouseY);
    }
    if (maus == 3) //Zweiter Klick: Zweite Ecke des Rechecks speichern
    {
      x_2 = mouseX;
      y_2 = mouseY;
      //Falls die erste Ecke ncht oben links ist und die zweite Ecke unten Rechts, müssen Ecken getauscht werden, damit im Programm die erste Ecke oben links und die zweite Ecke unten rechts ist.
      if (x_2 < x_1)
      {
        int hilf = x_1;
        x_1 = x_2;
        x_2 = hilf;
      }
      if (y_2 < y_1) 
      {
        int hilf = y_1;
        y_1 = y_2;
        y_2 = hilf;
      }
      //Seitenlängen berechnen
      a = x_2 - x_1;
      b = y_2 - y_1;
      if (test == 1)
      {
        print("a= ");
        print(a);
        print("\tb= ");
        print(b);
        print("\tx_1= ");
        print(x_1);
        print("\ty_1=");
        print(y_1);
        print("\tx_2 =");
        print(x_2);
        print("\ty_2= ");
        println(y_2);
      }
      maus = 0; //Zurück auf Anfang für ein neues Kästchen bereit sein

      //Bereich auf Schwarz untersuchen
      schwarz = bereichTesten(a, b);
      println(schwarz);
      
      if (schwarz) {
        text("Schwarz vorhanden", 20, height - 60);
      } else {
        text("Kein Schwarz vorhanden", 20, height - 60);
      }

      //Roboter dreht sich Richtung linkes Ende des Kaestchens
      if (schwarz) 
      {
        winkel = berechneWinkel(mouseX-punktdistanz, mouseY); //Drehwinkel berechnen
        println(winkel);
       // actualState = 1;
        roboterDrehen(winkel);
        schwarz = false;
      }
      if (actualState < programState) //Wenn aktueller Zustand kleiner als Sollzustand ist...
      {
        actualState++; //..in Sollzustand gehen
        if ((actualState > 7) && (programState > 7)) //Wenn beide Zustände > 7 -> Roboter ist fertig -> Zustände zurücksetzen
        {
          actualState = 0;
          programState = 0;
        }

        switch (actualState)
        {
        case 1:  //zur linken oberen eck des kaestchens bewegen
          delay(3000);
          fahren(dist((int)rot_xPos, (int)rot_yPos, x_1-schwammdistanz, y_1));
          delay(3000);

        case 2:  //Roboter waagerecht drehen
          winkel = berechneWinkel(rot_xPos+1, rot_yPos);
          roboterDrehen(winkel);
          println(winkel);
          angekommen = true;
          println(angekommen);

        case 3: //Schwamm absenken
          servo();

        case 4: //kaestchen abfahren
          kaestchenAbfahren();
          angekommen = false;

        case 5:  //Schwamm hoch
          servo();

        case 6: //Roboter zurueck drehen
          winkel = berechneWinkel(100, 400);
          roboterDrehen(winkel);

        case 7:  //Zum Startpunkt fahren
          fahren(dist((int)rot_xPos, (int)rot_yPos, 100, 400));
        }
      }
    }
    if (maus == 0) { //Mit Klick fertig -> Kästchen malen, bis zum nächsten Klick
      noFill();
      stroke(0, 255, 0);
      rect(x_1, y_1, a, b);
    }
  }

  fill(255);
  stroke(0);
  rect(0, height - 100, width, 100);
  fill(0);
  text("(x,y) = (" + mouseX + ", " + mouseY + ")", 20, height - 80);
}

void mouseClicked() { //Mauszähler erhöhen
  if (maus == 0) {
    maus = 1;
  } else if (maus == 2) {
    maus = 3;
  }
}

/*void keyReleased() { //Wlan Testfunktion
  try
  {
    outToServer.writeBytes("v3");
  }
  catch (Exception e)
  {
    println(e);
  }
}*/

void videoStart() 
{
  //Verfügbare Kameras suchen und ausgeben
  String[] cameras = Capture.list();
  if (cameras.length == 0)   
  {
    println("There are no cameras available for capture.");
    exit();
  } else {
    println("Available cameras:");
    for (int i = 0; i < cameras.length; i++) 
    {
      println(i + " " + cameras[i]);
    }
    //Kamera initialisieren
    video = new Capture(this, 640, 480, kameraName, 30);

    //Kameraaufnahme starten
    video.start(); 

    numPixels = video.width * video.height; //Anzahl der Pixel des Kamerabildes berechnen
    println(numPixels);
  }
}

void Bildfaerbung() 
{
  //Kamerbild und Pixel lesen
  video.read();
  video.loadPixels();
  float pixelBrightness;
  loadPixels();

  //Variablen initialisieren
  int currPos = 0;
  color tempPixel;
  int rotxmax = 0;
  int rotxmin = wide;
  int rotymax = 0;
  int rotymin = high;

  int blauxmax = 0;
  int blauxmin = wide;
  int blauymax = 0;
  int blauymin = high;

  for (int y = 1; y <= 480; y++) //Y-Achse des Bildes durchgehen (Zeilenweise)
  {
    for (int x = 1; x <= 640; x++) //X-Achse des Bildes durchgehen (Spaltenweise)
    {   
      currPos = ((y-1) * wide) + (x-1); //von x,y-Koordinaten in Array-Position umrechnen
      pixelBrightness = brightness(video.pixels[currPos]); //Helligkeit des Pixels speichern
      tempPixel = video.pixels[currPos];

      //int pixelColor = video.pixels[currPos];
      int r = (tempPixel >> 16) & 0xff; //Rotanteil des Pixels bestimmen
      int g = (tempPixel >> 8) & 0xff; //Grünanteil des Pixels bestimmen
      int b = tempPixel & 0xff; //Blauanteil des Pixels bestimmen

      if (r > 1.7*g && r > 1.7*b && r>50) //Wenn Rotanteil überwiegt...
      {
        pixels[currPos] = color(255, 20, 20); //...Pixel rot färben

        //Ersten und letzten rote Pixel bestimmen, sowhl auf der x-Achse, als auch auf der y-Achse
        if (x < rotxmin) 
          rotxmin = x;
        else if (x > rotxmax) 
          rotxmax = x;
        if (y < rotymin) 
          rotymin = y;
        else if (y > rotymax)
          rotymax = y;
      }
      /* else if(g > 1.5*r && g > 1.5*b && g>20)
       pixels[currPos] = color(0, 255, 0);*/

      else if (b > 1.7*r && b > 1.7*g && b>50) { //Wenn Blauanteil überwiegt...

        pixels[currPos] = color(0, 0, 255); //...Pixel blau färben

        //Ersten und letzten blauen Pixel bestimmen, sowhl auf der x-Achse, als auch auf der y-Achse
        if (x<blauxmin) blauxmin = x;
        else if (x>blauxmax) blauxmax = x;
        if (y<blauymin) blauymin = y;
        else if (y>blauymax) blauymax = y;
      } 
      else if (pixelBrightness > threshold) { //Wenn Pixel heller als der Grenzwert ist...
        pixels[currPos] = white; //...Pixel weiß färben
      } else { //Ansonsten...
        pixels[currPos] = black; //...Pixel schwarz färben
      }
    }
  }

  updatePixels();
  //Mittelpunkte des roten und des blauen Kreises
  rot_xPos = (rotxmin + rotxmax)/2;
  rot_yPos = (rotymin + rotymax)/2;
  blau_xPos = (blauxmin + blauxmax)/2;
  blau_yPos = (blauymin+blauymax)/2;
  //Mittelpunkte in Schwarz einzeichnen
  strokeWeight(5);
  stroke(0);
  point(rot_xPos, rot_yPos);
  point(blau_xPos, blau_yPos);
  //rot_xPos = 0;
  //rot_yPos = 0;
  //blau_xPos = 0;
  //blau_yPos = 0;
}

boolean bereichTesten(int seite_a, int seite_b) 
{
  for ( int i = x_1; i < seite_a + x_1; i++) { //x-Achse des Kästchens ablaufen
    for (int j = y_1; j < seite_b + y_1; j++) { //y-Achse des Kästchen ablaufens
      int testFarbe = get(i, j);
      float testHelligkeit = brightness(testFarbe);
      if (testHelligkeit < threshold) { //Falls Helligkeit am Punkt kleiner als der Grenzwert ist...
        return true; //...ist Schwarz vorhanden
      }
      //print(j);
      //print(" ");
    }
    //println(i);
  }
  return false; //Ansonsten ist kein Schwarz vorhanden
}

float berechneWinkel(float xPos, float yPos) {  // berechnet winkel zwischen richtung des Roboters und gerade Mittelpunkt des Roboters und punkt xPos yPos
  float alphav = arctan(blau_xPos-rot_xPos, (height-blau_yPos)-(height-rot_yPos));
  float alphaz = arctan(xPos-rot_xPos, (height-yPos)-(height-rot_yPos));
  float winkel = alphaz-alphav;

  if (winkel > PI) {
    winkel = winkel - 2*PI;
  } else if (winkel < -PI) {
    winkel = winkel + 2*PI;
  }
  return winkel;
}

float arctan(float deltax, float deltay) { //Berechnung des Arkustangens von 0 bis 360 Grad, damit die Drehrichtung bestimmt werden kann
  if (deltay == 0) {
    if (deltax > 0) {
      return PI;
    } else {
      return -PI;
    }
  } else if (deltay > 0) {
    return atan(deltax/deltay);
  } else {
    return atan(deltax/deltay) + PI;
  }
}

void roboterDrehen(float winkel) 
{
  //gewünschte Radumdrehungen des Roboters berechnen
  float strecke = (winkel*robumfang) / (2*PI);
  float umdrehungen = strecke / radumfang;
  //Signal an Roboter schicken
  if (winkel < 0) //nach links drehen
  {
    val = 'l';
    try
    {
      outToServer.writeBytes(val + str(umdrehungen)); //Beispiel: "l3.24"
    }
    catch (Exception e)
    {
      println(e);
    }
  } else if (winkel > 0) //nach rechts drehen
  {
    val = 'r';
    try
    {
      outToServer.writeBytes(val + str(umdrehungen));
    }
    catch (Exception e)
    {
      println(e);
    }
  } else if (winkel == 0) { //nichts tun bzw. stoppen
    val = 's';
    try
    {
      outToServer.write(val);
    }
    catch (Exception e)
    {
      println(e);
    }
  }
}

void fahren(float zieldistanz) { //Vorwärts fahren
  //gewünschte Radumdrehungen berechnen
  float punktdistanz = dist(rot_xPos, rot_yPos, blau_xPos, blau_yPos);
  float strecke = (punktabstand/punktdistanz) * zieldistanz;
  float wegumdrehungen = strecke / radumfang;
  //Signal an Roboter schicken
  val = 'v';
  try
  {
    outToServer.writeBytes(val + str(wegumdrehungen));
  }
  catch (Exception e)
  {
    println(e);
  }
  delay(10000);
}

void kaestchenAbfahren() {

  int i = (int ) (b / schwammbreite) + 1;

  fahren(a);
  warten(); //Auf Rückmeldung des Roboters warten

  for (int j = 2; j<i; j++) 
  {
    if (j%2 == 0) 
    {
      roboterDrehen(PI/2);
      warten();
      val = 'v';
      try
      {
        outToServer.writeBytes(val + str(schwammumdrehungen));
      }
      catch (Exception e)
      {
        println(e);
      }
      delay(3000);
      roboterDrehen(PI/2);
      warten();
      fahren(a - 2 * schwammdistanz);
      warten();
    } else {
      println("else");
      roboterDrehen(-PI/2);
      warten();
      val = 'v';
      try
      {
        outToServer.writeBytes(val + str(schwammumdrehungen));
      }
      catch (Exception e)
      {
        println(e);
      }
      delay(3000);
      roboterDrehen(-PI/2);
      warten();
      fahren(a - 2 * schwammdistanz);
      warten();
    }
  }
}

void servo() {
  if (angekommen) //Servo Runter lassen
  {
    val = 'R';
    try
    {
      outToServer.write(val);
    }
    catch (Exception e)
    {
      println(e);
    }
    delay(1000);
  } else { //Servo hochfahren
    val = 'H';
    try
    {
      outToServer.write(val);
    }
    catch (Exception e)
    {
      println(e);
    }
    delay(1000);
  }
}

//empfaengt und speichert Variable vom Input
char antwortArduino()
{
  try
  {
    char input = ' ';
    while (inFromServer.available() != 0)
    {
      input = (char) inFromServer.read();
      return input;
    }
  }
  catch (Exception e)
  {
    println(e);
  }
  return ' ';
}

//wartet bis der Arduino seine Aktion zu Ende ausgefuehrt hat
void warten()
{
  char signal = ' ';
  do {
    signal = antwortArduino();
  } while (signal != 'f');
}