#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/clocks.h"
# include <stdio.h>

const int SERVO_PIN = 20;
const int BUT_LO = 2;
const int BUT_MID = 4;
const int BUT_HI = 6;
const int BUT_FLOW = 7;

// Funktion zur Berechnung des PWM-Wertes für einen gegebenen Winkel
// Dies ist die "Übersetzung" von Winkel zu Pulsbreite!
int get_pwm_wrap_for_angle(float angle) {
    // Standard-Servos: 0° -> 1ms Puls, 180° -> 2ms Puls
    // PWM-Frequenz: 50 Hz, also 20ms pro Zyklus
    // Pico Taktfrequenz: 125 MHz. Mit einem Prescaler von 125 wird jeder Tick zu 1 Mikrosekunde.
    // Ein 20ms-Zyklus (20000 µs) entspricht dann einem PWM-Wrap-Wert von 20000.
    const float min_pulse_us = 1000.0f;
    const float max_pulse_us = 2000.0f;
    
    // Lineare Umrechnung des Winkels (0-180) in die Pulsbreite (1000-2000 µs)
    float pulse_us = min_pulse_us + (angle / 180.0f) * (max_pulse_us - min_pulse_us);
    
    return (int)pulse_us;
}

int conf_pins (void) {
  // Konfiguriere den Servo-Pin für die PWM-Funktion
  gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);

    // Konfiguration der Taster-Pins
    // 1. Initialisiere den GPIO-Pin
    // 2. Setze die Richtung auf INPUT
    // 3. Aktiviere den internen Pull-Up-Widerstand (da Taster meist gegen GND schalten)
    gpio_init(BUT_LO);
    gpio_set_dir(BUT_LO, GPIO_IN);
    gpio_pull_up(BUT_LO);

    gpio_init(BUT_MID);
    gpio_set_dir(BUT_MID, GPIO_IN);
    gpio_pull_up(BUT_MID);

    gpio_init(BUT_HI);
    gpio_set_dir(BUT_HI, GPIO_IN);
    gpio_pull_up(BUT_HI);

    gpio_init(BUT_FLOW);
    gpio_set_dir(BUT_FLOW, GPIO_IN);
    gpio_pull_up(BUT_FLOW);

  return 0;
}


int main() {
  stdio_init_all();

  conf_pins();

  // Finde heraus, welcher PWM-Slice und Channel zu unserem Pin gehört
  int slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
  int chan = pwm_gpio_to_channel(SERVO_PIN);

  float freq = clock_get_hz(clk_sys);
  printf("System Frequency: %f \n", freq); // -> 125 MHz

  // Setze die PWM-Frequenz auf 50 Hz.
  // Der Systemtakt ist 125 MHz. Wir teilen ihn durch 125, um auf 1 MHz (1 Tick = 1µs) zu kommen.
  pwm_set_clkdiv(slice_num, 125.0f); 
  //pwm_set_clkdiv(slice_num, freq/1000000.0); 
  // Wir setzen den "Wrap"-Wert auf 20000. Bei 1µs pro Tick dauert ein Zyklus 20000µs = 20ms = 50Hz.
  pwm_set_wrap(slice_num, 20000);

  // Aktiviere den PWM-Slice
  pwm_set_enabled(slice_num, true);

  printf("Servo-Steuerung gestartet. Der Motor wird sich bewegen.\n");

  // Endlosschleife, um den Motor zu bewegen
  while (1) {
    if (gpio_get(BUT_LO) == 0)
    {
      printf("Bewege zu 0 Grad...\n");
      pwm_set_chan_level(slice_num, chan, get_pwm_wrap_for_angle(0));
      sleep_ms(2000); // Warte 2 Sekunden
    }

    if (gpio_get(BUT_MID) == 0)
    {
      printf("Bewege zu 90 Grad...\n");
      pwm_set_chan_level(slice_num, chan, get_pwm_wrap_for_angle(90));
      sleep_ms(2000); // Warte 2 Sekunden
    }

    if (gpio_get(BUT_HI) == 0)
    {    
      printf("Bewege zu 180 Grad...\n");
      pwm_set_chan_level(slice_num, chan, get_pwm_wrap_for_angle(180));
      sleep_ms(2000); // Warte 2 Sekunden
    }

    if (gpio_get(BUT_FLOW) == 0)
    {    
      printf("Bewege von 0 bis 180 Grad... hin und zurück\n");
      for (int i=0; i<=180; i+=10)
      {  
        pwm_set_chan_level(slice_num, chan, get_pwm_wrap_for_angle(i));
        sleep_ms(100);
      }
      sleep_ms(500);
      for (int i=180; i>=0; i-=10)
      {  
        pwm_set_chan_level(slice_num, chan, get_pwm_wrap_for_angle(i));
        sleep_ms(100);
      }

      sleep_ms(2000); // Warte 2 Sekunden
    }
  }

  return 0;
}