void CPsydukCore::Initialise() {
	
	CPU_PRESCALE(CPU_125kHz);
	_delay_ms(1);           // allow slow power supply startup
	CPU_PRESCALE(CPU_16MHz); // set for 16 MHz clock
	
	usb_init();
	while (!usb_configured()) {	}
	
	Delay(2000);
	
	m_bIsInitialised = true;
}

/** Configures the board hardware and chip peripherals for the demo's functionality. */
void SetupHardware(void)
{
    /* Disable watchdog */
    //MCUSR &= ~(1 << WDRF);
    //wdt_disable();
	
	//Switch off JTAG (to free up ports F pins 4,5,6,7)
	MCUCR = (1 << JTD) | (1 << IVCE) | (0 << PUD);
	MCUCR = (1 << JTD) | (0 << IVSEL) | (0 << IVCE) | (0 << PUD); 
	//#define DISABLE_JTAG_APPLICATION

	// Clock @ 16Mhz
	CPU_PRESCALE(0);
	
	//adc_init();
	
	//Initialise AVR Pins
    DDRA=0x00;
    DDRB=0x00;
    DDRC=0x00;
    DDRD=0x00;
    DDRE=0x00;
    DDRF=0x00;
    PORTA=0xFF;
    PORTB=0xFF;
    PORTC=0xFF;
    PORTD=0xFF;
    PORTE=0xFF;
    PORTF=0xFF;

    /* Hardware Initialization */
    USB_Init();
}

int main(void)
{
	uint16_t val, count=1;

	// set for 16 MHz clock, and turn on the LED
	CPU_PRESCALE(0);
	LED_CONFIG;
	LED_ON;

	// initialize the USB, and then wait for the host
	// to set configuration.  If the Teensy is powered
	// without a PC connected to the USB port, this 
	// will wait forever.
	usb_init();
	while (!usb_configured()) /* wait */ ;

	// wait an extra second for the PC's operating system
	// to load drivers and do whatever it does to actually
	// be ready for input
	_delay_ms(1000);

	// start printing stuff.  If hid_listen is running on
	// the host, this should appear.
	print("USB debug only example\n");
	while (1) {
		print("Hello World ");
		phex16(count++);
		print("\n");
		_delay_ms(1000);
	}
}

int main(void)
{
    // set for 16 MHz clock
    CPU_PRESCALE(0);
    LED_CONFIG;
    LED_OFF;

    // Initialize the USB, and then wait for the host to set configuration.
    // If the Teensy is powered without a PC connected to the USB port,
    // this will wait forever.
    usb_init();
    while (!usb_configured()) /* wait */ ;

    // Wait an extra second for the PC's operating system to load drivers
    // and do whatever it does to actually be ready for input
    _delay_ms(1000);

    // Setup the timer interrupt that handles the touch screen,
    // and setup the ADC
    setup();

    // initialize PIN D0 as digital input, others are set to 
    // pullup resistor
    DDRD = 0b00000000; 
    DDRD = 0b11111110; 

    // Enable interrupts
    sei();

    while (1) {}
}

/* returns
 * - success: 0
 */
uint8_t teensy_init(void) {
	// CPU speed : should match F_CPU in makefile
	#if F_CPU != 16000000
		#error "Expecting different CPU frequency"
	#endif
	CPU_PRESCALE(CPU_16MHz);

	// PD2 as interrupt for N35P112
	DDRD &=~ (1 << 2); //Input
	PORTD |= (1 << 2); //Use Pullup

	// PD3 as reset for N35P112
	DDRD |= (1 << 3); //Output

	// PB7 as pushbutton for N35P112
	DDRB &=~ (1 << 7); //Input
	PORTB &=~ (1 << 7); //No Pullup

	// I2C (TWI)
	uint8_t twiPrescaler = TWI_BIT_PRESCALE_1;
	uint8_t twiBitRate = TWI_BITLENGTH_FROM_FREQ(1, TWI_FREQ);
	TWI_Init(twiPrescaler, twiBitRate);

	return 0;  // success
}

/** Main program entry point. This routine contains the overall program flow, including initial
 *  setup of all components and the main program loop.
 */
