void set_leds(uint8_t x, uint8_t y, uint8_t z)
{
	uint8_t neighbours = get_amount_of_neighbours((int8_t)x, (int8_t)y, (int8_t)z);

	if(neighbours >= 9 && neighbours <= 14) set_led(x, y, z, MAX_INTENSITY);
	else set_led(x, y, z, 0);
}

void f_cubes(frame_t *buffer, uint16_t frame) {
	uint8_t x = 0;
	uint8_t y = 0;
	uint8_t z = 0;
	uint8_t a = 0;
	uint8_t b = 0;

	memset(buffer, 0, sizeof(frame_t) * CUBE_HEIGHT);

	frame = frame % 6;
	if (frame < 3) {
		a = CUBE_SIZE - frame;
		b = frame;
	} else if (frame >= 3 && frame < 6) {
		a = frame;
		b = CUBE_SIZE - frame;
	}

	for (x = b; x < a; x++) {
		for (y = b; y < a; y++) {
			for (z = b; z < a; z++) {
				if (z == b || z == a - 1) {
					if ((x == b || x == a - 1) || (y == b || y == a - 1))
						set_led(buffer, x, y, z);
				} else if (z != b && z != a - 1) {
					if ((x == b || x == a - 1) && (y == b || y == a - 1))
						set_led(buffer, x, y, z);
				}
			}
		}
	}
}

/**************************************************************************//**
 * @brief  Main function
 *****************************************************************************/
int main(void)
{
	// Chip errata
	CHIP_Init();

	setup_utilities();

	CMU_ClockEnable(cmuClock_GPIO, true);

	// Set up the user interface buttons
	GPIO_PinModeSet(BUTTON_PORT, SET_BUTTON_PIN, gpioModeInput,  0);

	while (1)
	{
		if (get_button())
		{
			set_led(0, 1);
			delay(DELAY_VALUE);
			set_led(1, 1);
		}
		else
		{
			set_led(0, 0);
			set_led(1, 0);
		}
	}
}

void effect(void)
{
	// TODO use real kernels instead of loops!
	
	clear_buffer();

	for (uint8_t x=0; x<8; x++) {
		for (uint8_t y=0; y<8; y++) {
			srand(3*(x*LEDS_Y+y));
			set_led(x,y,((ticks >> 3)+rand()) & 7, MAX_INTENSITY >> 3);
		}
	}

	const uint8_t water = ((ticks >> 7) % 7) + 1;
	const uint8_t surface = ticks & 127;

	for (uint8_t z=0; z<water; z++) {
		for (uint8_t x=0; x<8; x++) {
			for (uint8_t y=0; y<8; y++) {
				set_led(x,y,7-z, MAX_INTENSITY);
			}
		}
	}

	for (uint8_t x=0; x<8; x++) {
		for (uint8_t y=0; y<8; y++) {
			set_led(x,y,8-water, weber_fechner(surface << 1));
		}
	}
}

/************************************************************************
* 	arm_controller()
*		- zero out the controller
*		- set the setpoint.armed_state to ARMED
*		- enable motors
************************************************************************/
int arm_controller(){
	zero_out_controller();
	setpoint.arm_state = ARMED;
	set_led(GREEN,HIGH);
	set_led(RED,HIGH);
	return 0;
}

void hexbright::print_charge(unsigned char led) {
  unsigned char charge_state = get_charge_state();
  if(charge_state == CHARGING && get_led_state(led) == LED_OFF) {
    set_led(led, 350, 350);
  } else if (charge_state == CHARGED) {
    set_led(led,50);
  }
}

