/* Initialize the LKM */
int init_module()
{
  console_print("Hello, world - this is the kernel speaking\n");
  /* More normal is printk(), but there's less that can go wrong with 
     console_print(), so let's start simple.
  */

  /* If we return a non zero value, it means that 
   * init_module failed and the LKM can't be loaded 
   */
  return 0;
}

bool contact_added_callback( btManifoldPoint &btmanifoldpoint,
							 const btCollisionObject *btcollisionobject0,
							 int part_0, int index_0,
							 const btCollisionObject *btcollisionobject1,
							 int part_1, int index_1 ) {

	OBJMESH *objmesh0 = ( OBJMESH * )( ( btRigidBody * )btcollisionobject0 )->getUserPointer();

	OBJMESH *objmesh1 = ( OBJMESH * )( ( btRigidBody * )btcollisionobject1 )->getUserPointer();

	console_print("Object #0: %s\n", objmesh0->name );
	console_print("Point  #0: %.3f %.3f %.3f\n",
				  btmanifoldpoint.m_positionWorldOnA.x(),
				  btmanifoldpoint.m_positionWorldOnA.y(),
				  btmanifoldpoint.m_positionWorldOnA.z() );

	console_print("Object #1: %s\n", objmesh1->name );
	console_print("Point  #1: %.3f %.3f %.3f\n",
				  btmanifoldpoint.m_positionWorldOnB.x(),
				  btmanifoldpoint.m_positionWorldOnB.y(),
				  btmanifoldpoint.m_positionWorldOnB.z() );

	console_print("Normal   : %.3f %.3f %.3f\n",
				  btmanifoldpoint.m_normalWorldOnB.x(),
				  btmanifoldpoint.m_normalWorldOnB.y(),
				  btmanifoldpoint.m_normalWorldOnB.z() );
	
	console_print( "%d\n\n", get_milli_time() );

	return false;
}

/**************************************************************************//***
 * @brief Sets the RX2 RF gain.
 *
 * @return None.
*******************************************************************************/
void set_rx2_rf_gain(double* param, char param_no) // "rx2_rf_gain=" command
{
	int32_t gain_db;

	if(param_no >= 1)
	{
		gain_db = param[0];
		ad9361_set_rx_rf_gain (ad9361_phy, 1, gain_db);
		console_print("rx2_rf_gain=%d\n", gain_db);
	}
	else
		show_invalid_param_message(1);
}

/*! open current working directory for ftp session
 *
 *  @param[in] session ftp session
 *
 *  @return -1 for failure
 */
static int
ftp_session_open_cwd(ftp_session_t *session)
{
  /* open current working directory */
  session->dp = opendir(session->cwd);
  if(session->dp == NULL)
  {
    console_print(RED "opendir '%s': %d %s\n" RESET, session->cwd, errno, strerror(errno));
    return -1;
  }

  return 0;
}

/**
 * Fault Handler: General Protection Fault
 *
 * Kernel: Panic
 * User: Terminate Process
 */
void fault_gp(cpu_int_state_t *state) {
    // Is in kernel?
    if (state->cs == 0x8) {
        console_print("PANIC: General Protection Fault in kernel at ");
        console_print_hex(state->rip);
        console_print(" (error code: ");
        console_print_hex(state->error_code);
        console_print(").\n");
        while (1);
    }

    // TODO: Remove this debug warning
    DEBUG("General Protection Fault at ");
    DEBUG_HEX(state->rip);
    DEBUG(" (error code: ");
    DEBUG_HEX(state->error_code);
    DEBUG(").\n");

    // Terminate process
    process_terminate(process_current->pid);
    thread_switch(scheduler_next(), state);
}

/**************************************************************************//***
 * @brief Sets the DDS TX2 Tone 2 frequency [Hz].
 *
 * @return None.
*******************************************************************************/
void set_dds_tx2_tone2_freq(double* param, char param_no)	// dds_tx2_tone2_freq=
{
	uint32_t freq = (uint32_t)param[0];

	if(param_no >= 1)
	{
		dds_set_frequency(DDS_CHAN_TX2_I_F2, freq);
		dds_set_frequency(DDS_CHAN_TX2_Q_F2, freq);
		console_print("dds_tx2_tone2_freq=%d\n", freq);
	}
	else
		show_invalid_param_message(1);
}

