#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#include <TimerOne.h>        // Zeit Interrupts
//#include <MultiFuncShield.h> // API für das Multi Function Shield

#include <Servo.h>


#define PinTemp A0  // sensor  
#define PinServo 5 // Servo pin
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define NUMFLAKES     10 // Number of snowflakes in the animation example

#define LOGO_HEIGHT   16
#define LOGO_WIDTH    16
static const unsigned char PROGMEM logo_bmp[] =
{ B00000000, B11000000,
  B00000001, B11000000,
  B00000001, B11000000,
  B00000011, B11100000,
  B11110011, B11100000,
  B11111110, B11111000,
  B01111110, B11111111,
  B00110011, B10011111,
  B00011111, B11111100,
  B00001101, B01110000,
  B00011011, B10100000,
  B00111111, B11100000,
  B00111111, B11110000,
  B01111100, B11110000,
  B01110000, B01110000,
  B00000000, B00110000 };

Servo mainServo;
int position = 0;
int previousPosition;

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

Timer1.initialize();
 // MFS.initialize(&Timer1);    // initialize multi-function shield library
  
 // MFS.write("1234");
  pinMode(PinTemp, INPUT);    // input setzen 
  mainServo.attach(5);



  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(2000); // Pause for 2 seconds

  // Clear the buffer
  display.clearDisplay();

  // Draw a single pixel in white
  display.drawPixel(10, 10, SSD1306_WHITE);

  // Show the display buffer on the screen. You MUST call display() after
  // drawing commands to make them visible on screen!
  display.display();
  delay(2000);
  // display.display() is NOT necessary after every single drawing command,
  // unless that's what you want...rather, you can batch up a bunch of
  // drawing operations and then update the screen all at once by calling
  // display.display(). These examples demonstrate both approaches...



 // testscrolltext();    // Draw scrolling text
 


  delay(1000); 
}
void loop() {
  while (1) 
  {
    int wert = analogRead(PinTemp);
    float mV = wert * 4935. / 10024.; // MFS liefert gemessene 4,935 V an +5V
    float temp = mV / 10.; // OUTPUT = 0 mV + 10.0 mV/°C
    int position = map(temp, 28, 40, 0, 180);
    if(previousPosition != position){
     mainServo.write(position);}

    
     Serial.println(position);
     Serial.println(temp); 
     display.clearDisplay();

  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Temp:"));
  display.println(temp);
  display.println(F("Position"));
  display.println(position);
  display.display();      // Show initial text
  delay(10);  
    //MFS.write (temp);

  
  }
 
}



void testscrolltext(void) {
  display.clearDisplay();

  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("test"));
  display.display();      // Show initial text
  delay(100);

}