void process_command()
{
  DEBUG_PRINTLN("process_command");
  DEBUG_PRINTLN2("command ", command.command);
  DEBUG_PRINTLN2H("com.addr  ", command.node_id.addr);
  DEBUG_PRINTLN2H("this.addr ", THIS_NODE);

  answer.node_id.addr = THIS_NODE;
  answer.command = command.command;
  answer.number = command.number;
  answer.answer = 0;

  if (command.node_id.addr == THIS_NODE ||
    command.node_id.addr == 0)
  {

    switch(command.command)
    {
      case ECommand_LIGHT_ON:
      {
        DEBUG_PRINTLN("ECommand_LIGHT_ON");
        //digitalWrite(ledPin, HIGH);
        // digitalWrite(ledPin, LOW);
        current_color = 7;
        set_led(true);

        answer.answer = 1;

        break;
      }
      case ECommand_LIGHT_OFF:
      {
        DEBUG_PRINTLN("ECommand_LIGHT_OFF");
        // digitalWrite(ledPin, LOW);
        // digitalWrite(ledPin, HIGH);
        set_led(false);


        answer.answer = 2;

        break;
      }
      case ECommand_BEACON:
      {
        // answer is ready for beacon, no processing needed
        DEBUG_PRINTLN("ECommand_BEACON");
        break;
      }
      default:
      {
        answer.answer = -1;
      }
    }

    send_package(SimpleAnswer_fields, &answer, packet_xb_writer);
  }

}

void peek_pix(unsigned char pix)
{
	unsigned char p = getpeek();

	if (pix & 16)
		set_led(0x00f0, MAX_LVL);
	if (p)
		set_led(0xff00, MAX_LVL);
}

/*
 * Show fatal error indicated by Kinght Rider(tm) effect
 * in LEDS 0-7. LEDS 8-11 contain 4 bit error code.
 * Note: this function will not terminate.
 */
void fatal_error(unsigned int error_code)
{
    int i, d;

    for (i = 0; i < 12; i++) {
	set_led(i, LED_0);
    }

    /*
     * Write error code.
     */
    set_led(8,  (error_code & 0x01) ? LED_1 : LED_0);
    set_led(9,  (error_code & 0x02) ? LED_1 : LED_0);
    set_led(10, (error_code & 0x04) ? LED_1 : LED_0);
    set_led(11, (error_code & 0x08) ? LED_1 : LED_0);

    /*
     * Yay - Knight Rider effect!
     */
    while(1) {
	unsigned int delay = 2000;

	for (i = 0; i < 8; i++) {
	    set_led(i, LED_1);
	    for (d = 0; d < delay; d++);
	    set_led(i, LED_0);
	}

	for (i = 7; i > 0; i--) {
	    set_led(i, LED_1);
	    for (d = 0; d < delay; d++);
	    set_led(i, LED_0);
	}
    }
}

void fish_shape(uint8_t xi, uint8_t yi, uint8_t zi, uint16_t intensity)
{
	if(xi > 0) set_row(xi - 1, zi + 1, yi + 1, yi + 4, intensity);

	set_led(xi, yi, zi, intensity);
	set_row(xi, zi, yi + 2, yi + 4, intensity);
	set_row(xi, zi + 1, yi, yi + 5, intensity);
	set_row(xi, zi + 2, yi + 2, yi + 3, intensity);
	set_led(xi, yi, zi + 2, intensity);

	if(xi < LEDS_X - 1) set_row(xi + 1, zi + 1, yi + 1, yi + 4, intensity);
}

void tap_tempo_task(void)
{
    static uint8_t taps = 0;
    static uint8_t buffer_index = 0;

    // Increment counter
    static uint16_t counter = 0;
    ++counter;

    if (!tap_arrived) {
        if (counter < 400) {
            return;
        }

        // Reset after timeout
        set_led(LED_RED, false);
        counter = 0;
        taps = 0;
        buffer_index = 0;
        return;
    }
    tap_arrived = false;

    // Increment tap counter to buffer size
    if (taps < TAP_TEMPO_BUFFER_SIZE) {
        ++taps;
    }

    if (taps == 1) {
        set_led(LED_RED, true);
    }
    else {
        // Register tap interval with cyclic buffer
        static fixed_t tap_tempo_buffer[TAP_TEMPO_BUFFER_SIZE] = {0, };
        fixed_t tap_frequency = fixed_from_int(TAP_TEMPO_TASK_FREQUENCY) / counter;
        tap_tempo_buffer[buffer_index] = tap_frequency;
        ++buffer_index;
        buffer_index %= TAP_TEMPO_BUFFER_SIZE;

        // Compute average
        fixed_t average = 0;
        for (int i=0; i<taps; i++) {
            average += tap_tempo_buffer[i];
        }
        average /= taps;

        // Set wave frequency
        set_frequency(tap_tempo_wave, average);
    }

    // Reset counter
    counter = 0;
}