int main(void)
{
  unsigned char i;

	// set for 16 MHz clock, and make sure the LED is off
	CPU_PRESCALE(0);
	LED_CONFIG;
	LED_OFF;
	
	// Justy's bootup LED hack
  int d,e;
	for (d=0; d<100; d++) {
    LED_ON;
		_delay_ms(1+.1*d);
  LED_OFF;
  _delay_ms(21-.1*d);
	}
	
	
	SetupHardware();

	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
	sei();

	for (;;)
	{
		MS_Device_USBTask(&Disk_MS_Interface);
		USB_USBTask();
	}
}

int main(void)
{
	int8_t x, y, *p;
	uint8_t i;

	// set for 16 MHz clock
	CPU_PRESCALE(0);
	LED_CONFIG;
	LED_OFF;

	// Initialize the USB, and then wait for the host to set configuration.
	// If the Teensy is powered without a PC connected to the USB port,
	// this will wait forever.
	usb_init();
	while (!usb_configured()) /* wait */ ;

	// Wait an extra second for the PC's operating system to load drivers
	// and do whatever it does to actually be ready for input
	_delay_ms(1000);

	while (1) {
		// This sequence creates a left click
		usb_mouse_buttons(1, 0, 0);
		_delay_ms(10);
		usb_mouse_buttons(0, 0, 0);
	}
}

int main (void)
{
    uint8_t i;
    CPU_PRESCALE(0);
    DDRB = 0xff;
    DDRD = 0xff;
    usb_init();
    while(!usb_configured());
    usb_buffer[0] = 0xab;
    usb_buffer[1] = 0xcd; 
    usb_buffer[63] = 4;
    led();
/*    controller_mode = probe;*/
    controller_mode = poll;
    while(1){
        switch(controller_mode){
            case(probe):
                _delay_ms(12);
                controller_probe();
                usb_rawhid_send(usb_buffer, 50);
                break;
            case(poll):
                controller_poll();
                usb_rawhid_send(usb_buffer, 50);
               _delay_ms(6);
                break;
        }
    }
    return 0;
}

int main(void)
{
	CPU_PRESCALE(CPU_16MHz);
	SPI_SlaveInit();
	InitWEnc();
	
	u08 main_msg;
	
	// Start keeping track of position at 0
	WE_wheel_0 = 0;
	while(!WE_wheel_0);
	WE_position = 0;
	
	// Loop indefinitely
	while(1)
	{
		// obtain command from main uC
		main_msg = SPI_SlaveReceive();
		switch(main_msg)
		{
			case SPI_WHEEL_POS:
			SPI_SlaveReceiveX(WE_position);
			break;
			
			case SPI_WHEEL_RPM:
			SPI_SlaveReceiveX(WE_RPM);
			break;
			
			default:
			break;
		}
	}
}

int main(void)
{
    unsigned char i;

    // set for 16 MHz clock, and make sure the LED is off
    CPU_PRESCALE(0);
    LED_CONFIG;
    LED_OFF;

    // initialize the USB, but don't want for the host to
    // configure.  The first several messages sent will be
    // lost because the PC hasn't configured the USB yet,
    // but we care more about blinking than debug messages!
    usb_init();

    // blink morse code messages!
    while (1) {
        for (i=0; i<6; i++) {
            morse_P(PSTR("SOS"));
            _delay_ms(1500);
        }
        morse_P(PSTR("DOES ANYBODY STILL KNOW MORSE CODE?"));
        _delay_ms(4000);
    }
}

int main(void)
{
    // set for 16 MHz clock
    CPU_PRESCALE(0);

    // Initialize the USB, and then wait for the host to set configuration.
    // If the Teensy is powered without a PC connected to the USB port,
    // this will wait forever.
    usb_init();
    while (!usb_configured()) /* wait */ ;

    keyboard_init();
    host_set_driver(pjrc_driver());
#ifdef SLEEP_LED_ENABLE
    sleep_led_init();
#endif
    while (1) {
        while (suspend) {
            suspend_power_down();
            if (remote_wakeup && suspend_wakeup_condition()) {
                usb_remote_wakeup();
            }
        }

        keyboard_task(); 
    }
}

