#include <Adafruit_MPU6050.h>
#include <Servo.h>

Adafruit_MPU6050 mpu;   // Adresse von mpu6050 ist 0x68, SDA pin an A4, SCL pin an A5
sensors_event_t event;  // Werte von MPU6050
Servo myservo;
const int servoPin = 6;
const int ledPin = 11;
const int servoReset = 90;
int XPos, servoPos, lastXPos;
int ledValue;


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

  while (!mpu.begin()) {
    Serial.println("mpu6050 keine Verbindung");
    delay(2000);
    }
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);  // Range = 2G
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);       // 250 deg/s
  mpu.setFilterBandwidth(MPU6050_BAND_184_HZ);   // Filter bandwidth set to: 184 Hz

  lastXPos = 0;
  servoPos = servoReset;
  myservo.attach(servoPin);  // Servomotor an Arduino-Pin 6
  myservo.write(servoPos);

  Serial.println("mpu6050 ist verbunden");
  pinMode(ledPin, OUTPUT);
  }


void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  XPos = g.gyro.x * 57296 / 1000;  // 1 Radian = 57.2957795 degrees
  XPos = XPos + 90;

//  getLedValue (temp.temperature);  // LED-Helligkeit je nach Temperatur (0 bis 40 Grad)
//  temp.temperature = constrain(temp.temperature, 0, 40);  // limits range of sensor values to between 0 and 40 degrees
//  ledValue = map(temp.temperature, 0, 40, 0, 255);  // Re-maps a number from one range to another
  ledValue = mapConstrained (temp.temperature, 0, 40, 0, 255, true);
  analogWrite(ledPin, ledValue);

  Serial.print("xPosition=");
  Serial.print(XPos);
  Serial.print(" - servoPos=");
  Serial.print(servoPos);
  Serial.print(" Grad");
  Serial.print(" - temp=");
  Serial.print(temp.temperature);
  Serial.print(" - LED = ");  Serial.println(ledValue);

  if (abs(lastXPos - XPos) > 5) {  // erst bei 5 Grad Änderung reagieren

    if ((XPos < servoPos)  && (XPos > 0)) {         // Motor läuft zurück
      while (servoPos > XPos) {
        servoPos --;
        myservo.write(servoPos);
        delay(20);
        }
      }
    else if ((XPos > servoPos) && (XPos < 180)) {   // Motor läuft vorwärts
      while (servoPos < XPos) {
        servoPos ++;
        myservo.write(servoPos);
        delay(20);
        }
      }
    lastXPos = XPos;
    }

  delay(50);
  }


long mapConstrained (long x, long in_min, long in_max, long out_min, long out_max, bool constrained) {
  if (constrained) {
    if (x < in_min) x = in_min;
    if (x > in_max) x = in_max;
    }
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  }


void getLedValue (int temperatur) {
  if (temperatur < 10) {
    ledValue = 20;
      }
  else if (temperatur < 20) {
    ledValue = 80;
      }
  else if (temperatur < 30) {
    ledValue = 160;
      }
  else
    ledValue = 255;  // temperatur > 30 Grad
  }