#include <stdio.h>
#include <stm32f4xx.h>
#include <stm32f4xx_rcc.h>
#include <stm32f4xx_gpio.h>
#include <stm32f4xx_tim.h>

#include "misc.h"
#include "mpconfig.h"
#include "qstr.h"
#include "obj.h"
#include "servo.h"

// PWM
// TIM2 and TIM5 have CH1, CH2, CH3, CH4 on PA0-PA3 respectively
// they are both 32-bit counters
// 16-bit prescaler
// TIM2_CH3 also on PB10 (used below)
void servo_init(void) {
    // TIM2 clock enable
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

    // for PB10
    /*
    // GPIOB Configuration: TIM2_CH3 (PB10)
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    // Connect TIM2 pins to AF1
    GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_TIM2);
    */

    // for PA0, PA1, PA2, PA3
    {
        // GPIOA Configuration: TIM2_CH0, TIM2_CH1 (PA0, PA1)
        GPIO_InitTypeDef GPIO_InitStructure;
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
        GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
        GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
        GPIO_Init(GPIOA, &GPIO_InitStructure);

        // Connect TIM2 pins to AF1
        GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
        GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
        GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2);
        GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM2);
    }

    // Compute the prescaler value so TIM2 runs at 100kHz
    uint16_t PrescalerValue = (uint16_t) ((SystemCoreClock / 2) / 100000) - 1;

    // Time base configuration
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_TimeBaseStructure.TIM_Period = 2000; // timer cycles at 50Hz
    TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

    // PWM Mode configuration
    TIM_OCInitTypeDef TIM_OCInitStructure;
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 150; // units of 10us
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC1Init(TIM2, &TIM_OCInitStructure); // channel 1
    TIM_OC2Init(TIM2, &TIM_OCInitStructure); // channel 2
    TIM_OC3Init(TIM2, &TIM_OCInitStructure); // channel 3
    TIM_OC4Init(TIM2, &TIM_OCInitStructure); // channel 4

    // ?
    TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 1
    TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 2
    TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 3
    TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); // channel 4

    // ?
    TIM_ARRPreloadConfig(TIM2, ENABLE);

    // TIM2 enable counter
    TIM_Cmd(TIM2, ENABLE);
}

/******************************************************************************/
/* Micro Python bindings                                                      */

mp_obj_t pyb_servo_set(mp_obj_t port, mp_obj_t value) {
    int p = mp_obj_get_int(port);
    int v = mp_obj_get_int(value);
    if (v < 50) { v = 50; }
    if (v > 250) { v = 250; }
    switch (p) {
        case 1: TIM2->CCR1 = v; break;
        case 2: TIM2->CCR2 = v; break;
        case 3: TIM2->CCR3 = v; break;
        case 4: TIM2->CCR4 = v; break;
    }
    return mp_const_none;
}

mp_obj_t pyb_pwm_set(mp_obj_t period, mp_obj_t pulse) {
    int pe = mp_obj_get_int(period);
    int pu = mp_obj_get_int(pulse);
    TIM2->ARR = pe;
    TIM2->CCR3 = pu;
    return mp_const_none;
}

typedef struct _pyb_servo_obj_t {
    mp_obj_base_t base;
    uint servo_id;
} pyb_servo_obj_t;

static void servo_obj_print(void (*print)(void *env, const char *fmt, ...), void *env, mp_obj_t self_in, mp_print_kind_t kind) {
    pyb_servo_obj_t *self = self_in;
    print(env, "<Servo %lu>", self->servo_id);
}

static mp_obj_t servo_obj_angle(mp_obj_t self_in, mp_obj_t angle) {
    pyb_servo_obj_t *self = self_in;
    machine_int_t v = 152 + 85.0 * mp_obj_get_float(angle) / 90.0;
    if (v < 65) { v = 65; }
    if (v > 210) { v = 210; }
    switch (self->servo_id) {
        case 1: TIM2->CCR1 = v; break;
        case 2: TIM2->CCR2 = v; break;
        case 3: TIM2->CCR3 = v; break;
        case 4: TIM2->CCR4 = v; break;
    }
    return mp_const_none;
}

static MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_angle_obj, servo_obj_angle);

static const mp_method_t servo_methods[] = {
    { "angle", &servo_obj_angle_obj },
    { NULL, NULL },
};

static const mp_obj_type_t servo_obj_type = {
    { &mp_type_type },
    .name = MP_QSTR_Servo,
    .print = servo_obj_print,
    .methods = servo_methods,
};

mp_obj_t pyb_Servo(mp_obj_t servo_id) {
    pyb_servo_obj_t *o = m_new_obj(pyb_servo_obj_t);
    o->base.type = &servo_obj_type;
    o->servo_id = mp_obj_get_int(servo_id);
    return o;
}