/**
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "pico/stdlib.h"

int* get_curve(int len, int d0, int w) {
    int *ptr = NULL;
    ptr = (int*) malloc(len * sizeof(int));
    if (ptr == NULL) return NULL;
    else {
        ptr[0] = d0;
        ptr[1] = (int) (d0 + w)/2;
        ptr[2] = (int) (d0 + ptr[1]+ w)/3;
        for (int i = 3; i<len; i++) ptr[i] = (int)(ptr[i-3] + ptr[i-2] + ptr[i-1]+ w)/4;
        return ptr;
    }
}

void move_speed(int pin_num, int d)
{
    gpio_put(pin_num, 1);
    sleep_us(d);
    gpio_put(pin_num, 0);
    sleep_us(d);
}

int main() {
 gpio_init(4);
    gpio_set_dir(4, GPIO_OUT);
    int d0 = 10000;
    int w = 1000;
    int *d = NULL;
    d = (int*) malloc(500 * sizeof(int));
    d = get_curve(500, d0, w);
    for(int i=0; i<500; i++)
    {
        move_speed(4, d[i]);
        printf("%d\n",d[i]);
    } 
    while(1) move_speed(4,1000);
}