/*
 * Copyright (c) 2021 Mathema GmbH
 * SPDX-License-Identifier: BSD-3-Clause
 * https://opensource.org/licenses/BSD-3-Clause
 * Created: Günter Woigk
 */

#include <stdio.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/pio.h"
#include "XY2-100.pio.h"


const uint32_t MAIN_CLOCK = 125 * 1000000;

namespace xy2 {

enum XY100_PIN : uint               // GPIO pins to use
{
    XY100_PIN_CLK = 8,
    XY100_PIN_CLK_NEG,
    XY100_PIN_SYNC,
    XY100_PIN_SYNC_NEG,
    XY100_PIN_X = 12,
    XY100_PIN_X_NEG,
    XY100_PIN_Y,
    XY100_PIN_Y_NEG,
    XY100_PIN_SYNC_XY = 17
};

static const PIO pio = pio0;        // PIO to use
static const uint sm_clk = 1;       // State Machines in the PIO
static const uint sm_x = 2;
static const uint sm_y = 3;


void init_clock_and_sync ()
{
    // setup the "clock+sync" program on state machine 'sm_clk' in pio block 'pio'
    // signals are sent to pins 'XY100_PIN_CLK' to 'XY100_PIN_CLK+3'

    const uint pin_base = XY100_PIN_CLK;

    // set initial pin states in PIO:
    // note: this is for the whole PIO, state machine 'sm_clk' is used to *do* this!
    pio_sm_set_pins_with_mask(pio, sm_clk, 0b1111u, 0b1111u << pin_base);

    // set i/o direction for pins in PIO:
    // note: this is for the whole PIO, state machine 'sm_clk' is used to *do* this!
    pio_sm_set_consecutive_pindirs(pio, sm_clk, pin_base, 4, true/*output*/);

    // tell gpio pins to use pio output: (=> gpio_set_function)
    for(uint i=0;i<4;i++) pio_gpio_init(pio, pin_base+i);

    // load the program into PIO memory:
    uint offset = pio_add_program(pio, &XY2_100_clock_program);

    // configure the state machine:
    // 'XY2_100_clock_sm_clock' and 'XY2_100_clock_offset_start' are defined in the pio program
    pio_sm_config c = XY2_100_clock_program_get_default_config(offset);
    sm_config_set_set_pins(&c, pin_base, 4);    // pins to use in SET instr
    sm_config_set_clkdiv(&c, float(MAIN_CLOCK) / XY2_100_clock_sm_clock);
    pio_sm_init(pio, sm_clk, offset + XY2_100_clock_offset_start, &c); 
}

void init_x_and_y_data ()
{
    // setup x and y "data" programs on state machine 'sm_x' and 'sm_y' in pio block 'pio'
    // data signals are sent to pins 'XY100_PIN_X' to 'XY100_PIN_X+3'
    // pin 'XY100_PIN_SYNC_XY' is used to synchronize the decision of the x and y state machines 
    // for 'pull next point' vs. 'repeat last point'

    const uint data_pins = XY100_PIN_X;
    const uint sync_pin = XY100_PIN_SYNC_XY;

    // set initial pin states in PIO:
    // note: this is for the whole PIO, state machine 'sm_clk' is used to *do* this!
    pio_sm_set_pins_with_mask(pio, sm_x, 0b1111u, 0b1111u << data_pins);
    pio_sm_set_pins_with_mask(pio, sm_x, 0b1u, 0b1u << sync_pin);

    // set i/o direction for pins in PIO:
    // note: this is for the whole PIO, state machine 'sm_clk' is used to *do* this!
    pio_sm_set_consecutive_pindirs(pio, sm_x, data_pins, 4, true/*output*/);
    pio_sm_set_consecutive_pindirs(pio, sm_x, sync_pin, 1, true/*output*/);

    // tell gpio pins to use pio output: (=> gpio_set_function)
    for(uint i=0;i<4;i++) pio_gpio_init(pio, data_pins+i);
    pio_gpio_init(pio, sync_pin);

    // load the program into PIO memory:
    uint offset = pio_add_program(pio, &XY2_100_data_program);

    // configure the state machines:
    // 'XY2_100_data_sm_clock' and 'XY2_100_data_offset_start' are defined in the pio program
    pio_sm_config c = XY2_100_data_program_get_default_config(offset);
    sm_config_set_clkdiv(&c, float(MAIN_CLOCK) / XY2_100_data_sm_clock);
    sm_config_set_out_shift(&c, false/*shift left not right*/, false /*!autopull*/, 32 /*pull threshold bits*/);
    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
    sm_config_set_mov_status(&c, STATUS_TX_LESSTHAN, 1);    
    sm_config_set_out_pins(&c, sync_pin, 1);      // pins to use in OUT instr and MOV dest.  
    sm_config_set_in_pins (&c, sync_pin);         // pins to use in IN instr  and MOV source
    sm_config_set_sideset_pins(&c, data_pins+0);  // pins for 'side setting' of x+ and x- pins
    pio_sm_init(pio, sm_x, offset + XY2_100_data_offset_start, &c); // configure state machine 'sm_x'

    sm_config_set_sideset_pins(&c, data_pins+2);  // pins for 'side setting' of y+ and y- pins
    pio_sm_init(pio, sm_y, offset + XY2_100_data_offset_start, &c); // configure state machine 'sm_y'
}

void start ()
{
    // stop and start all 3 state machines in sync

    pio_sm_set_enabled(pio, sm_clk, false);
    pio_sm_set_enabled(pio, sm_x, false);
    pio_sm_set_enabled(pio, sm_y, false);

    pio_sm_clear_fifos(pio, sm_x);
    pio_sm_clear_fifos(pio, sm_y);

    // Enable multiple PIO state machines synchronizing their clock dividers
    pio_enable_sm_mask_in_sync(pio, (1<<sm_clk)+(1<<sm_x)+(1<<sm_y));
}

void send_data_blocking (float x, float y)
{
    // DEMO image: send on data set to pio:

    while (pio_sm_is_tx_fifo_full(pio,sm_y)) {} 

    pio_sm_put(pio, sm_x, 0x8000 + int(x));
    pio_sm_put(pio, sm_y, 0x8000 + int(y));
}

} // namespace


constexpr float pi = 3.14159265359f;
float rad(float deg) { return deg*pi/180; }

int main() 
{
    stdio_init_all();
    printf("PIO example: XY2-100 Laserscanner Interface\n");

    xy2::init_clock_and_sync();
    xy2::init_x_and_y_data();
    xy2::start();

    const float size = 0x10000/4;    // use 1/2 scan field
    const float dx   = rad(2.0f/3);
    const float dy   = rad(2.0f/7.01);
    float x = rad(0);
    float y = rad(90);

    // show Lissajous figure:
    while (true) 
    {
        x += dx; if (x >= pi) x -= 2*pi;
        y += dy; if (y >= pi) y -= 2*pi;
        xy2::send_data_blocking(sin(x)*size,cos(y)*size);
    }
}