#include <Arduino.h>//port 8 rechts vorne
//SLAVE

//Libraries-----------------------------------------------------------------------------------------------------------------

#include <SoftwareSerial.h>
#include "U8glib.h" //Display

#include <Servo.h>

//Initialization-----------------------------------------------------------------------------------------------------------------

SoftwareSerial mySerial(3, 2);
U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_NONE | U8G_I2C_OPT_DEV_0);

int motor1 = 5;
int motor2 = 6;
int motor3 = 9;
int motor4 = 10;

Servo myservo;

//Variables-----------------------------------------------------------------------------------------------------------------

//int Button =8;
//int led =10;

String OutputString ="";
String str ="";
String data ="";
String value1String="";
String value2String="";
String value3String="";
String value4String="";
int value1=100;
int value2=100;
int value3=100;
int value4=100;
int kg1 =0;
int kg2 =0;
int kg3 =0;
int kg4 =0;

//char *data1 = " ";
String data1 ="";
const byte address[6] = "00001";

char text[32] = "";


//-----------------------------------------------------
String input_line = "";
bool read_input_line() {
  while(mySerial.available()) {
    char data = (char) mySerial.read();
    // was \n? don't add to string, indicate full input value
    if (data == '\n') {
      input_line.replace("\r", "");
      input_line.replace("\n", "");
      return input_line.length() != 0;
    }
    input_line += data;
  }
  return false;
}
//Setup-----------------------------------------------------------------------------------------------------------------

void setup(){
  Serial.begin(9600);
  mySerial.begin(38400);

  pinMode(motor1, OUTPUT);
  pinMode(motor2, OUTPUT);
  pinMode(motor3, OUTPUT);
  pinMode(motor4, OUTPUT);

  u8g.setFont(u8g_font_8x13);

  //myservo.attach(11);



  /*
  pinMode(Button, INPUT_PULLUP);
  pinMode(led,OUTPUT);
  */
}

//Main-----------------------------------------------------------------------------------------------------------------



void loop() {
  if(read_input_line()) {
    // we have a new valid string!!
    //Serial.println("Received: " + input_line);
    // process it...

    VariableCreator(input_line);
    Serial.println(value1);
    myservo.attach(11);
    Motor(value1,value2,value3,value4);
    delay(100); // wait for servo to reach its position
    myservo.detach();


    // reeset
    input_line = "";
  }
}

//Subs-----------------------------------------------------------------------------------------------------------------

/*
void ButtonLED(){

  if (mySerial.read() == 'a'){
    digitalWrite(led, HIGH);
  }
  else{
    digitalWrite(led, LOW);
  }

  int button = digitalRead(Button);
  if(button == LOW){
    mySerial.println("b");
  }
}
*/

//-----------------------------------------------------------------------------------------------------------------

int VariableCreator(String input_line){
  //char Buf[50];
    //data.toCharArray(Buf, 50);
  sscanf(input_line.c_str(),"%d %d %d %d", &value1, &value2, &value3, &value4);
  return(value1, value2, value3, value4);
}

void Motor(int value1,int value2,int value3, int value4){
    analogWrite(motor1, value1);                  // sets the servo angle according to the scaled value
    analogWrite(motor2, value2);
    analogWrite(motor3, value3);
    analogWrite(motor4, value4);
    myservo.write(value1);
}

int VariableManipulation(int value1,int value2,int value3,int value4){
  kg1 = map(value1, 0, 255, 0, 100);
  kg2 = map(value2, 0, 255, 0, 100);
  kg3 = map(value3, 0, 255, 0, 100);
  kg4 = map(value4, 0, 255, 0, 100);
  return(kg1, kg2, kg3, kg4);
}

void Display(int kg1, int kg2, int kg3, int kg4){
  u8g.firstPage();
    do {
      //Write text. (x, y, text)
      String Sensor1 = "S1: "+String(kg1)+"% |";
      String Sensor2 = "S2: "+String(kg2)+"%";
      String Sensor3 = "S3: "+String(kg3)+"% |";
      String Sensor4 = "S4: "+String(kg4)+"%";
      //String text1= String(kg1);
      u8g.drawStr(5, 15, Sensor1.c_str());
      u8g.drawStr(70, 15, Sensor2.c_str());
      u8g.drawStr(5, 30, Sensor3.c_str());
      u8g.drawStr(70, 30, Sensor4.c_str());
    }
    while (u8g.nextPage());
    //delay(5);
}