void setup() {

  CPU_PRESCALE(0x03);

  CONFIG_OUT_B(0);
  CONFIG_OUT_B(1);
  CONFIG_OUT_B(2);
  CONFIG_OUT_B(3);
  CONFIG_OUT_B(4);
  CONFIG_OUT_B(5);
  CONFIG_OUT_B(6);
  CONFIG_OUT_B(7);
  CONFIG_OUT_C(6);
  CONFIG_OUT_C(7);
  CONFIG_OUT_D(0);
  CONFIG_OUT_D(1);
  CONFIG_OUT_D(2);
  CONFIG_IN_D(3); // switch
  LED_ON_D(3); // pullup resistor
  CONFIG_OUT_D(4);
  CONFIG_IN_D(5); // switch
  LED_ON_D(5); // pullup resistor
  CONFIG_OUT_D(6);
  CONFIG_OUT_D(7);
  CONFIG_OUT_F(7);
}

void init()
{
	GTCCR |= (1 << PSRSYNC);
	CPU_PRESCALE(0);

	USART_init(BAUD_RATE);
	USART_send_string("\n\nWe're online jack!\r\n");

	new_motor_state = 0;

	DDRD = 0xFF;
	PORTD = 0x50;

	sei();
	UCSR1B |= (1 << RXCIE1);
	if (UCSR1B & ~(1 << RXCIE1)){
		USART_send_string("Recieving Interrupt enabled.\r\n");
	}

	TCCR0B &= 0b11110000;
	TCCR0B |= 0b00000101;	//Max prescaling
	TCCR0A &= 0b11111100;	//Normal mode...
	OCR0A = 255;
	TIMSK0 |= 0b00000010;
	USART_send_string("Set timer to use CPU ticks, normal mode, with 255 comparison.");

}

int
main(void)
{
  static uint8_t state[16];
  static uint8_t prev_state[16];

  // set for 16 MHz clock
  CPU_PRESCALE(0);

  init_keyboard_interface();

  // Initialize the USB, and then wait for the host to set configuration.
  // If the Teensy is powered without a PC connected to the USB port,
  // this will wait forever.
  usb_init();
  while (!usb_configured()) /* wait */ ;

  // Wait for the PC's operating system to load drivers
  // and do whatever it does to actually be ready for input
  _delay_ms(3000);

  memset(prev_state, 0, sizeof prev_state);
  while (1) {
    poll_keyboard(state);
    if (memcmp(state, prev_state, sizeof state)) {
      send_keys(state);
      memcpy(prev_state, state, sizeof state);
    }
    _delay_ms(10);
  }
}

