//
// Motors module common operations library
// Homelab library
//
// Department of Mechatronics
// Tallinn University of Technology
// ITT Group
// © Copyrights 2014
//

//
// Take use of pin and timer operations
//
#include "xmega/clksys_driver.h"
#include "module/motors.h"
#include <util/delay.h>
#include <avr/interrupt.h>

//
// Initialize specified DC-motor (1-4)
//
void dcmotor_init(unsigned char motor)
{
    switch(motor)
    {
    case 1:
        PORTC.DIRSET = (1<<0);
        PORTJ.DIRSET = (1<<4);
        PORTC.OUTCLR = (1<<0);
        PORTJ.OUTCLR = (1<<4);
        break;
    case 2:
        PORTC.DIRSET = (1<<1);
        PORTJ.DIRSET = (1<<5);
        PORTC.OUTCLR = (1<<1);
        PORTJ.OUTCLR = (1<<5);
        break;
    case 3:
        PORTC.DIRSET = (1<<2);
        PORTJ.DIRSET = (1<<6);
        PORTC.OUTCLR = (1<<2);
        PORTJ.OUTCLR = (1<<6);
        break;
    case 4:
        PORTC.DIRSET = (1<<3);
        PORTJ.DIRSET = (1<<7);
        PORTC.OUTCLR = (1<<3);
        PORTJ.OUTCLR = (1<<7);
        break;
    }
}

//
// Drive command for specified DC-motor
//   index - Motor number 1-4
//   direction - Motor turning direction -1, 0 or 1
void dcmotor_drive(unsigned char index, signed char direction)
{
    switch(index)
    {
    case 1:
        if(direction > 0)
        {
            PORTC.OUTSET = (1<<0);
            PORTJ.OUTSET = (1<<4);
        }
        else if(direction < 0)
        {
            PORTC.OUTSET = (1<<0);
            PORTJ.OUTCLR = (1<<4);
        }
        else
        {
            PORTC.OUTCLR = (1<<0);
        }
        break;
    case 2:
        if(direction > 0)
        {
            PORTC.OUTSET = (1<<1);
            PORTJ.OUTSET = (1<<5);
        }
        else if(direction < 0)
        {
            PORTC.OUTSET = (1<<1);
            PORTJ.OUTCLR = (1<<5);
        }
        else
        {
            PORTC.OUTCLR = (1<<1);
        }
        break;
    case 3:
        if(direction > 0)
        {
            PORTC.OUTSET = (1<<2);
            PORTJ.OUTSET = (1<<6);
        }
        else if(direction < 0)
        {
            PORTC.OUTSET = (1<<2);
            PORTJ.OUTCLR = (1<<6);
        }
        else
        {
            PORTC.OUTCLR = (1<<2);
        }
        break;
    case 4:
        if(direction > 0)
        {
            PORTC.OUTSET = (1<<3);
            PORTJ.OUTSET = (1<<7);
        }
        else if(direction < 0)
        {
            PORTC.OUTSET = (1<<3);
            PORTJ.OUTCLR = (1<<7);
        }
        else
        {
            PORTC.OUTCLR = (1<<3);
        }
        break;
    }
}

//
// Initialize PWM for specified DC-motor. You can also choose prescaler.
//
void dcmotor_drive_pwm_init(uint8_t motor, uint8_t prescaler)
{
    if(_sys_freq == 2)
        Homelab_clock_init();

    switch(motor)
    {
    case 4:	//OCC0A
        PORTC.DIRSET |= (1<<0);
        PORTJ.DIRSET |= (1<<4);
        PORTC.OUTCLR = (1<<0);
        PORTJ.OUTCLR = (1<<4);

        TC_SetCompareA(&TCC0, 0);							// Duty cycle
        TC0_EnableCCChannels(&TCC0, TC0_CCAEN_bm);			// Enable channel A
        break;
    case 3:	//OCC0B
        PORTC.DIRSET |= (1<<1);
        PORTJ.DIRSET |= (1<<5);
        PORTC.OUTCLR = (1<<1);
        PORTJ.OUTCLR = (1<<5);
        TC_SetCompareB(&TCC0, 0);							// Duty cycle
        TC0_EnableCCChannels(&TCC0, TC0_CCBEN_bm);			// Enable channel B
        break;
    case 2:	//OCC0C
        PORTC.DIRSET |= (1<<2);
        PORTJ.DIRSET |= (1<<6);
        PORTC.OUTCLR = (1<<2);
        PORTJ.OUTCLR = (1<<6);
        TC_SetCompareC(&TCC0, 0);							// Duty cycle
        TC0_EnableCCChannels(&TCC0, TC0_CCCEN_bm);			// Enable channel C
        break;
    case 1:	//OCC0D
        PORTC.DIRSET |= (1<<3);
        PORTJ.DIRSET |= (1<<7);
        PORTC.OUTCLR = (1<<3);
        PORTJ.OUTCLR = (1<<7);
        TC_SetCompareD(&TCC0, 0);							// Duty cycle
        TC0_EnableCCChannels(&TCC0, TC0_CCDEN_bm);			// Enable channel D
        break;
    }
    if(motor)
    {
        TC_SetPeriod(&TCC0, 255);					// Set timer period
        TC0_ConfigClockSource(&TCC0, prescaler);	// Set timer prescaler
        TC0_ConfigWGM(&TCC0, TC_WGMODE_SS_gc);		// SingleSlope waveform
    }
}