/**************************************************************************//***
 * @brief Sets the RX LO frequency [MHz].
 *
 * @return None.
*******************************************************************************/
void set_rx_lo_freq(double* param, char param_no) // "rx_lo_freq=" command
{
	uint64_t lo_freq_hz;

	if(param_no >= 1)
	{
		lo_freq_hz = param[0];
		lo_freq_hz *= 1000000;
		ad9361_set_rx_lo_freq(ad9361_phy, lo_freq_hz);
		lo_freq_hz /= 1000000;
		console_print("rx_lo_freq=%d\n", (uint32_t)lo_freq_hz);
	}
}

/**************************************************************************//***
 * @brief Sets the DDS TX2 Tone 2 scale.
 *
 * @return None.
*******************************************************************************/
void set_dds_tx2_tone2_scale(double* param, char param_no)	// dds_tx2_tone2_scale=
{
	uint32_t scale = (uint32_t)param[0];

	if(param_no >= 1)
	{
		dds_set_scale(DDS_CHAN_TX2_I_F2, scale);
		dds_set_scale(DDS_CHAN_TX2_Q_F2, scale);
		console_print("dds_tx2_tone2_scale=%d\n", scale);
	}
	else
		show_invalid_param_message(1);
}

/**************************************************************************//***
 * @brief Sets the RX RF bandwidth [Hz].
 *
 * @return None.
*******************************************************************************/
void set_rx_rf_bandwidth(double* param, char param_no) // "rx_rf_bandwidth=" command
{
	uint32_t bandwidth_hz;

	if(param_no >= 1)
	{
		bandwidth_hz = param[0];
		ad9361_set_rx_rf_bandwidth(ad9361_phy, bandwidth_hz);
		console_print("rx_rf_bandwidth=%d\n", bandwidth_hz);
	}
	else
		show_invalid_param_message(1);
}

/**************************************************************************//***
 * @brief Sets the RX FIR state.
 *
 * @return None.
*******************************************************************************/
void set_rx_fir_en(double* param, char param_no) // "rx_fir_en=" command
{
	uint8_t en_dis;

	if(param_no >= 1)
	{
		en_dis = param[0];
		ad9361_set_rx_fir_en_dis(ad9361_phy, en_dis);
		console_print("rx_fir_en=%d\n", en_dis);
	}
	else
		show_invalid_param_message(1);
}

/*! deinitialize ftp subsystem */
void
ftp_exit(void)
{
#ifdef _3DS
  Result ret;
#endif

  /* clean up all sessions */
  while(sessions != NULL)
    ftp_session_destroy(sessions);

  /* stop listening for new clients */
  if(listenfd >= 0)
    ftp_closesocket(listenfd, 0);

#ifdef _3DS
  /* deinitialize SOC service */
  ret = SOC_Shutdown();
  if(ret != 0)
    console_print(RED "SOC_Shutdown: 0x%08X\n" RESET, (unsigned int)ret);
  free(SOC_buffer);

#ifdef ENABLE_LOGGING
  /* close log file */
  if(fclose(stderr) != 0)
    console_print(RED "fclose: 0x%08X\n" RESET, errno);

  /* deinitialize sdmc_dev */
  ret = sdmcExit();
  if(ret != 0)
    console_print(RED "sdmcExit: 0x%08X\n" RESET, (unsigned int)ret);
#endif

  /* deinitialize FS service */
  ret = fsExit();
  if(ret != 0)
    console_print(RED "fsExit: 0x%08X\n" RESET, (unsigned int)ret);
#endif
}

/**************************************************************************//***
 * @brief Sets the RX2 GC mode.
 *
 * @return None.
*******************************************************************************/
void set_rx2_gc_mode(double* param, char param_no) // "rx2_gc_mode=" command
{
	uint8_t gc_mode;

	if(param_no >= 1)
	{
		gc_mode = param[0];
		ad9361_set_rx_gain_control_mode(ad9361_phy, 1, gc_mode);
		console_print("rx2_gc_mode=%d\n", gc_mode);
	}
	else
		show_invalid_param_message(1);
}

