#include <stdint.h>
#include "unicore-mx/stm32/gpio.h"
#include "unicore-mx/stm32/rcc.h"
#include "system.h"
#include "led.h"
#include "pinout.h"
#include "settings.h"
#include "pot.h"

extern volatile uint32_t jiffies;



static int pot_cs[3] = { 8, 7, 6 }; // PC6, PC7, PC8

#define POT0 GPIO8  // C8
#define POT1 GPIO7  // C7
#define POTM GPIO6  // C6
#define POTINC GPIO14 // D14
#define POTUD  GPIO15 // D15

#define POT_CS_PINS (POT0 | POT1 | POTM)
#define POT_CTRL_PINS (POTINC | POTUD)

static int HWLevel[3] = {0, 0, 0};
static uint32_t Pots[NUM_POTS] = { POT0, POT1, POTM };


static void p_offset(int p, int offset)
{
    uint32_t cs;
    int i;
    int sign = 0;
    int u_off = offset;
    volatile uint32_t now;
    if((p < 0) || (p > NUM_POTS))
        return;
    cs = Pots[p];
    if (offset < 0) {
        sign = 1;
        u_off = 0 - offset;
    }
    /* U/!D setting */
    if (sign)
        gpio_clear(GPIOD, POTUD);
    else
        gpio_set(GPIOD, POTUD);

    for (i = 0; i < u_off; i++) {

        /* /CS on */
        gpio_clear(GPIOC, cs);
        DMB();
        /* /INC on */
        gpio_clear(GPIOD, POTINC);
        DMB();
        /* /INC off */
        gpio_set(GPIOD, POTINC);
        /* /CS off first (no store) */
        gpio_set(GPIOC,cs);
        //WFI();
        for (int j = 0; j < 1000; j++)
            ;;
    }

    HWLevel[p] += offset;
    if (HWLevel[p] > 100)
        HWLevel[p] = 100;
    if (HWLevel[p] < 0)
        HWLevel[p] = 0;
}

void pot_offset(int p, int offset)
{
    if ((HWLevel[p] + offset) > 100) {
        offset = 100 - HWLevel[p];
    }

    if ((HWLevel[p] + offset) < 0) {
        offset = 0 - HWLevel[p];
    }
    
    if (offset == 0)
        return;

    p_offset(p, offset);
}

void pot_init(void)
{
    int i;
    rcc_periph_clock_enable(RCC_GPIOC);
    rcc_periph_clock_enable(RCC_GPIOD);

    gpio_mode_setup(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_PULLUP, POT_CS_PINS);
    gpio_set_output_options(GPIOC, GPIO_OTYPE_OD, GPIO_OSPEED_100MHZ, POT_CS_PINS); 
    gpio_set(GPIOC, POT_CS_PINS);

    gpio_mode_setup(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, POT_CTRL_PINS);
    gpio_set_output_options(GPIOD, GPIO_OTYPE_OD, GPIO_OSPEED_100MHZ, POT_CTRL_PINS); 
    gpio_set(GPIOD, POT_CTRL_PINS);

    for (i = 0; i < NUM_POTS; i++)
        p_offset(i, -100);
    pot_set(0,100);
    pot_set(1,100);

}

void pot_sync(int p)
{
    pot_set(p, Settings->levels[p]);
}


int pot_get(int p)
{
    return HWLevel[p];
}

void pot_set(int p, int val)
{
    int old = HWLevel[p];
    int new = val;
    int off;


    if (val < 0)
        val = 0;
    if (val > 100)
        val = 100;

    Settings->levels[p] = val;

    off = val - old;
    if (off == 0)
        return;
    pot_offset(p, off);
}