#include <al_smd.h>
#include <al_common.h>
#include <stdlib.h>
#include <util/delay.h>
#include <avr/io.h>
#include <avr/interrupt.h>

volatile uint8_t	smd_flags;
volatile int32_t	smd_curpos;
#ifdef SMD_LSW_DEPTH
volatile uint8_t	smd_lsw_steps;
#endif
int32_t			smd_oldpos,
			smd_newpos;
uint16_t		smd_speed,
			smd_ramp;


void smd_update_flags()
{
#ifdef SMD_PIN_BEG_LSW
#ifdef SMD_NEGATIVE_LSW
    if (!bit_is_set(SMD_PIN_BEG_LSW,SMD_BIT_BEG_LSW))
#else
    if (bit_is_set(SMD_PIN_BEG_LSW,SMD_BIT_BEG_LSW))
#endif
	smd_flags|=SMD_IS_ON_BEG_LSW;
    else
	smd_flags&=~SMD_IS_ON_BEG_LSW;
#endif

#ifdef SMD_PIN_END_LSW
#ifdef SMD_NEGATIVE_LSW
    if (!bit_is_set(SMD_PIN_END_LSW,SMD_BIT_END_LSW))
#else
    if (bit_is_set(SMD_PIN_END_LSW,SMD_BIT_END_LSW))
#endif
	smd_flags|=SMD_IS_ON_END_LSW;
    else
	smd_flags&=~SMD_IS_ON_END_LSW;
#endif
}


#if defined(SMD_MOVE_TIMER_0)
SIGNAL(SIG_OVERFLOW0)
#elif defined(SMD_MOVE_TIMER_1)
SIGNAL(SIG_OVERFLOW1)
#elif defined(SMD_MOVE_TIMER_2)
SIGNAL(SIG_OVERFLOW2)
#endif
{
    smd_update_flags();

    if (bit_is_set(SMD_PORT_CLK,SMD_BIT_CLK))
	cbi(SMD_PORT_CLK,SMD_BIT_CLK);
    else if (smd_curpos==smd_newpos)
	smd_stop();
    else
    {
	if (bit_is_set(SMD_PORT_DIR,SMD_BIT_DIR))
	{
	    if (smd_flags & SMD_IS_ON_END_LSW)
#ifdef SMD_LSW_DEPTH
		smd_lsw_steps++;
	    else
		smd_lsw_steps=0;

	    if (smd_lsw_steps>SMD_LSW_DEPTH)
#endif
		smd_stop();
	    else
	    {
		sbi(SMD_PORT_CLK,SMD_BIT_CLK);
		smd_curpos++;
	    }
	}	
	else
	{
	    if (smd_flags & SMD_IS_ON_BEG_LSW)
#ifdef SMD_LSW_DEPTH
	    	smd_lsw_steps++;
	    else
		smd_lsw_steps=0;

	    if (smd_lsw_steps>SMD_LSW_DEPTH)
#endif
		smd_stop();
	    else
	    {
		sbi(SMD_PORT_CLK,SMD_BIT_CLK);
		smd_curpos--;
	    }
	}
    }

    if (smd_flags & SMD_IS_RUNNING)
    {
	uint32_t	bd=abs(smd_curpos-smd_oldpos),
			ed=abs(smd_curpos-smd_newpos);
	uint32_t	t;

	if (smd_ramp==0 || (bd>=smd_ramp && ed>=smd_ramp))
	    t=smd_speed;
	else
	    t=(smd_speed*MIN(bd,ed))/smd_ramp;

	t=CLOCK_FREQ/SMD_MOVE_TIMER_DIV/t/2;

#ifdef SMD_MOVE_TIMER_1
	if (t<=1)
	    t=0xFFFF;
	else if (t>=0x10000)
	    t=0;
	else
	    t=0x10000-t;
#else
	if (t<=1)
	    t=0xFF;
	else if (t>=0x100)
	    t=0;
	else
	    t=0x100-t;
#endif

#if defined(SMD_MOVE_TIMER_0)
	TCNT0=t;
#elif defined(SMD_MOVE_TIMER_1)
	TCNT1H=HBYTE(t);
	TCNT1L=LBYTE(t);
#elif defined(SMD_MOVE_TIMER_2)
	TCNT2=t;
#endif
    }
}