void bootstrap() {
    DPRINT("Device booted at time: %d\n", timer_get_counter_value());

#ifndef FRAMEWORK_LOG_BINARY
    console_print("\r\nPER TEST - commands:\r\n");
    console_print("  CHANfffriii  channel settings:\r\n");
    console_print("               fff frequency : 433, 868, 915\r\n");
    console_print("               r   rate      : L(ow) N(ormal) H(igh)\r\n");
    console_print("               iii center_freq_index\r\n");
    console_print("  TRANsss      transmit a packet every sss seconds.\r\n");
    console_print("  RECV         receive packets\r\n");
    console_print("  RSET         reset module\r\n");
#endif

    id = hw_get_unique_id();
    hw_radio_init(&alloc_new_packet, &release_packet);

    rx_cfg.channel_id = current_channel_id;
    tx_cfg.channel_id = current_channel_id;

    ubutton_register_callback(0, &userbutton_callback);
    ubutton_register_callback(1, &userbutton_callback);

    fifo_init(&uart_rx_fifo, uart_rx_buffer, sizeof(uart_rx_buffer));

    console_set_rx_interrupt_callback(&uart_rx_cb);
    console_rx_interrupt_enable();

    sched_register_task(&start_rx);
    sched_register_task(&transmit_packet);
    sched_register_task(&start);
    sched_register_task(&process_uart_rx_fifo);

    current_state = STATE_CONFIG_DIRECTION;

    sched_post_task(&start);
    sched_post_task(&process_uart_rx_fifo);


#ifdef PLATFORM_EFM32GG_STK3700
#else
    	char str[20];
    	channel_id_to_string(&current_channel_id, str, sizeof(str));
    	lcd_write_line(6, str);
#endif

    timer_post_task(&transmit_packet, TIMER_TICKS_PER_SEC * 1);

}

static void task_timer (const char _name [], void *_p_arg)
{
    queue_handle_t queue = (queue_handle_t)_p_arg;
    timer_handle_t handle;

    for (;;) {
        if (0 != queue_message_receive (queue, 0, &handle)) {
            console_print ("Error: task \"%s\" cannot recieve message", _name);
            (void) task_suspend (task_self ());
        }
        handle->callback_(handle, handle->arg_);
    }
}

static void con_tune_param(void *result, void *user_data, int cid)
{
	const char *param_name = console_arg_string(result, 0);
	float new_value = console_arg_float(result, 1);

	if(tuning.set(param_name, new_value))
	{
		dbg_msg("tuning", "%s changed to %.2f", param_name, new_value);
		send_tuning_params(-1);
	}
	else
		console_print("No such tuning parameter");
}

/*!
	Start the audio system. This function will automatically use the first valid
	OpenAL device found on your device and will create an OpenAL context. Make sure
	you call this function at initialization time if you plan to use audio. In addition,
	this function will link the necessary callbacks to handle the OGG decompression
	in memory.
*/
void AUDIO_start( void )
{
	memset( &audio, 0, sizeof( AUDIO ) );

	audio.al_device = alcOpenDevice( NULL );

	audio.al_context = alcCreateContext( audio.al_device, NULL );

	alcMakeContextCurrent( audio.al_context );

	console_print( "\nAL_VENDOR:      %s\n", ( char * )alGetString ( AL_VENDOR     ) );
	console_print( "AL_RENDERER:    %s\n"  , ( char * )alGetString ( AL_RENDERER   ) );
	console_print( "AL_VERSION:     %s\n"  , ( char * )alGetString ( AL_VERSION    ) );
	console_print( "AL_EXTENSIONS:  %s\n"  , ( char * )alGetString ( AL_EXTENSIONS ) );
	
	audio.callbacks.read_func  = AUDIO_ogg_read;
	audio.callbacks.seek_func  = AUDIO_ogg_seek;
	audio.callbacks.tell_func  = AUDIO_ogg_tell;
	audio.callbacks.close_func = AUDIO_ogg_close;

	AUDIO_error();
}

