#include <avr/io.h>
#include <util/delay.h>
#include <homelab/pin.h>
#include <homelab/module/motors.h>
#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);
 
    }
}