#define WITH_LIB 1
#if WITH_LIB
#include <arduino.h>
#endif
#include <avr/io.h>
#include <avr/interrupt.h>


#if !WITH_LIB
#define PIN_NONE 0
// PB0
#define PIN_D0 1
// PB1
#define PIN_D1 2
// PB2
#define PIN_D2 3
// PB3
#define PIN_D3 4
// PB4
#define PIN_D4 5
// PB5
#define PIN_D5 6
#define PIN_A0 6
#define PIN_A1 3
#define PIN_A2 5
#define PIN_A3 4


enum pin_mode {
        OUTPUT,
        INPUT,
        INPUT_PULLUP
};

struct __attribute__((packed)) pin_t {
        volatile uint8_t *type_reg;
        volatile uint8_t *out_reg;
        volatile uint8_t *in_reg;
        uint8_t mask;
};

void pin_write(uint8_t pin, uint8_t val);
void pin_toggle(uint8_t pin);

#if 1
struct pin_t pins[] = {
        {0, 0, 0, 0},
        {&DDRB, &PORTB, &PINB, 1},
        {&DDRB, &PORTB, &PINB, 2},
        {&DDRB, &PORTB, &PINB, 4},
        {&DDRB, &PORTB, &PINB, 8},
        {&DDRB, &PORTB, &PINB, 16},
        {&DDRB, &PORTB, &PINB, 32}
};

void
pin_write(uint8_t pin, uint8_t val)
{
	uint8_t sreg_prev;

        if (!(*pins[pin].type_reg & pins[pin].mask))
                return;
        sreg_prev = SREG;
        cli();
        if (val)
                *pins[pin].out_reg |= pins[pin].mask;
        else
                *pins[pin].out_reg &= ~pins[pin].mask;
		SREG = sreg_prev;
}

void
pin_toggle(uint8_t pin)
{
	uint8_t sreg_prev;

	sreg_prev = SREG;
	cli();
    *pins[pin].out_reg ^= pins[pin].mask;
	SREG = sreg_prev;
}

uint8_t
pin_read(uint8_t pin)
{
        return (*pins[pin].in_reg & pins[pin].mask) ? 1 : 0;
}

void
pin_mode(uint8_t pin, enum pin_mode mode)
{
        switch (mode) {
        case OUTPUT:
                *pins[pin].type_reg |= pins[pin].mask;
                break;
        case INPUT:
                *pins[pin].type_reg &= ~pins[pin].mask;
                *pins[pin].out_reg &= ~pins[pin].mask;
                break;
        case INPUT_PULLUP:
                *pins[pin].type_reg &= ~pins[pin].mask;
                *pins[pin].out_reg |= pins[pin].mask;
                break;
        }
}

void
pin_get_spec(uint8_t pin, volatile uint8_t **ddr, volatile uint8_t **port,
volatile uint8_t **_pin, uint8_t *bit)
{
        if (ddr)
                *ddr = pins[pin].type_reg;
        if (port)
                *port = pins[pin].out_reg;
        if (_pin)
                *_pin = pins[pin].in_reg;
        *bit = pins[pin].mask;
}

#endif
#endif

volatile uint16_t tmr_cnt = 0;
volatile uint8_t pwm_width = 0;
uint8_t pwm_width_on = 11, pwm_width_off = 8;


ISR(TIM0_COMPA_vect)
{
	tmr_cnt++;
	pin_toggle(PIN_D0);
}

void
main(void)
{
	DDRB |= _BV(0)|_BV(1)|_BV(2);
	DDRB |= _BV(3);

	TCCR0B &= ~(1 << WGM02);
	TCCR0A &= ~((1 << WGM01) | (1 << WGM00));
	TCCR0A |= 2 << WGM00;
	TCCR0A &= ~((1 << COM0A1) | (1 << COM0A0));
	TCCR0A |= 0 << COM0A0;
	TCCR0B &= ~(3 << CS00);
	TCCR0B |= 2 << CS00;
	TIMSK0 &= ~(3 << TOIE0);
	TIMSK0 |= 2 << TOIE0;
	// ~100us
	OCR0A = 15;
	//OCR0A = 25;
	sei();
	asm("sbi %[io], %[bit]" :: [io] "I" (_SFR_IO_ADDR(PORTB)), [bit] "I" (3));

/*
	volatile uint16_t portb = 0x38;
    while(1)
    {
        uint8_t sreg = SREG;
        asm("cli");
        *(volatile uint8_t*)portb |= 2;
        *(volatile uint8_t*)portb &= ~2;
        SREG = sreg;
    }
	asm("cbi %[io], %[bit]" :: [io] "I" (_SFR_IO_ADDR(PORTB)), [bit] "I" (3));
*/

	uint8_t i = 0;
	while (1) {
		if (bit_is_set(SREG, 7))
			PORTB |= _BV(1);
		else
			PORTB &= ~(_BV(1));

		if (pwm_width > pwm_width_on)
			pin_write(PIN_D2, 1);
		else if (pwm_width < pwm_width_off)
			//PORTB &= ~(_BV(2));
			pin_write(PIN_D2, 0);
			/*
		if ((i & 1) == 0)
			asm("sbi %[io], %[bit]" :: [io] "I" (_SFR_IO_ADDR(PORTB)), [bit] "I" (3));
		else
			asm("cbi %[io], %[bit]" :: [io] "I" (_SFR_IO_ADDR(PORTB)), [bit] "I" (3));
		i++;
		*/
	}
}