//
// Change PWM for specified DC-motor
//
void dcmotor_drive_pwm(uint8_t motor, int8_t dir, uint8_t speed)
{
    switch (motor)
    {
    case 4:
        if(dir==1)
            PORT_SetPins(&PORTJ, (1<<4));
        else
            PORT_ClearPins(&PORTJ, (1<<4));
        TC_SetCompareA(&TCC0, speed);
        break;
    case 3:
        if(dir==1)
            PORT_SetPins(&PORTJ, (1<<5));
        else
            PORT_ClearPins(&PORTJ, (1<<5));
        TC_SetCompareB(&TCC0, speed);
        break;
    case 2:
        if(dir==1)
            PORT_SetPins(&PORTJ, (1<<6));
        else
            PORT_ClearPins(&PORTJ, (1<<6));
        TC_SetCompareC(&TCC0, speed);
        break;
    case 1:
        if(dir==1)
            PORT_SetPins(&PORTJ, (1<<7));
        else
            PORT_ClearPins(&PORTJ, (1<<7));
        TC_SetCompareD(&TCC0, speed);
        break;
    }
}

//
// Initialize specified servo motor
//
void servomotor_init(uint8_t index)
{
    static uint8_t timers_initialized = 0;
    if(_sys_freq == 2)
        Homelab_clock_init();


    if ((index <= 2) && (!(timers_initialized & 1))) // use OCC1 A/B
    {
        TC1_ConfigWGM(&TCC1, TC_WGMODE_SS_gc);
        TC_SetPeriod(&TCC1, 9999);
        TC1_ConfigClockSource(&TCC1, TC_CLKSEL_DIV64_gc);
        timers_initialized |= 1;
    }
    else if ((index <= 4) && (!(timers_initialized & 2))) // use OCD0 A/B
    {
        TC0_ConfigWGM(&TCD0, TC_WGMODE_SS_gc);
        TC_SetPeriod(&TCD0, 9999);
        TC0_ConfigClockSource(&TCD0, TC_CLKSEL_DIV64_gc);
        timers_initialized |= 2;
    }

    if(index == 1)
    {
        PORTC.DIRSET |= (1<<4);
        TC_SetCompareA(&TCC1, 750);							// Duty cycle
        TC1_EnableCCChannels(&TCC1, TC0_CCAEN_bm);			// Enable channel A
    }
    else if(index == 2)
    {
        PORTC.DIRSET |= (1<<5);
        TC_SetCompareB(&TCC1, 750);							// Duty cycle
        TC1_EnableCCChannels(&TCC1, TC0_CCBEN_bm);			// Enable channel A
    }
    else if(index == 3)
    {
        PORTD.DIRSET |= (1<<0);
        TC_SetCompareA(&TCD0, 750);							// Duty cycle
        TC0_EnableCCChannels(&TCD0, TC0_CCAEN_bm);			// Enable channel A
    }
    else if(index == 4)
    {
        PORTD.DIRSET |= (1<<1);
        TC_SetCompareB(&TCD0, 750);							// Duty cycle
        TC0_EnableCCChannels(&TCD0, TC0_CCBEN_bm);			// Enable channel A
    }
}

//
// Position servo motors
// pos is from -100 to 100
//
void servomotor_position(unsigned char index, signed short position)
{
    switch (index)
    {
    case 1:
        TC_SetCompareA(&TCC1, 750+((int16_t)position*5/2)); //500 - 750 - 1000
        break;
    case 2:
        TC_SetCompareB(&TCC1, 750+((int16_t)position*5/2)); //500 - 750 - 1000
        break;
    case 3:
        TC_SetCompareA(&TCD0, 750+((int16_t)position*5/2)); //500 - 750 - 1000
        break;
    case 4:
        TC_SetCompareB(&TCD0, 750+((int16_t)position*5/2)); //500 - 750 - 1000
        break;
    }
}

//
// Initialize specified unipolar stepper motor
//
void unipolar_init(uint8_t index)
{
    if(_sys_freq == 2)
        Homelab_clock_init();

    // Set control pins as output
    PORTJ.DIRSET |= (1<<0)|(1<<1)|(1<<2)|(1<<3);
}

//
// Unipolar stepper motor half-step
//
void unipolar_halfstep(unsigned char index, signed char direction, unsigned short num_steps, unsigned char speed)
{
    uint8_t state1 = 0, state2 = 1;
    uint8_t pattern;
    uint8_t timeout;

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

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

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

        // Make pattern
        pattern =
            (1 << ((state1 % 8) >> 1)) |
            (1 << ((state2 % 8) >> 1));

        // Set output
        PORTJ.OUT = (PORTJ.OUT & 0xF0) | (pattern & 0xF);

        // Wait for the motor to complete the step
        timeout = speed;

        while (timeout-- > 0)
        {
            _delay_ms(1);
        }
    }

    // Turn off motor
    PORTJ.OUTCLR = 0x0F;
}