#include <stdint.h>

struct Message {
        uint32_t id;
        uint32_t timestamp;
        int32_t control_signal;
        int32_t error_signal;
        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 UserControlVars {
        // set by pru before halting
        char const *abort_file;
        int abort_line;
        // read-pointer updated by python
        uint16_t ridx;
        uint16_t buffer;
        // Shared variables for PRU
        // motor status
        uint16_t target_pos;
        uint16_t current_pos;
        uint32_t speed;
        bool halt_requested;
        bool moving;
};


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

#define ddr   (*(struct DDRLayout volatile *)0x10000)
#define shmem (*(struct UserControlVars volatile *)0x10100)


// define some r30 bits
#define CW  (1 << 3)
#define CCW (1 << 7)

volatile register uint32_t __R30;

#define delay_us(n) __delay_cycles((n) * 200u)
#define delay_ms(n) delay_us((n) * 1000u)

int main() {
    int32_t pos = shmem.current_pos;
    __R30 = 0;
    delay_us(1);
    while ( !shmem.halt_requested ) {
        uint32_t speed = shmem.speed;
        int32_t delta = shmem.target_pos - pos;
        if ( delta == 0 ) {
            shmem.moving = false;
            continue;
        }
        if ( delta < 0 ) {
            --pos;
            __R30 |= CW;
            __R30 |= CCW;
            delta *= -1;
            shmem.moving = true;
        } else {
            ++pos;
            __R30 |= CCW;
            __R30 |= CW;
            shmem.moving = true;
        }
        shmem.current_pos = pos;
            switch(speed){
                case 1:
                    delay_us(1);
                    __R30 = 0;
                    delay_us(1);
                    break;
                case 2:
                    delay_us(2);
                    __R30 = 0;
                    delay_us(2);
                    break;
                case 3:
                    delay_us(3);
                    __R30 = 0;
                    delay_us(3);
                    break;
                case 4:
                    delay_us(4);
                    __R30 = 0;
                    delay_us(4);
                    break;
                case 5:
                    delay_us(5);
                    __R30 = 0;
                    delay_us(5);
                    break;
                case 10:
                    delay_us(100);
                    __R30 = 0;
                    delay_us(100);
                    break;
                case 20:
                    delay_us(200);
                    __R30 = 0;
                    delay_us(200);
                    break;
                case 30:
                    delay_us(300);
                    __R30 = 0;
                    delay_us(300);
                    break;
                case 40:
                    delay_us(400);
                    __R30 = 0;
                    delay_us(400);
                    break;
                case 50:
                    delay_us(500);
                    __R30 = 0;
                    delay_us(500);
                    break;
                case 60:
                    delay_us(600);
                    __R30 = 0;
                    delay_us(600);
                    break;
                case 70:
                    delay_us(700);
                    __R30 = 0;
                    delay_us(700);
                    break;
                case 80:
                    delay_us(800);
                    __R30 = 0;
                    delay_us(800);
                    break;
                case 90:
                    delay_us(900);
                    __R30 = 0;
                    delay_us(900);
                    break;
                case 100:
                    delay_us(1000);
                    __R30 = 0;
                    delay_us(1000);
                    break;
                case 999:
                    delay_us(1000000);
                    __R30 = 0;
                    delay_us(1000000);
                    break;
                default:
                    delay_us(3000);
                    __R30 = 0;
                    delay_us(3000);
            }

    }
    __halt();
}