#ifndef MSP430TOOLS_H
#define MSP430TOOLS_H

#include <stdint.h>
#include <msp430.h>

#define inline_ __attribute__((always_inline)) static inline

#ifndef MHZ
#define MHZ 1
#endif

#define PIN1 0x0001
#define PIN2 0x0002
#define PIN3 0x0004
#define PIN4 0x0008
#define PIN5 0x0010
#define PIN6 0x0020
#define PIN7 0x0040
#define PIN8 0x0080

extern const char shift_table[8];

#define pin_dir_port_(pin) (*(volatile char *)(((pin) >= 8 ) ? P2DIR_ : P1DIR_))
#define pin_in_port_(pin) (*(volatile char *)(((pin) >= 8) ? P2IN_ : P1IN_))
#define pin_out_port_(pin) (*(volatile char *)(((pin) >= 8) ? P2OUT_ : P1OUT_))
#define pin_bit_mask_(pin) (shift_table[(pin) & 0x07])

//#define pin_dir_port_(pin) (*(volatile char *)(((pin) & 0x0100) ? P2DIR_ : P1DIR_))
//#define pin_in_port_(pin) (*(volatile char *)(((pin) & 0x0100) ? P2IN_ : P1IN_))
//#define pin_out_port_(pin) (*(volatile char *)(((pin) & 0x0100) ? P2OUT_ : P1OUT_))
//#define pin_bit_mask_(pin) ((pin) & 0xFF)

//#define pin_dir_port_(pin) (*((pin) < 7 ? (volatile char *)P1DIR_ : (volatile char*)P2DIR_))
//#define pin_in_port_(pin) (*((pin) < 7 ? (volatile char *)P1IN_ : (volatile char*)P2IN_))
//#define pin_out_port_(pin) (*((pin) < 7 ? (volatile char *)P1OUT_ : (volatile char*)P2OUT_))
//#define pin_bit_mask_(pin) (1 << ((pin) & 0x07))

#define INPUT 0
#define OUTPUT 1

void delay_ms(int ms);

char pin_dir_dynamic_(char pin, char dir);
inline_ void pin_dir(char pin, char dir) {
    if (__builtin_constant_p(pin) && __builtin_constant_p(dir)) {
        if (dir)
            pin_dir_port_(pin) |= pin_bit_mask_(pin);
        else
            pin_dir_port_(pin) &= ~pin_bit_mask_(pin);
    } else {
        pin_dir_dynamic_(pin, dir);
    }
}

char pin_read_dynamic_(char pin);
inline_ char pin_read(char pin) {
    if (__builtin_constant_p(pin))
        return (pin_in_port_(pin) & pin_bit_mask_(pin)) > 0;
    else
        return pin_read_dynamic_(pin);
}

char pin_write_dynamic_(uint16_t pin, char value);
inline_ void pin_write(uint16_t pin, char value) {
    if (__builtin_constant_p(pin) && __builtin_constant_p(value)) {
        if (value)
            pin_out_port_(pin) |= pin_bit_mask_(pin);
        else
            pin_out_port_(pin) &= ~pin_bit_mask_(pin);
    } else {
        pin_write_dynamic_(pin, value);
    }
}

inline_ void msp430_init() {

#if MHZ == 1
    BCSCTL1 = CALBC1_1MHZ;
    DCOCTL = CALDCO_1MHZ;
#elif MHZ == 8
    BCSCTL1 = CALBC1_8MHZ;
    DCOCTL = CALDCO_8MHZ;
#elif MHZ == 12
    BCSCTL1 = CALBC1_12MHZ;
    DCOCTL = CALDCO_12MHZ;
#elif MHZ == 16
    BCSCTL1 = CALBC1_12MHZ;
    DCOCTL = CALDCO_12MHZ;
#else
    #error "Unknown MHZ value"
#endif

}

#endif