#include "avrio.h"
#include "avrTypes.h"
#include <avr/io.h>

/*
 Bare minimum functions to be able to do all of the examples in the Arduino Basic Examples on the traditional IDE:

done
 - pinMode
 - digitalRead
 - digitalWrite

in progress
 - delay functions

not started
 - analogRead
 - analogWrite
 - serial functions
*/


__attribute__((noinline)) unsigned char pinMask(unsigned char pin) {
    if (pin<8) {
        return ((unsigned char)1) << pin;
    } else if (pin<14) {
        return ((unsigned char)1) << (pin - (unsigned char)8);
    }
    
    return 0;
}

// reads as false if you pass an invalid pin number like 14
_Bool _digitalRead(unsigned char pin) {
    unsigned char mask = pinMask(pin);

    if (!mask) return false;
    
    if (pin<8) {
        return PIND & mask;
    } else {
        return PINB & mask;
    }
    
    return false;
}

/* 
    These functions are stupidly verbose and spread out to try and force the compiler to make code that works.
    Currently there are bugs in the AVR back end that produce non functional code unless we force its hand.
*/

__attribute__((noinline)) void resetPortD(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        PORTD &= ~mask;
    }
}

__attribute__((noinline)) void resetPortB(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        PORTB &= ~mask;
    }
}

__attribute__((noinline)) void setPortD(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        PORTD |= mask;
    }
}

__attribute__((noinline)) void setPortB(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        PORTB |= mask;
    }
}

void _digitalWrite(unsigned char pin, _Bool value) {
    if (value) {
        if (pin<8) {
            setPortD(pin);
        } else {
            setPortB(pin);
        }
    } else {
        if (pin<8) {
            resetPortD(pin);
        } else {
            resetPortB(pin);
        }
    }
}

void enablePortDWrite(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        DDRD |= mask;
    }
}

void enablePortBWrite(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        DDRB |= mask;
    }
}

void disablePortDWrite(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        DDRD &= ~mask;
    }
}

void disablePortBWrite(unsigned char pin) {
    unsigned char mask = pinMask(pin);
    if (mask) {
        DDRB &= ~mask;
    }
}

void _pinMode(unsigned char pin, _Bool write) {
    if (write) {
        if (pin<8) {
            enablePortDWrite(pin);
        } else {
            enablePortBWrite(pin);
        }
    } else {
        if (pin<8) {
            disablePortDWrite(pin);
        } else {
            disablePortBWrite(pin);
        }
    }
}