int main(void) {
    CPU_PRESCALE(0);

    GBA_DDR &= ~(1<<MISO_BIT);
    GBA_DDR |= (1<<MOSI_BIT) | (1<<CLK_BIT);
    CLK_HIGH();

    usb_init();
    while (!usb_configured());
    _delay_ms(1000);

    INIT_TIMER();

    while (1) {
        if (usb_serial_available() >= 4) {
            uint32_t data = 0;
            data |= (uint32_t)usb_serial_getchar()<<24;
            data |= (uint32_t)usb_serial_getchar()<<16;
            data |= (uint32_t)usb_serial_getchar()<<8;
            data |= (uint32_t)usb_serial_getchar();
            xfer(&data);
            usb_serial_putchar((data>>24) & 0xff);
            usb_serial_putchar((data>>16) & 0xff);
            usb_serial_putchar((data>>8) & 0xff);
            usb_serial_putchar(data & 0xff);
            usb_serial_flush_output();
        }
    }

/** Configures all hardware required for the bootloader. */
void SetupHardware(void)
{
	/* Disable watchdog if enabled by bootloader/fuses */
	MCUSR &= ~(1 << WDRF);
	wdt_disable();

	/* Disable clock division */
	clock_prescale_set(clock_div_1);

	/* Relocate the interrupt vector table to the bootloader section */
	MCUCR = (1 << IVCE);
	MCUCR = (1 << IVSEL);
	
	LED_SETUP();
	CPU_PRESCALE(0); 
	L_LED_OFF();
	TX_LED_OFF();
	RX_LED_OFF();
	
	/* Initialize TIMER1 to handle bootloader timeout and LED tasks.  
	 * With 16 MHz clock and 1/64 prescaler, timer 1 is clocked at 250 kHz
	 * Our chosen compare match generates an interrupt every 1 ms.
	 * This interrupt is disabled selectively when doing memory reading, erasing,
	 * or writing since SPM has tight timing requirements.
	 */ 
	OCR1AH = 0;
	OCR1AL = 250;
	TIMSK1 = (1 << OCIE1A);					// enable timer 1 output compare A match interrupt
	TCCR1B = ((1 << CS11) | (1 << CS10));	// 1/64 prescaler on timer 1 input

	/* Initialize USB Subsystem */
	USB_Init();
}

/* returns
 * - success: 0
 */
uint8_t teensy_init(void) {
	CPU_PRESCALE(CPU_16MHz);  // speed should match F_CPU in makefile

	// onboard LED
	DDRD  &= ~(1<<6);  // set D(6) as input
	PORTD &= ~(1<<6);  // set D(6) internal pull-up disabled

	// keyboard LEDs (see "PWM on ports OC1(A|B|C)" in "teensy-2-0.md")
	_led_all_off();  // (just to put the pins in a known state)
	TCCR1A  = 0b10101001;  // set and configure fast PWM
	TCCR1B  = 0b00001001;  // set and configure fast PWM

	// I2C (TWI)
	twi_init();  // on pins D(1,0)

	// unused pins
	teensypin_write_all_unused(DDR, CLEAR); // set as input
	teensypin_write_all_unused(PORT, SET);  // set internal pull-up enabled

	// rows
	teensypin_write_all_row(DDR, CLEAR); // set as input
	teensypin_write_all_row(PORT, SET);  // set internal pull-up enabled

	// columns
	teensypin_write_all_column(DDR, CLEAR);   // set as input (hi-Z)
	teensypin_write_all_column(PORT, CLEAR);  // set internal pull-up
	                                          //   disabled

	return 0;  // success
}

int main(void) {
	CPU_PRESCALE(CPU_16MHz);
	usb_init();
	while (!usb_configured()) /* wait */
		_delay_ms(1000);

	init_cols();
	//  TCCR0A = 0x00;
	//	TCCR0B =(1<<CS00);
	//	TIMSK0 = (1<<TOIE0);
	////////////////////////////////////////////////
			DDRD |= (1 << 5);
			PORTD &= ~(1 << 5);
			for(r=0;r<10;r++){
			keyboard_keys[0] = KEY_CAPS_LOCK;
			usb_keyboard_send();
			_delay_ms(100);
			keyboard_keys[0] = 0;
			usb_keyboard_send();
			_delay_ms(500);
			}
			if (PIND & (1 << 6)){
					unselect_rows();
					_delay_ms(500);
					pokerMode();
			}else{
				unselect_rows();
				_delay_ms(500);
				osuMode();
			}
}

/*
 ********************************************************************************
 * main
 ********************************************************************************
 */
int main( void )
{


    /* Disable watchdog if enabled by bootloader/fuses */
    MCUSR &= ~(1 << WDRF);
    wdt_disable();

    /* Disable clock division */
    CPU_PRESCALE(0);
    LED_CONFIG;
    LED_ON;
    
    /* Initialize Keyboard Driver */
    KB_Init();

    /* Initialize Mouse Driver */
    MS_Init();
    
    /* Initialize USB Joystick variables */
    usb_joystick_raz();

    /* Enable interrupts */
    sei( );

    /* Initialize USB Joystick */
    usb_init();
    while (!usb_configured()) ;
    _delay_ms(1000);
    LED_OFF;

    /* Send a first packet */
    usb_joystick_send();

    /* Reset the mouse */
    MS_MouseReset();

    /*
	 * Comment that code to test rotation speed for a given position.
	 */
    while( 1 )
    {
        KB_EventTask();
		MS_EventTask();
    }
    
    /*
     * Uncomment that code to test rotation speed for a given position.
     */
//	while( 1 )
//    {
//		usb_joystick_move_zrz(153, 128);
//		usb_joystick_send();
//		_delay_ms(5000);
//	}

    return 0;
}

void init(void){
	
	CPU_PRESCALE(CPU_16MHz);
	adc_init();
	Counter_init();
	port_init();
	uart_init();
	sei();
}