void smd_init()
{
    uint8_t	b;

    cbi(SMD_PORT_CLK,SMD_BIT_CLK);
    sbi(SMD_DDR_CLK,SMD_BIT_CLK);
    cbi(SMD_PORT_DIR,SMD_BIT_DIR);
    sbi(SMD_DDR_DIR,SMD_BIT_DIR);

#ifdef SMD_PORT_RESET
    cbi(SMD_PORT_RESET,SMD_BIT_RESET);
    sbi(SMD_DDR_RESET,SMD_BIT_RESET);
    _delay_loop_2(SMD_RESET_DELAY_LOOPS);
    sbi(SMD_PORT_RESET,SMD_BIT_RESET);
#endif

#if defined(SMD_L297) && defined(SMD_PORT_HF)
    cbi(SMD_PORT_HF,SMD_BIT_HF);
    sbi(SMD_DDR_HF,SMD_BIT_HF);
#endif

#if defined(SMD_A3977) && defined(SMD_PORT_MS1)
    cbi(SMD_PORT_MS1,SMD_BIT_MS1);
    sbi(SMD_DDR_MS1,SMD_BIT_MS1);
#endif

#if defined(SMD_A3977) && defined(SMD_PORT_MS2)
    cbi(SMD_PORT_MS2,SMD_BIT_MS2);
    sbi(SMD_DDR_MS2,SMD_BIT_MS2);
#endif

#ifdef SMD_PORT_ENABLE
    sbi(SMD_PORT_ENABLE,SMD_BIT_ENABLE);
    sbi(SMD_DDR_ENABLE,SMD_BIT_ENABLE);
#endif

#ifdef SMD_PIN_BEG_LSW
    cbi(SMD_DDR_BEG_LSW,SMD_BIT_BEG_LSW);
#endif

#ifdef SMD_PIN_END_LSW
    cbi(SMD_DDR_END_LSW,SMD_BIT_END_LSW);
#endif

    smd_flags=0;
    smd_curpos=smd_oldpos=smd_newpos=0;
    smd_speed=100;
    smd_ramp=0;

#if defined(SMD_MOVE_TIMER_0)
#if SMD_MOVE_TIMER_DIV==1
    b=_BV(CS00);
#elif SMD_MOVE_TIMER_DIV==8
    b=_BV(CS01);
#elif SMD_MOVE_TIMER_DIV==64
    b=_BV(CS01) | _BV(CS00);
#elif SMD_MOVE_TIMER_DIV==256
    b=_BV(CS02);
#elif SMD_MOVE_TIMER_DIV==1024
    b=_BV(CS02) | _BV(CS00);
#else
#error Invalid value of SMD_MOVE_TIMER_DIV
#endif

#ifdef TCCR0B
    TCCR0B=b;
#else
    TCCR0=b;
#endif
#elif defined(SMD_MOVE_TIMER_1)

#if SMD_MOVE_TIMER_DIV==1
    b=_BV(CS10);
#elif SMD_MOVE_TIMER_DIV==8
    b=_BV(CS11);
#elif SMD_MOVE_TIMER_DIV==64
    b=_BV(CS11) | _BV(CS10);
#elif SMD_MOVE_TIMER_DIV==256
    b=_BV(CS12);
#elif SMD_MOVE_TIMER_DIV==1024
    b=_BV(CS12) | _BV(CS10);
#else
#error Invalid value of SMD_MOVE_TIMER_DIV
#endif

    TCCR1A=0;
    TCCR1B=b;
#elif defined(SMD_MOVE_TIMER_2)

#if SMD_MOVE_TIMER_DIV==1
    b=_BV(CS20);
#elif SMD_MOVE_TIMER_DIV==8
    b=_BV(CS21);
#elif SMD_MOVE_TIMER_DIV==32
    b=_BV(CS20) | _BV(CS21);
#elif SMD_MOVE_TIMER_DIV==64
    b=_BV(CS22);
#elif SMD_MOVE_TIMER_DIV==128
    b=_BV(CS22) | _BV(CS20);
#elif SMD_MOVE_TIMER_DIV==256
    b=_BV(CS22) | _BV(CS21);
#elif SMD_MOVE_TIMER_DIV==1024
    b=_BV(CS22) | _BV(CS21) | _BV(CS20);
#else
#error Invalid value of SMD_MOVE_TIMER_DIV
#endif

#ifdef TCCR2B
    TCCR2B=b;
#else
    TCCR2=b;
#endif
#endif

#if defined(SMD_CURCTRL_OC1A)
    TCCR1A=_BV(COM1A1) | _BV(WGM10);				// Fast PWM, 8bit, positive OC1A
    TCCR1B=_BV(WGM12) | _BV(CS10);				// No prescale
    sbi(DDRB,DDB1);
#elif defined(SMD_CURCTRL_OC1B)
    TCCR1A=_BV(COM1B1) | _BV(WGM10);				// Fast PWM, 8bit, positive OC1B
    TCCR1B=_BV(WGM12) | _BV(CS10);				// No prescale
    sbi(DDRB,DDB2);
#elif defined(SMD_CURCTRL_OC2)
    TCCR2=_BV(WGM21) | _BV(WGM20) | _BV(COM21) | _BV(CS20)	// Fast PWM, positive, no prescale
    sbi(DDRB,DDB3);
#endif

    smd_set_step(SMD_STEP_FULL);
    smd_set_current(100);
}