/**************************************************************************//***
 * @brief Sets the TX2 attenuation [mdB].
 *
 * @return None.
*******************************************************************************/
void set_tx2_attenuation(double* param, char param_no) // "tx1_attenuation=" command
{
	uint32_t attenuation_mdb;

	if(param_no >= 1)
	{
		attenuation_mdb = param[0];
		ad9361_set_tx_attenuation(ad9361_phy, 1, attenuation_mdb);
		console_print("tx2_attenuation=%d\n", attenuation_mdb);
	}
	else
		show_invalid_param_message(1);
}

void
on_tty_intr(char c)
{
    char sz[2] = {c, 0};
    if (c == '\b') {
        int bs = _backspace_queue(&console_tty.td_read_q);
        if (bs == 0) {
            console_print(sz);
        }
    }
    else if(c == '\r') {
        if (_is_full_queue(&console_tty.td_read_q))  return;
        _put_queue(&console_tty.td_read_q, c);
        wakeup(console_tty.td_read_q.tq_wait_task);
        console_print(sz);
    }
    else {
        if (_is_full_queue(&console_tty.td_read_q))  return;
        _put_queue(&console_tty.td_read_q, c);
        console_print(sz);
    }
}

// --------------------------------------------------------- console_print ---
void
console_print( console_t *self, wchar_t *text )
{
    // Make sure there is at least one line
    if( self->lines->size == 0 )
    {
        wchar_t *line = wcsdup( L"" );
        vector_push_back( self->lines, &line );
    }

    // Make sure last line does not end with '\n'
    wchar_t *last_line = *(wchar_t **) vector_get( self->lines, self->lines->size-1 ) ;
    if( wcslen( last_line ) != 0 )
    {
        if( last_line[wcslen( last_line ) - 1] == L'\n' )
        {
            wchar_t *line = wcsdup( L"" );
            vector_push_back( self->lines, &line );
        }
    }
    last_line = *(wchar_t **) vector_get( self->lines, self->lines->size-1 ) ;

    wchar_t *start = text;
    wchar_t *end   = wcschr(start, L'\n');
    size_t len = wcslen( last_line );
    if( end != NULL)
    {
        wchar_t *line = (wchar_t *) malloc( (len + end - start + 2)*sizeof( wchar_t ) );
        wcpncpy( line, last_line, len );
        wcpncpy( line + len, text, end-start+1 );

        line[len+end-start+1] = L'\0';

        vector_set( self->lines, self->lines->size-1, &line );
        free( last_line );
        if( (end-start)  < (wcslen( text )-1) )
        {
            console_print(self, end+1 );
        }
        return;
    }
    else
    {
        wchar_t *line = (wchar_t *) malloc( (len + wcslen(text) + 1) * sizeof( wchar_t ) );
        wcpncpy( line, last_line, len );
        wcpcpy( line + len, text );
        vector_set( self->lines, self->lines->size-1, &line );
        free( last_line );
        return;
    }
}

// --------------------------------------------------------- console_print ---
void
console_print( console_t *self, char *text )
{
    // Make sure there is at least one line
    if( self->lines->size == 0 )
    {
        char *line = strdup( "" );
        vector_push_back( self->lines, &line );
    }

    // Make sure last line does not end with '\n'
    char *last_line = *(char **) vector_get( self->lines, self->lines->size-1 ) ;
    if( strlen( last_line ) != 0 )
    {
        if( last_line[strlen( last_line ) - 1] == '\n' )
        {
            char *line = strdup( "" );
            vector_push_back( self->lines, &line );
        }
    }
    last_line = *(char **) vector_get( self->lines, self->lines->size-1 ) ;

    char *start = text;
    char *end   = strchr(start, '\n');
    size_t len = strlen( last_line );
    if( end != NULL)
    {
        char *line = (char *) malloc( (len + end - start + 2)*sizeof( char ) );
        strncpy( line, last_line, len );
        strncpy( line + len, text, end-start+1 );

        line[len+end-start+1] = '\0';

        vector_set( self->lines, self->lines->size-1, &line );
        free( last_line );
        if( (end-start)  < (strlen( text )-1) )
        {
            console_print(self, end+1 );
        }
        return;
    }
    else
    {
        char *line = (char *) malloc( (len + strlen(text) + 1) * sizeof( char ) );
        strncpy( line, last_line, len );
        strcpy( line + len, text );
        vector_set( self->lines, self->lines->size-1, &line );
        free( last_line );
        return;
    }
}