// How to send OSC messages from an Arduino.
// This Example is in the public domain.

// Important known issues:
// The Wifly provides no means to get the data from a single UDP package at a time
// Everything is just dumped to the serial connection.
// The only hint that a package is finished is that reading from the serial *might* time out
// To make things work anyway:
// 1. Use a high baud rate for communication with the Wifly: 115200 on a hardware serial is perfect.
// 2. Adjust the timeout in server.availableCheck(). 2ms works fine with 115200baud.
// 3. If possible, let the controlling Program pause at least 1.5 times the timout between sending OSC messages.

#include <WiFlyHQ.h>
#include <ArdOSCForWiFlyHQ.h>
#include <Servo.h>
//#include <SoftwareSerial.h>
//SoftwareSerial softSerial(2,3); // Rx is the recieving pin, Tx is the transmitting pin

WiFly wifly;
OSCServer server(&wifly);
int winkel = 0;
int ledPin=13;
Servo myservo;
int wifiwert=1; //wird benutzt für nur 1 wert immer senden
int array[180];
int hoechsterWifiWert=-500;
int ausrichtungsWinkel;

void setup()
{ 
  myservo.attach(22);
  myservo.write(winkel);
  Serial.begin(115200);
  //use the convenient setup.
  wifly.setupForUDP<HardwareSerial>(
    &Serial3,   //the serial you want to use (this can also be a software serial)
    57600, // if you use a hardware serial, I would recommend the full 115200. This does not work with a software serial.
    true,	// should we try some other baudrates if the currently selected one fails?
    "arduinonet",  //Your Wifi Name (SSID)
    "florian3", //Your Wifi Password 
    "WiFly",                 // Device name for identification in the network
    0,         // IP Adress of the Wifly. if 0 (without quotes), it will use dhcp to get an ip
    8000,                    // WiFly receive port
    "255.255.255.255",       // Where to send outgoing Osc messages. "255.255.255.255" will send to all hosts in the subnet
    8001,                     // outgoing port
    true	// show debug information on Serial
  );
  wifly.printStatusInfo(); //print some debug information 

  pinMode(ledPin,OUTPUT); //prepare to fade th einternal LED by OSC
   // Register a callback fuction of the LED fading.
   // When a message is with the adress "/ard/ledFade" is received, "setLedFade" will be invoked.
   server.addCallback("/ard/ledFade",&setLedFade);
   wifly.startCommand();
   
}


void loop()
{
	//check for new incoming messages. If a message with a registered adress is recieved, the callback function will be invoked.
  if(server.availableCheck(2)>0)
  {  
    //debugoutln("alive!"); //callback after process
  }
  for(int i=0;i<180;i++) {
    myservo.write(i);
    delay(100);
    array[i]=wifly.getRSSI();
    Serial.println(wifly.getRSSI());
    
    if(array[i]>hoechsterWifiWert) {
      hoechsterWifiWert=array[i];
      ausrichtungsWinkel=i;
    }
    
  }
  myservo.write(ausrichtungsWinkel);
  delay(10000);
  hoechsterWifiWert=-500;
    
    
  
   //if(wifly.getRSSI()!=wifiwert){  //nur wenn sich was verändert wirds geprinted
   //..
   //  Serial.println(wifly.getRSSI());    //...
  //}
  Serial.println(wifly.getRSSI());
  
}

void setLedFade(OSCMessage *_mes){
  //get 1st argument(int32) and use it to fade the LED
  int fadeValue=_mes->getArgInt32(0);
  Serial.println(fadeValue);
  analogWrite(ledPin,fadeValue );
}

void rotateRight(int delayValue){
  delay(delayValue);
  winkel--;
  if(winkel<0){
    winkel = 180;
  }
  myservo.write(winkel);
}

void rotateLeft(int delayValue){
  delay(delayValue);
  winkel++;
  if(winkel==180){
    winkel = 0;
  }
  myservo.write(winkel);
}