uint8_t smd_get_flags()
{
    if (!(smd_flags & SMD_IS_RUNNING))
	smd_update_flags();

    return smd_flags;
}


int32_t smd_get_position()
{
    return smd_curpos;
}


uint16_t smd_get_speed()
{
    return smd_speed;
}


uint16_t smd_get_ramp()
{
    return smd_ramp;
}


void smd_set_speed(uint16_t speed_)
{
    smd_speed=speed_;
}


void smd_set_ramp(uint16_t ramp_)
{
    smd_ramp=ramp_;
}


void smd_set_step(uint8_t step_)
{
#if defined(SMD_A3977) && defined(SMD_PORT_MS1) && defined(SMD_PORT_MS2)
    switch (step_)
    {
    case SMD_STEP_HALF:
	sbi(SMD_PORT_MS1,SMD_BIT_MS1);
	cbi(SMD_PORT_MS2,SMD_BIT_MS2);
	break;

    case SMD_STEP_QUATER:
	cbi(SMD_PORT_MS1,SMD_BIT_MS1);
	sbi(SMD_PORT_MS2,SMD_BIT_MS2);
	break;

    case SMD_STEP_EIGHTH:
	sbi(SMD_PORT_MS1,SMD_BIT_MS1);
	sbi(SMD_PORT_MS2,SMD_BIT_MS2);
	break;

    default:
	cbi(SMD_PORT_MS1,SMD_BIT_MS1);
	cbi(SMD_PORT_MS2,SMD_BIT_MS2);
    }

#elif defined(SMD_L297) && defined(SMD_PORT_HF)
    cbi(SMD_PORT_RESET,SMD_BIT_RESET);

    if (step_==SMD_STEP_HALF)
	sbi(SMD_PORT_HF,SMD_BIT_HF);
    else
	cbi(SMD_PORT_HF,SMD_BIT_HF); 

    _delay_loop_2(SMD_RESET_DELAY_LOOPS);
    sbi(SMD_PORT_RESET,SMD_BIT_RESET);
#endif
}


void smd_reset()
{
    if (smd_flags & SMD_IS_RUNNING)
	return;

    smd_curpos=0;
}


void smd_move(int32_t position_)
{
    if (position_==smd_curpos)
	return;

    if (smd_flags & SMD_IS_RUNNING)
	smd_stop();

    smd_oldpos=smd_curpos;
    smd_newpos=position_;

#ifdef SMD_LSW_DEPTH
    smd_lsw_steps=0;
#endif

    if (smd_newpos>smd_oldpos)
	sbi(SMD_PORT_DIR,SMD_BIT_DIR);
    else
	cbi(SMD_PORT_DIR,SMD_BIT_DIR);

    smd_flags|=SMD_IS_RUNNING;

#if defined(SMD_MOVE_TIMER_0)
    TCNT0=0xFF;
#ifdef TIMSK0
    sbi(TIMSK0,TOIE0);
#else
    sbi(TIMSK,TOIE0);
#endif
#elif defined(SMD_MOVE_TIMER_1)
    TCNT1H=0xFF;
    TCNT1L=0xFF;
#ifdef TIMSK1
    sbi(TIMSK1,TOIE1);
#else
    sbi(TIMSK,TOIE1);
#endif
#elif defined(SMD_MOVE_TIMER_2)
    TCNT2=0xFF;
#ifdef TIMSK2
    sbi(TIMSK2,TOIE2);
#else
    sbi(TIMSK,TOIE2);
#endif
#endif
}


void smd_stop()
{
#if defined(SMD_MOVE_TIMER_0)
#ifdef TIMSK0
    cbi(TIMSK0,TOIE0);
#else
    cbi(TIMSK,TOIE0);
#endif
#elif defined(SMD_MOVE_TIMER_1)
#ifdef TIMSK1
    cbi(TIMSK1,TOIE1);
#else
    cbi(TIMSK,TOIE1);
#endif
#elif defined(SMD_MOVE_TIMER_2)
#ifdef TIMSK2
    cbi(TIMSK2,TOIE2);
#else
    cbi(TIMSK,TOIE2);
#endif
#endif
    smd_flags&=~SMD_IS_RUNNING;
}


uint8_t smd_get_current()
{
#if defined(SMD_CURCTRL_OC1A)
    return OCR1AL;
#elif defined(SMD_CURCTRL_OC1B)
    return OCR1BL;
#elif defined(SMD_CURCTRL_OC2)
    return OCR2;
#else
    return 0;
#endif
}


void smd_set_current(uint8_t current_)
{
#if defined(SMD_CURCTRL_OC1A)
    OCR1AL=current_;
#elif defined(SMD_CURCTRL_OC1B)
    OCR1BL=current_;
#elif defined(SMD_CURCTRL_OC2)
    OCR2=current_;
#endif
}
