#include <stdio.h>
#include <unistd.h>
#include <system.h>
#include <io.h>

int halfStepCounterClockwiseValues[9] = {0b0001,0b0011,0b0010,0b0110,0b0100,0b1100,0b1000,0b1001};
int halfStepClockwiseValues[9] = {0b1000,0b1100,0b0100,0b0110,0b0010,0b0011,0b0001,0b1001};
int velocityValues[10] = {0b0000000001,0b0000000011,0b0000000111,0b0000001111,0b0000011111,0b0000111111,0b0001111111,0b0011111111,0b0111111111,0b1111111111};

void triggerHalfStepping(unsigned int step, unsigned int direction){
    unsigned int halfStepValue;

    switch (direction) {
        case 1:
            IOWR(PIO_GPIO_BASE, 0, halfStepClockwiseValues[step]); break;
        case 2:
            IOWR(PIO_GPIO_BASE, 0, halfStepCounterClockwiseValues[step]); break;
        default:
            IOWR(PIO_GPIO_BASE, 0, halfStepClockwiseValues[step]); break;
    }
    printf("%d", halfStepValue);
}

void indicateVelocity(unsigned int velocity){
    IOWR(PIO_LED_BASE, 0, velocityValues[velocity]);
}

int main(void)
{
    unsigned int switchValue=0, keySwitchValue=0;
    unsigned int stepValue,velocityValue=1;
    while(1)
    {

        switchValue=IORD(PIO_SW_BASE,0x00);
        keySwitchValue=IORD(PIO_KEY_BASE,0x00);
        IOWR(PIO_LED_BASE,0x00,switchValue);

        if((keySwitchValue&0x02)!=0 && velocityValue<11) {
            velocityValue++;
        }
        if((keySwitchValue&0x01)!=0 && velocityValue>1) {
            velocityValue--;
        }
        indicateVelocity(velocityValue);

        if((switchValue&0x01)!=0)
        {
            for(stepValue=1;stepValue<9;stepValue++)
            {
                triggerHalfStepping(stepValue,1);
                usleep(-(5000*velocityValue)+100000);
            }
        }
        else
        {
            for(stepValue=1;stepValue<9;stepValue++)
            {
                triggerHalfStepping(stepValue,2);
                usleep(-(5000*velocityValue)+100000);
            }
        }
    }
    return 0;
}