int
/**********************************************************/
init_sio (int led, unsigned long base)
/**********************************************************/
{
	unsigned char val;

	set_led (led, YELLOW);
	val = cradle_inb (base, CRADLE_SIO_INDEX);
	val = cradle_inb (base, CRADLE_SIO_INDEX);
	if (val != 0) {
		set_led (led, RED);
		return -1;
	}

	/* map SCC2 to COM1 */
	cradle_outb (0x01, base, CRADLE_SIO_INDEX);
	cradle_outb (0x00, base, CRADLE_SIO_DATA);

	/* enable SCC2 extended regs */
	cradle_outb (0x40, base, CRADLE_SIO_INDEX);
	cradle_outb (0xa0, base, CRADLE_SIO_DATA);

	/* enable SCC2 clock multiplier */
	cradle_outb (0x51, base, CRADLE_SIO_INDEX);
	cradle_outb (0x04, base, CRADLE_SIO_DATA);

	/* enable SCC2 */
	cradle_outb (0x00, base, CRADLE_SIO_INDEX);
	cradle_outb (0x04, base, CRADLE_SIO_DATA);

	/* map SCC2 DMA to channel 0 */
	cradle_outb (0x4f, base, CRADLE_SIO_INDEX);
	cradle_outb (0x09, base, CRADLE_SIO_DATA);

	/* read ID from SIO to check operation */
	cradle_outb (0xe4, base, 0x3f8 + 0x3);
	val = cradle_inb (base, 0x3f8 + 0x0);
	if ((val & 0xf0) != 0x20) {
		set_led (led, RED);
		/* disable SCC2 */
		cradle_outb (0, base, CRADLE_SIO_INDEX);
		cradle_outb (0, base, CRADLE_SIO_DATA);
		return -1;
	}
	/* set back to bank 0 */
	cradle_outb (0, base, 0x3f8 + 0x3);
	set_led (led, GREEN);
	return 0;
}

static void hwnotify_conversation_updated(PurpleConversation *conv, 
                                          PurpleConvUpdateType type) {
	if( type != PURPLE_CONV_UPDATE_UNSEEN ) {
		return;
	}

        gboolean unread, important;

	get_pending_events (&unread, &important);
        unsigned char state = get_led_state ();
        state = set_led (state, color_unread, unread);
        state = set_led (state, color_important, important);
        set_led_state (state);
}

/******************************************************************
*	blink_red()
*	used to warn user that the program is exiting
*******************************************************************/
int blink_red(){
	const int us_to_blink = 2000000; // 2 seconds
	const int blink_hz = 10;
	const int delay = 1000000/(2*blink_hz); 
	const int blinks = blink_hz*us_to_blink/1000000;
	int i;
	for(i=0;i<blinks;i++){
		usleep(delay);
		set_led(RED,HIGH);
		usleep(delay);
		set_led(RED,LOW);
	}
	return 0;
}

/*
 * Routine: misc_init_r
 * Description: Configure board specific parts
 */
int misc_init_r(void)
{
	t2_t *t2_base = (t2_t *)T2_BASE;
	u32 pbias_lite;

	twl4030_power_init();

	/* set VSIM to 1.8V */
	twl4030_pmrecv_vsel_cfg(TWL4030_PM_RECEIVER_VSIM_DEDICATED,
				TWL4030_PM_RECEIVER_VSIM_VSEL_18,
				TWL4030_PM_RECEIVER_VSIM_DEV_GRP,
				TWL4030_PM_RECEIVER_DEV_GRP_P1);

	/* set up dual-voltage GPIOs to 1.8V */
	pbias_lite = readl(&t2_base->pbias_lite);
	pbias_lite &= ~PBIASLITEVMODE1;
	pbias_lite |= PBIASLITEPWRDNZ1;
	writel(pbias_lite, &t2_base->pbias_lite);
	if (get_cpu_family() == CPU_OMAP36XX)
		writel(readl(OMAP34XX_CTRL_WKUP_CTRL) |
					 OMAP34XX_CTRL_WKUP_CTRL_GPIO_IO_PWRDNZ,
					 OMAP34XX_CTRL_WKUP_CTRL);

	setup_net_chip();

	omap_die_id_display();

	set_led();

	set_boardname();

	return 0;
}

