#include "aufgaben.h"

void aufgabe_servo(void)
{
    /* Definieren des Magnet-Buffers */
    float MagBuffer[3] = { 0 };
    float XWert, YWert, ZWert;


    //hier werden alle notwendigen Variablen definiert
    double Radiant;
    double Kurs;


    /* solange der UserButton nicht gedrückt ist bzw. der ResetButton betätigt wurde */
    while (UserButtonPressed == 0)
    {

        /* Daten vom Magnerometer empfangen*/

        /* Kompass und Pulsweitenmodulation (Frequenz, DutyCycle) konfigurieren */
        KompassAccelConfig();
        Config_PWM(55, 800);
        EmpfangeKompassDaten(MagBuffer);
        XWert = MagBuffer[0];
        YWert = MagBuffer[1];


            while (DataReady != 5)                  //Warten bis der Sensor 5 Datensätze geliefert hat.
            {


            }
            DataReady = 0;

            //Bogenmaßberechnung
            Radiant = atan( YWert / XWert);
            Kurs = Radiant * 360 / PI;
            //Servo




    }
}















void set_PWM_DC(uint32_t dc) {
    TIM4->CCR4 = dc;
}

// PWM1. TIM4, PD15 (PD14, PD13, PD12),
void Config_PWM(uint16_t freq, uint32_t pulse) {
    /* AHB peripheral clock für Port D aktivieren */
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOD, ENABLE);

    /*GPIO Pins konfigurieren */
    GPIO_InitTypeDef GPIO_InitStruct;
    //GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_DOWN;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_Init(GPIOD, &GPIO_InitStruct);

    /* Alternate function 2 für die konfigurierten Pins aktivieren */
    //  GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_2 );
    //  GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_2 );
    //  GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_2 );
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_2);

    uint16_t prescaler = (uint16_t)((RCC_Clocks.HCLK_Frequency / 1000000) - 1); // tIM4 auf 1 MHz Tastrate setzen, (1us)

    uint32_t pwm_period = 1000000 / freq; // Zum festlegen der PWM-Frequenz

    // TIM4 Peripherie aktivieren
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

    // Konfiguration von TIM4
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Prescaler = prescaler;
    TIM_TimeBaseStructure.TIM_Period = pwm_period - 1;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

    // Initialisierung der Timerchannels
    TIM_OCInitTypeDef TIM_OCInitStructure;

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;

    TIM_OCInitStructure.TIM_Pulse = pulse; // Pulsbreite 0..pwm_period
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
    TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
    TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Set;

    // Konfiguration der 4 Channels
    // Channel 1, pin PD12
    //  TIM_OC1Init(TIM4, &TIM_OCInitStructure);
    //  TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable );
    //
    //  // Channel 2, pin PD13
    //  TIM_OC2Init(TIM4, &TIM_OCInitStructure);
    //  TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable );
    //
    //  // Channel 3, pin PD14
    //  TIM_OC3Init(TIM4, &TIM_OCInitStructure);
    //  TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable );

    // Channel 4, pin PD15
    TIM_OC4Init(TIM4, &TIM_OCInitStructure);
    TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);

    // Starup the timer
    TIM_ARRPreloadConfig(TIM4, DISABLE);
    TIM_CtrlPWMOutputs(TIM4, ENABLE);
    TIM_Cmd(TIM4, ENABLE);
}