#include "betalupi_ergodox.h"
#include "features/beta_rawhid.h"

extern inline void ergodox_board_led_on(void);
extern inline void ergodox_right_led_1_on(void);
extern inline void ergodox_right_led_2_on(void);
extern inline void ergodox_right_led_3_on(void);
extern inline void ergodox_right_led_on(uint8_t led);

extern inline void ergodox_board_led_off(void);
extern inline void ergodox_right_led_1_off(void);
extern inline void ergodox_right_led_2_off(void);
extern inline void ergodox_right_led_3_off(void);
extern inline void ergodox_right_led_off(uint8_t led);

extern inline void ergodox_led_all_on(void);
extern inline void ergodox_led_all_off(void);

extern inline void ergodox_right_led_1_set(uint8_t n);
extern inline void ergodox_right_led_2_set(uint8_t n);
extern inline void ergodox_right_led_3_set(uint8_t n);
extern inline void ergodox_right_led_set(uint8_t led, uint8_t n);

extern inline void ergodox_led_all_set(uint8_t n);

keyboard_config_t keyboard_config;
bool i2c_initialized = 0;
i2c_status_t mcp23018_status = 0x20;

void matrix_init_kb(void) {
	// keyboard LEDs (see "PWM on ports OC1(A|B|C)" in "teensy-2-0.md")
	TCCR1A = 0b10101001;  // set and configure fast PWM
	TCCR1B = 0b00001001;  // set and configure fast PWM

	// (tied to Vcc for hardware convenience)
	DDRB  &= ~(1<<4);  // set B(4) as input
	PORTB &= ~(1<<4);  // set B(4) internal pull-up disabled

	// unused pins - C7, D4, D5, D7, E6
	// set as input with internal pull-up enabled
	DDRC  &= ~(1<<7);
	DDRD  &= ~(1<<5 | 1<<4);
	DDRE  &= ~(1<<6);
	PORTC |=  (1<<7);
	PORTD |=  (1<<5 | 1<<4);
	PORTE |=  (1<<6);

	keyboard_config.raw = eeconfig_read_kb();

	#ifdef RGB_MATRIX_ENABLE
		if (keyboard_config.rgb_matrix_enable) {
			rgb_matrix_set_flags(LED_FLAG_ALL);
		} else {
			rgb_matrix_set_flags(LED_FLAG_NONE);
		}
	#endif

	ergodox_blink_all_leds();
	matrix_init_user();
}

void ergodox_blink_all_leds(void) {
	ergodox_led_all_off();
	ergodox_led_all_set(LED_BRIGHTNESS_DEFAULT);

	ergodox_right_led_1_on();
	_delay_ms(50);
	ergodox_right_led_2_on();
	_delay_ms(50);
	ergodox_right_led_3_on();
	_delay_ms(50);

	ergodox_right_led_1_off();
	_delay_ms(50);
	ergodox_right_led_2_off();
	_delay_ms(50);
	ergodox_right_led_3_off();

	ergodox_led_all_off();
}

uint8_t init_mcp23018(void) {
	mcp23018_status = 0x20;

	// I2C subsystem

	// uint8_t sreg_prev;
	// sreg_prev=SREG;
	// cli();

	if (i2c_initialized == 0) {
		i2c_init();  // on pins D(1,0)
		i2c_initialized = true;
		_delay_ms(1000);
	}
	// i2c_init(); // on pins D(1,0)
	// _delay_ms(1000);

	// set pin direction
	// - unused  : input  : 1
	// - input   : input  : 1
	// - driving : output : 0
	mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);    if (mcp23018_status) goto out;
	mcp23018_status = i2c_write(IODIRA, ERGODOX_EZ_I2C_TIMEOUT);            if (mcp23018_status) goto out;
	mcp23018_status = i2c_write(0b00000000, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
	mcp23018_status = i2c_write(0b00111111, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
	i2c_stop();

	// set pull-up
	// - unused  : on  : 1
	// - input   : on  : 1
	// - driving : off : 0
	mcp23018_status = i2c_start(I2C_ADDR_WRITE, ERGODOX_EZ_I2C_TIMEOUT);    if (mcp23018_status) goto out;
	mcp23018_status = i2c_write(GPPUA, ERGODOX_EZ_I2C_TIMEOUT);             if (mcp23018_status) goto out;
	mcp23018_status = i2c_write(0b00000000, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;
	mcp23018_status = i2c_write(0b00111111, ERGODOX_EZ_I2C_TIMEOUT);        if (mcp23018_status) goto out;

out:
	i2c_stop();

	// SREG=sreg_prev;

	return mcp23018_status;
}


void keyboard_post_init_kb(void) {

	// Start with matrix enabled
	// (We enable RGB effects and layer colors seperately)
	#ifdef RGB_MATRIX_ENABLE
	rgb_matrix_enable_noeeprom();
	#endif

	// Start with backlight disabled
	#ifdef RGBLIGHT_ENABLE
	#ifdef RGBLIGHT_OFF_AT_START
	rgblight_disable_noeeprom();
	#endif
	#endif

	keyboard_post_init_user();
}

// EEPROM is getting reset!
void eeconfig_init_kb(void) {
	keyboard_config.raw = 0;
	keyboard_config.led_level = 4;
	keyboard_config.rgb_matrix_enable = true;
	eeconfig_update_kb(keyboard_config.raw);
	eeconfig_init_user();
}


layer_state_t layer_state_set_kb(layer_state_t state) {
	//uint8_t layer = biton32(state);

	hid_send_state(
		.state = state
	);
	return layer_state_set_user(state);
};


void matrix_scan_kb(void) {

	#ifdef CAPS_LOCK_STATUS
		led_t led_state = host_keyboard_led_state();
		if(led_state.caps_lock) {
			ergodox_right_led_3_on();
		}
		else {
			uint8_t layer = get_highest_layer(layer_state);
			if(layer != 3) {
				ergodox_right_led_3_off();
			}
		}
	#endif

	matrix_scan_user();
}