//#include "fixmath.h"
#include <hardware/pwm.h>

#define KILO      _u(1000)
#define PWM_FREQ  _u(50)
#define WRAP      _u(20000)
#define MICRO     1e-6

typedef struct {
  uint16_t  freq;
  float     clkdiv;
  uint8_t   us_res;
  uint16_t  wrap;
} servo_cfg_t;

typedef struct {
    uint16_t us_min;
    uint16_t us_max;
    uint16_t degree_range;

} servo_limit_t;

typedef struct {
  uint8_t       pin;
  int8_t        num_slice;
  uint16_t      level;
  bool          is_defined;
  servo_cfg_t   *cfg;
  servo_limit_t *limit;
} servo_pin_t;

void servo_do(uint8_t pin
            , servo_pin_t *spin
            , servo_cfg_t *cfg
            , servo_limit_t *limit) {

    spin->num_slice = -1;  // -1 slice unconfig
    spin->pin = pin;
    spin->cfg = cfg;
    spin->limit = limit;
    spin->is_defined = true;
    if (cfg->freq == 0 || cfg->wrap == 0 || cfg->clkdiv == 0
          || limit->us_min == 0 || limit->us_max == 0 || limit->degree_range == 0) {
            spin->is_defined = false;
    }

        
}

void servo_attach(servo_pin_t *spin) {
    
    if (gpio_get_function(spin->pin) == GPIO_FUNC_PWM || !spin->is_defined) 
        return; // gpio is alredy attacched
        
    spin->num_slice = pwm_gpio_to_slice_num(spin->pin);
    
    if ( !(pwm_hw->slice[spin->num_slice].top == spin->cfg->wrap) ) {
        pwm_config c = pwm_get_default_config();
        pwm_config_set_wrap(&c, spin->cfg->wrap);
        pwm_config_set_clkdiv(&c, spin->cfg->clkdiv);
        pwm_init(spin->num_slice, &c, true);
    }
    gpio_set_function(spin->pin, GPIO_FUNC_PWM);
    pwm_set_chan_level(spin->num_slice, spin->pin % 2, spin->limit->us_min / 2);
    
}

void servo_set_angle(servo_pin_t *spin, uint16_t angle) {
    uint us = (angle / (float)spin->limit->degree_range) * 
               (spin->limit->us_max - spin->limit->us_min) + spin->limit->us_min;
    servo_set_pos_us(spin, us);
}

void servo_set_pos_us(servo_pin_t *spin, uint16_t us) {
    if (spin->is_defined != true)
        return;
    if (us >= spin->limit->us_min && us <= spin->limit->us_max) {
        spin->level = us / spin->cfg->us_res;
        pwm_set_chan_level(spin->num_slice, spin->pin % 2, spin->level);
    }
    
}

int8_t servo_cfg(servo_cfg_t *scfg, uint16_t freq, uint8_t us_res) {
    const uint32_t f_khz = 125000;
    scfg->wrap = (1.0/freq) * (1000000/us_res);
    scfg->freq    =   freq;
    scfg->us_res  =   us_res;
    scfg->clkdiv  =   (float)f_khz * (float)KILO / (freq * scfg->wrap);
    if (scfg->clkdiv > 256.f) {
        scfg->clkdiv = 256.f;
        return -1;
    }
    return 0;
                      
}

void servo_set_limit(servo_limit_t *limit
                    , uint16_t us_min
                    , uint16_t us_max 
                    , uint16_t degree) {
    limit->us_min = us_min;
    limit->us_max = us_max;
    limit->degree_range = degree;
}





static float clkdiv;
static uint g_min;
static uint g_max;
static uint min_us = 500;
static uint max_us = 2500;
static uint us_per_unit = 0;

void set_pos(uint16_t angle) {
  /*uint val = (uint)fix16_to_int(
        fix16_mul(
            fix16_div(fix16_from_int(angle), fix16_from_int(180)),
            fix16_from_int(g_max - g_min)
        )
    ) + g_min;*/
    uint val = (angle / 180.0) * (g_max - g_min) + g_min;
    Serial1.printf("level: %u\n", val);
}

void setup() {
    
  Serial1.begin(115200);
  Serial1.println("Raspberry Pi Pico servo Example");
  uint8_t src = CLOCKS_FC0_SRC_VALUE_PLL_SYS_CLKSRC_PRIMARY;
  Serial1.printf("1e-6: %f\n", MICRO);
  //(float)frequency_count_khz(src) dovrebbe resistuire 125000
  // invece ancora macina dopo 4 secondi al 3%
  float f_khz = 125000;
  clkdiv = (float)f_khz * (float)KILO / (PWM_FREQ * WRAP);

  Serial1.printf("clkdiv: %f\n", clkdiv);
  float pwm_freq = f_khz * (float)KILO / clkdiv;
  Serial1.printf("pwm_freq; %f\n", pwm_freq);
  Serial1.printf("1.f: %f\n", 1.f);
  //uint32_t us_per_unit = (float)1.0 / ((PWM_FREQ * WRAP) / MICRO);
  uint32_t us_per_unit = (1.0 / pwm_freq) * 1e6;
  Serial1.printf("us_per_unit: %u\n", us_per_unit);
  g_min = min_us / us_per_unit;
  g_max = max_us / us_per_unit;
  set_pos(180);
  set_pos(179);
  set_pos(178);

  servo_cfg_t servo_std;
  servo_limit_t servo_limit_std;
  servo_pin_t servo0;

  servo_cfg(&servo_std, 75, 1);
  servo_set_limit(&servo_limit_std, 500, 2500, 270);
  servo_do(12, &servo0, &servo_std, &servo_limit_std);
  servo_attach(&servo0);
  Serial1.printf("clkdiv: %f\n", servo_std.clkdiv);
  Serial1.printf("wrap: %u\n", servo_std.wrap);
  servo_set_angle(&servo0, 270);
  Serial1.printf("270° -> level: %u\n", servo0.level);
  servo_set_angle(&servo0, 135);
  Serial1.printf("135° -> level: %u\n", servo0.level);
  servo_set_angle(&servo0, 0);
  Serial1.printf("0° -> level: %u\n", servo0.level);
}


void loop() {
  delay(1);
  
}