uint8_t DeviceCommands::next_command(void)
{
    static char c;
    c = getchr();
    if (c>0)
        put(c);
        if (parse()) {
            if (strcmp(command.name, "echo") == 0)
                echo(command.parameters[0]);
            else if (strcmp(command.name, "set_pwm") == 0)
                set_pwm((uint8_t)command.parameters[0], command.parameters[1]);
            else if (strcmp(command.name, "set_motors") == 0)
                set_motors(command.parameters[0], command.parameters[1], command.parameters[2], command.parameters[3]);
            else if (strcmp(command.name, "set_led") == 0)
                set_led((uint8_t)command.parameters[0], (uint8_t)command.parameters[1]);
            else if (strcmp(command.name, "read_adc") == 0)
                read_adc();
            else if (strcmp(command.name, "stop") == 0)
                stop();

            
            
  //          comm << cp.command.name<<"\t"<<cp.command.nparameters<<"\n";
        }
    return 0;
}

int init(void)
{
    // initialize SDL video
    if ( SDL_Init( SDL_INIT_VIDEO | SDL_INIT_AUDIO ) < 0 )
    {
        printf( "Unable to init SDL: %s\n", SDL_GetError() );
        return 1;
    }
    set_led(0); // off by default

    // make sure SDL cleans up before exit
    atexit(SDL_Quit);

    // create a default new window
    set_mode(VGA_H_PIXELS,VGA_V_PIXELS);

    #ifndef NO_AUDIO
    audio_init();
    #endif

    joy_init();

    next_time = SDL_GetTicks();

    if (!quiet)
        printf("Screen is now %dx%d with a scale of %d\n",screen_width,screen_height,scale);
    
    SDL_ShowCursor(SDL_DISABLE);
    
    return 0;
}

/* signals usr1 & usr2 are sent by the spnav_x11 script to start/stop the
 * daemon's connection to the X server.
 */
static void sig_handler(int s)
{
    int tmp;

    switch(s) {
    case SIGHUP:
        tmp = cfg.led;
        read_cfg("/etc/spnavrc", &cfg);
        if(cfg.led != tmp) {
            set_led(cfg.led);
        }
        break;

    case SIGSEGV:
        fprintf(stderr, "Segmentation fault caught, trying to exit gracefully\n");
    case SIGINT:
    case SIGTERM:
        exit(0);

#ifdef USE_X11
    case SIGUSR1:
        init_x11();
        break;

    case SIGUSR2:
        close_x11();
        break;
#endif

    default:
        break;
    }
}

void prvGetRegistersFromStack(unsigned int *pulFaultStackAddress )
{
#if DEBUG_PRINTF_EXCEPTIONS

    int frame;
    int reg;

    for(frame = 0; frame < STACKTRACE_DEPTH; frame++)
    {
        printf("frame %d\n", frame);
        for(reg = 0; reg < STACKFRAME_DEPTH; reg++)
        {
            printf("\t%s=%#08x\n", stack_regs[reg], pulFaultStackAddress[(frame * STACKFRAME_DEPTH) + reg]);
        }
    }
#else
    (void)pulFaultStackAddress;
#endif

#if ERROR_LED
    set_led(ERROR_LED);
#endif

    for( ;; );
}

void init(void)
{
	for(uint8_t i = 0; i < matrix_xyz_len; i++) {
		vars.xyz[i] = (xyz_t){
			.x = randint(0, LEDS_X),
			.y = randint(0, LEDS_Y),
			.z = randint(0, LEDS_Z)
		};
	}

	clear_buffer();
}
void effect(void)
{
	clear_buffer();

	for(uint8_t i = 0; i < matrix_xyz_len; i++) {
		xyz_t xyz = vars.xyz[i];

		for(uint8_t j = 0; j < 3; j++) {
			if(xyz.z + j < LEDS_Z) {
				set_led(xyz.x, xyz.y, xyz.z + j, MAX_INTENSITY);
			}
		}

		uint8_t z = xyz.z;

		z++;

		if(z >= LEDS_Z) z = 0;

		vars.xyz[i].z = z;
	}
}