#include <stdlib.h>
#include <stdint.h>
#include <stddef.h>
#include "pru_uart.h"
#include "pru_ecap.h"
#include "sys_gpio.h"

// define PWM inputs for motor controll
#define EPWM1A  ( *(uint16_t volatile *)0x48302212 )
#define EPWM1B  ( *(uint16_t volatile *)0x48302214 )

// encoder position in other PRU core's memory
#define position_var ((uint32_t const volatile *)0x00002000)

struct Message {
    uint32_t id;
    uint32_t timestamp;
    int32_t control_signal;
    int32_t magnet_voltage;
    int16_t force;
    int16_t cycle_count;
    uint16_t counter;
    uint16_t signal_length;
};

// layout of shared ddr3 memory (filled in by loader)
struct DDRLayout {
    Message volatile *msgbuf;
    uint16_t num_msgs;
    uint16_t msg_size;
};

struct SharedVars {
    // set by pru before halting
    char const *abort_file;
    int abort_line;
    uint32_t time;
    // read-pointer updated by python
    uint16_t ridx;
    uint16_t cycle_limit;
    uint16_t time_limit;
    uint16_t signal_length;
    uint16_t signal[2000];
};

far struct DDRLayout ddr __attribute__((location(0x10000))) = {};
far struct SharedVars volatile shmem __attribute__((location(0x10100))) = {};

static inline uint32_t timestamp_cycles()
{
    return CT_ECAP.TSCTR;
}

// for easier debugging, record where in the source code we halted
__attribute__((noreturn))
void abort_at( char const *file, int line )
{
    shmem.abort_file = file;
    shmem.abort_line = line;
    for(;;) __halt();
}

static inline void assert_at( bool cond, char const *file, int line )
{
    if( ! cond )
        abort_at( file, line );
}

#define abort() abort_at( __FILE__, __LINE__ )
#define assert(cond) assert_at( (cond), __FILE__, __LINE__ )

// local copy of write-pointer
static uint16_t widx = 0;

// global var for write-pointer is located right after message ringbuffer
#define ddr_msgbuf_end  ( ddr.msgbuf + ddr.num_msgs )
#define ddr_widx    ( *(uint16_t volatile *)ddr_msgbuf_end )

// receive byte from uart
static inline char uart_recv_byte()
{
    for(;;) {
        uint8_t lsr = CT_UART.LSR;
        if( lsr & 0x1e )
            abort();  // receive-error occurred
        if( lsr & 0x01 )
            return (char) CT_UART.RBR;
    }
}


// receive CR-terminated line from uart
static inline uint8_t uart_recv_line( char volatile msg[], uint8_t maxlen )
{
    uint8_t len = 0;

    for(;;) {
        char c = uart_recv_byte();
        if( c == '\r' )
            break;  // found end of line
        if( len == maxlen )
            abort();  // line does not fit in buffer
        msg[ len++ ] = c;
    }

    return len;
}


int16_t receive_measurement()
{
    // allocate buffer at fixed address for debugging convenience
    static char volatile msg[8] __attribute__((location(0x1f00)));

    // receive line from uart
    uint8_t len = uart_recv_line( msg, sizeof msg );

    // verify length and prefix
    if( len != 8 || msg[0] != 'N' || (msg[1] != '+' && msg[1] != '-'))
        abort();

    // parse the remainder as integer
    int16_t value = 0;
    uint8_t i;
    for( i = 2; i < len; i++ ) {
        if( msg[i] < '0' || msg[i] > '9' )
            abort();
        value = value * 10 + ( msg[i] - '0' );
    }
    if (msg[1] == '-')
            value *= -1;

    return value;
}


void initialize()
{
    // perform sanity-checking
    assert( 0x80000000 <= (uint32_t)ddr.msgbuf );
    assert( ddr.msgbuf < ddr_msgbuf_end );
    assert( ddr.msg_size == sizeof(Message) );

    assert( ddr_widx == widx );
    assert( shmem.ridx == widx );
}


static inline uint16_t next_idx( uint16_t idx )
{
    if( ++idx == ddr.num_msgs )
        idx = 0;
    return idx;
}


void send_message( uint32_t id, int16_t current_force,  int32_t input, int32_t output, uint16_t cycle,uint16_t counter, uint16_t signal_length)
{
    uint16_t next_widx = next_idx( widx );

    if( next_widx == shmem.ridx ) {
        // can't send message, ringbuffer is full
        abort();
    }

    Message volatile *msg = &ddr.msgbuf[ widx ];

    // fill in contents of message
    msg->id = id;
    msg->force = current_force;
    msg->timestamp = timestamp_cycles();
    msg->control_signal = input;
    msg->magnet_voltage = output;
    msg->cycle_count = cycle;
    msg->counter = counter;
    msg->signal_length = signal_length;
    // update write-pointer
    ddr_widx = widx = next_widx;
}

void main() {
        int32_t control_signal = 0;
    int32_t error[3];
        CT_UART.THR = 'S';
        CT_UART.THR = 'N';
        CT_UART.THR = '\r';
        initialize();

    uint32_t id =0;
        int16_t reference_force = receive_measurement(); // initil reading from load cell

    uint16_t counter = 0;
        gpio_set_one_low(&GPIO0,31);

    // PID parameers
    int32_t Kp = (int)(40 * (1 << 16)) ; // 20
        int32_t Ki = (int)(0.005 * (1 << 16)) ; // 0.5
        int32_t Kd = (int)(1 * (1 << 16)) ;// 0.4

        int32_t A[3] = {0};
    A[0] = Kp + Ki + Kd;
    A[1] = -Kp -2*Kd;
    A[2] = Kd;

    uint16_t cycle_count = 0;
    bool running = true;
    uint16_t last_cycle = shmem.cycle_limit;
    uint16_t time_limit = shmem.time_limit;
    uint16_t signal_length = shmem.signal_length;
    while (running) {
        if (cycle_count <= last_cycle)
        {

            int16_t force = receive_measurement();
            int32_t input_signal = shmem.signal[counter];

                EPWM1A = abs(input_signal);
            //report to python
            send_message( ++id,force, input_signal,control_signal,cycle_count,counter,signal_length);
            ++counter;
            // reset counter to enforce periodicy
            if (counter >= signal_length){
                counter = 0;
                ++cycle_count;
            }
        } else {

                EPWM1A = 0;

            int16_t force = receive_measurement();
            int32_t input_signal = shmem.signal[counter];
                int32_t output_signal = 0;
            send_message( ++id,force , input_signal,output_signal,cycle_count,counter,signal_length);
        }
    }
}