#include <avr/io.h>
#include <util/delay.h>
#include "arduinolib.h"

void Arduinolib__dwrite_step(int p, int v, Arduinolib__dwrite_out *out){

    if (8 <= p && p <= 13){
        int pin = p - 8;
        DDRB |= (1 << pin);
        PORTB |= (1 << pin);
        if (v == 0) PORTB &= ~(1 << pin);
    }
    else if ( 0 <= p && p <= 7){
        DDRD |= (1 << p);
        PORTD |= (1 << p);
        if (v == 0) PORTD &= ~(1 << p);
    }
    else { //by default
        DDRB |= (1 << 5);
        PORTB |= (1 << 5);
        if (v == 0) PORTB &= ~(1 << 5);
    }
    return;
}

void Arduinolib__dread_step(int p, Arduinolib__dread_out *out){
    if (8 <= p && p <= 13){
        int pin = p - 8;
        DDRB &= ~(1 << pin);
        if ((PINB & (1 << pin)) == (1 << pin) ) {
            out -> v = 1; //pin high so it is pressed
        }
        else {
            out -> v = 0; //pin low so it is not pressed
        }
    }
    else if ( 0 <= p && p <= 7){
        DDRD &= ~(1 << p);
        if ((PIND & (1 << p)) == (1 << p) ) {
            out -> v = 1; //pin high so it is pressed
        } else {
            out -> v = 0; //pin low so it is not pressed
        }
    }
    else { //by default to pin 13 of arduino
        DDRB &= ~(1 << 5);
        if ((PINB & (1 << 5)) == (1 << 5) ) {
            out -> v = 1; //pin high so it is pressed
        }
        else {
            out -> v = 0; //pin low so it is not pressed
        }
    }
    return;
}