#include "motor_driver.h"

#include <avr/io.h>


MotorDriver::MotorDriver()
{
}


void MotorDriver::begin()
{
    // Arduino D3  = PD3 = AIN1
    // Arduino D11 = PB3 = AIN2
    // Arduino D4  = PD4 = FLT 

    DDRD |= (1 << DDD3);
    DDRB |= (1 << DDB3);

    DDRD &= ~(1 << DDD4); 

    PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2


    // Timer2 Fast PWM
    TCCR2A = 0;
    TCCR2B = 0;

    // Fast PWM mode
    TCCR2A |= (1 << WGM20);
    TCCR2A |= (1 << WGM21);

    // Non-inverting PWM on OC2A and OC2B
    TCCR2A |= (1 << COM2A1);
    TCCR2A |= (1 << COM2B1);


    // Timer2 prescaler = 32
    TCCR2B |= (1 << CS21);
    TCCR2B |= (1 << CS20);

    OCR2A = 0;
    OCR2B = 0;

    //stop();
}


void MotorDriver::forward()
{
    // AIN1 / D3  = HIGH
    // AIN2 / D11 = LOW

    //PORTD |= (1 << PORTD3);
    //PORTB &= ~(1 << PORTB3);

    OCR2A = 0;
    OCR2B = 255;
}


void MotorDriver::reverse()
{
    // AIN1 / D3  = LOW
    // AIN2 / D11 = HIGH

    //PORTD &= ~(1 << PORTD3);
    //PORTB |= (1 << PORTB3);

    OCR2B = 0;
    OCR2A = 255;
}


void MotorDriver::stop()
{
    // Both LOW

    //PORTD &= ~(1 << PORTD3);
    //PORTB &= ~(1 << PORTB3);

    OCR2A = 0;
    OCR2B = 0;
}

void MotorDriver::set(double pwm)
{
    // Limit PWM command
    if (pwm > 255.0)
    {
        pwm = 255.0;
    }

    if (pwm < -255.0)
    {
        pwm = -255.0;
    }


    if (pwm > 0)
    {
        // Forward
        OCR2A = 0;
        OCR2B = (uint8_t)pwm;
    }
    else if (pwm < 0)
    {
        // Reverse
        OCR2B = 0;
        OCR2A = (uint8_t)(-pwm);
    }
    else
    {
        stop();
    }
}


bool MotorDriver::fault_active()
{
    return !(PIND & (1 << PIND4));
}