
#include 
#include 
#include 
#include 
#include "homelab/timer.h"

pin led_red    = PIN(C, 5);

static pin xunipolar_pins[2][4] =
{
    {
        PIN(E, 2), PIN(E, 3),
        PIN(E, 4), PIN(E, 5)
    },
    {
        PIN(E, 0), PIN(E, 1),
        PIN(E, 6), PIN(E, 7)
    }
};

void xunipolar_init(unsigned char xindex)
{   
   unsigned char xi;

     for (xi = 0; xi < 4; xi++)
    {
        // Set control pins as output
        pin_setup_output(xunipolar_pins[xindex][xi]);
        
        // Initially stop
        pin_clear(xunipolar_pins[xindex][xi]);
    }

}


void xunipolar_halfstep(unsigned char index, signed char direction, unsigned short num_steps, unsigned char speed)
{
    unsigned short i;
    unsigned char state1 = 0, state2 = 1;
    unsigned char pattern;
    unsigned char timeout;

    // Ensure +-1
    direction = ((direction < 0) ? -1 : +1);

    // Ensure speed <= 100
    speed = (speed > 100 ? 100 : speed);

    // Complete steps
    for (i = 0; i < num_steps; i++)
    {       
        state1 += direction;
        state2 += direction;

        // Make pattern
        pattern =
            (1 << ((state1 % 8) >> 1)) |
            (1 << ((state2 % 8) >> 1));
        
        // Set output
        PIN_ARRAY_OF_4_MASK(xunipolar_pins[index], pattern & 0x0F);

        // Wait for the motor to complete the step      
        timeout = speed;
        
        while (timeout-- > 0)
        {           
            _delay_ms(1);
        }
    }

    // Turn off motor
    PIN_ARRAY_OF_4_MASK(xunipolar_pins[index], 0x00);
}


///////////////// Main function //////////////////////////////////
int main(void){
    
   
    pin_setup_output(led_red);
    
    xunipolar_init(0); 
    xunipolar_init(1);

    bipolar_init();

    
    while(1){

        pin_toggle(led_red);
        
        //bipolar_halfstep(-1, 100, 10);
        //bipolar_halfstep(1, 100, 5);

        xunipolar_halfstep(0, 1, 1000, 3);
        xunipolar_halfstep(0, -1, 1000, 3);


        //see kanal ei tööta miskipärast
        //xunipolar_halfstep(1, 1, 1000, 50);

    }
}