/*
 * Reports an error. reason should roughly give the reason for the error. Further
 * details can be supplied in the message (which may contain null bytes and whose
 * length is indicated by length). action describes which actions to take after the
 * error has been reported.
 */
void error(err_reason_t reason, char *message, int length, err_action_t action) {
	switch(action) {
	case EA_RESUME:
		dled_toggle();
#ifdef TRACE_ERRORS
		dled_blink((int) reason);
		debug_write(message, length);
#endif
		return;
		break;
	case EA_RESET:
		led_set_state(LED_STOP);
		
		dled_blink((int) reason);
		debug_write(message, length);
		
		// Reset the system
		SCB_AIRCR |= SCB_AIRCR_SYSRESETREQ;
		break;
	case EA_PANIC:
		led_set_state(LED_STOP);
		
		while (1) {
			dled_blink((int) reason);
			debug_write(message, length);
		}
		
		break;
	}
}

void debug_writeBuf(ObjectDesc * self, ArrayDesc * arr, jint off, jint len)
{
#ifndef NO_DEBUG_OUT
#ifndef SMP
	jint i;
#ifdef ALL_ARRAYS_32BIT
	u4_t *field = (u4_t *) (arr->data);
#else
	u1_t *field = (u1_t *) (arr->data);
#endif

	DISABLE_IRQ;
	for (i = off; i < off + len; i++) {
		debug_write(self, field[i]);
	}
	RESTORE_IRQ;
#else				/* use a spinlock to prevent trouble with other CPUs */
	static spinlock_t printf_lock = SPIN_LOCK_UNLOCKED;
	jint i;
#ifdef ALL_ARRAYS_32BIT
	u4_t *field = (u4_t *) (arr->data);
#else
	u1_t *field = (u1_t *) (arr->data);
#endif

	DISABLE_IRQ;
	spin_lock(&printf_lock);
	for (i = off; i < off + len; i++) {
		debug_write(self, field[i]);
	}
	spin_unlock(&printf_lock);
	RESTORE_IRQ;
#endif
#endif				/* NO_DEBUG_OUT */
}

void assert_failed(uint8_t* file, uint32_t line) {
  debug_write("-assert_failed: file ");
  debug_write((const char*) file);
  debug_write(" on line ");
  debug_write_u32(line, 10);
  debug_write_line("");

  /* Infinite loop */
  while (1) {
  }
}

static size_t gdb_consume_packet(uint8_t *data, size_t len)
{
   size_t done;

   debug(GDBSTUB_PKT, "consume (%D): [", len);
#ifdef CONFIG_GDBSTUB_PKT_DBG
   debug_write(data, len);
   debug_write((uint8_t*)"]\n", 2);
#endif

   while(len)
   {
      switch(*data)
      {
      case GDB_ACK_BYTE:
	 if(!gdb_enabled())
	 {
	    debug(GDBSTUB_PKT, "gdb connect\n");
	    gdb_enable();
	 }
	 done = 1;
	 break;

      case GDB_PKT_BYTE:
	 done = gdb_parse_packet(data, len);
	 if(!done)
	    goto __end;
	 break;

      case GDB_INT_BYTE:
	 done = 1;
	 debug(GDBSTUB_PKT, "interrupt sequence requested\n");
	 gdb_interrupt_sequence();
	 break;

      case GDB_NAK_BYTE:
	 done = 1;
	 gdb_ack();
	 break;

      default:
	 done = 1;
	 debug(GDBSTUB_PKT, "gdb_stub unsupported '\\x%x' (sz %D)\n", *data, len);
	 gdb_unsupported();
	 break;
      }

      len -= done;
      data += done;
   }

__end:
   return len;
}

void
debug_write_n_str (const char * str, int len)
{
  int pos;

  if (len > 0) {
    for (pos = 0; pos < len; pos++)
      debug_write (*str++);
  } else {
    while (*str) {
      debug_write (*str++);
    }
  }
}

int sys_DebugPrint( const char *fmt, char **pzArgs )
{
    if ( SysBase )
    {
        Thread_s *psThread = CURRENT_THREAD;
        char String[512];
        char zBuffer[1024];
        uint32 nFlg;

        nFlg = cli();
        if ( NULL != psThread )
        {
            Process_s *psProc;

            if ( ( psProc = CURRENT_PROC ) )
            {
                sprintf( String, "%d:%s::%s : %s", get_processor_id(), psProc->tc_zName, psThread->tr_zName, fmt );
            }
            else
            {
                sprintf( String, "%d:%s : %s", get_processor_id(), psThread->tr_zName, fmt );
            }
        }
        else
        {
            sprintf( String, "%d : %s", get_processor_id(), fmt );
        }
        put_cpu_flags( nFlg );
        sprintf( zBuffer, String, pzArgs[0], pzArgs[1], pzArgs[2], pzArgs[3], pzArgs[4], pzArgs[5], pzArgs[6], pzArgs[7], pzArgs[8], pzArgs[9], pzArgs[10], pzArgs[11], pzArgs[12] );

        debug_write( zBuffer, strlen( zBuffer ) );
    }
    return ( 0 );
}

int main ( void )
{
		utils_init();
		debug_init();
		pwm_in_init();
		pwm_out_init();
		_delay_ms(2000);
		orientation_init();

		char out[200];
		float dt = 0;
		uint32_t t = _micros();
	  uint32_t dt_c = 20000;
		for ( ; ; )
		{

				t = _micros();
				/* Read all data from sensor 1 */
				orientation_update( 20.0 );

				bzero( out , sizeof ( out ) );
				/* Format data */
				sprintf( out, "{\"x\":%f,\"y\":%f,\"z\":%f,\"xx\":%f,\"xy\":%f,\"xz\":%f}\n" , angles[PITCH], angles[ROLL], angles[YAW],acc_smooth[PITCH], acc_smooth[ROLL], acc_smooth[YAW]);

				debug_write( out );
				while((_micros() - t) < dt_c);

		}
}

static ssize_t dbg_write(int fd, const void *buf, size_t count)
{
	if (ps.opened)
		if (ps.fd == fd)
			return debug_write(ps.tid, (long *)buf, count);
	return count;
}

/**
 * @brief Create a UNIX DGRAM socket
 *
 * @param bind_path If not `NULL`, bind to `bind_path`.
 * @param flags Flags to pass to `socket(2)` (varies from OS to OS; look in the man pages)
 *
 * @retval >0 Success. Value is socket.
 * @retval <0 Error.
 */
int create_unix_dgram_socket(const char* bind_path, int flags)
{
    int sfd, retval;
    struct sockaddr_un saddr;

    if ( -1 == check_error(sfd = socket(AF_UNIX,SOCK_DGRAM|flags,0)) )
	return -1;

    memset(&saddr,0,sizeof(struct sockaddr_un));

    if ( bind_path != NULL )
    {
	if ( (retval = unlink(bind_path)) == -1 && errno != ENOENT ) // If there's another error than "doesn't exist"
	{
	    check_error(retval);
	    return -1;
	}

	if ( strlen(bind_path) > (sizeof(saddr.sun_path)-1) )
	{
# ifdef VERBOSE
	    debug_write("create_unix_dgram_socket: UNIX socket path too long\n");
# endif
	    return -1;
	}

	saddr.sun_family = AF_UNIX;
	strncpy(saddr.sun_path,bind_path,sizeof(saddr.sun_path));

	bind(sfd,(struct sockaddr*)&saddr,sizeof(struct sockaddr));
    }

    return sfd;
}

/**
 * @brief Create and connect a new UNIX STREAM socket.
 *
 * Creates and connects a new STREAM socket with the socket given in `path`.
 *
 * @retval >0 Success; return value is a socket file descriptor
 * @retval <0 Error.
 */
int create_unix_stream_socket(const char* path, int flags)
{
    struct sockaddr_un saddr;
    int sfd;

    if ( path == NULL )
	return -1;

    if ( -1 == check_error(sfd = socket(AF_UNIX,SOCK_STREAM|flags,0)) )
	return -1;

    memset(&saddr,0,sizeof(struct sockaddr_un));

    if ( strlen(path) > (sizeof(saddr.sun_path)-1) )
    {
# ifdef VERBOSE
	debug_write("create_unix_stream_socket: UNIX socket destination path too long\n");
# endif
	return -1;
    }

    saddr.sun_family = AF_UNIX;
    strncpy(saddr.sun_path,path,sizeof(saddr.sun_path)-1);

    if ( -1 == check_error(connect(sfd,(struct sockaddr*)&saddr,sizeof saddr)) )
	return -1;

    return sfd;
}

void on_usart1_irq() {
  char line[MAX_LINE_LENGTH];

  if (USART_GetITStatus(DEBUG_USART, USART_IT_RXNE) != RESET) {
    uint8_t data[1];
    data[0] = USART_ReceiveData(DEBUG_USART);

    ring_buffer_u8_write(&g_usartInputRingBuffer, data, 1);
    while (ring_buffer_u8_readline(&g_usartInputRingBuffer, line, MAX_LINE_LENGTH) > 0) {
      if(strcmp(line, "!CONNECT\n") == 0) {
        debug_write_line("+OK");
        debug_write_line("!clear");
        debug_write_line("!set name,stm32-wifi-ir");
        debug_write_line("!set description,'IR TX/RX over WiFi'");

        debug_write_line("?add widgets");
        debug_write_line("!add label,code,1,0,1,1");

        debug_write_line("!code.set minWidth,150");
        debug_write_line("!code.set title,'Code'");
        debug_write_line("!code.set text,'xxxxxxxxxxxxxxxxxxxx'");
      } else if(strncmp(line, "!TX", 3) == 0) {
        debug_write_line("+OK");
        IrCode* code = ir_code_getByIndex(0);
        ir_tx_send(code);
      } else {
        debug_write("?Unknown command: ");
        debug_write_line(line);
      }
    }
  }
}

size_t __vprintf(const char *format, va_list params)
{
   size_t retval;

   retval = __vsnprintf(vprint_buffer, sizeof(vprint_buffer), format, params);
   debug_write((uint8_t*)vprint_buffer, retval-1);
   return retval;
}

void vdebug(int level, const char* fmt, va_list args)
{
	if (level > DebugLevel) return;

	char buffer[256]; buffer[255] = '\0';
	vsnprintf(buffer, 255, fmt, args);
	debug_write(buffer, strlen(buffer));
}

int _write_r(struct _reent *re, int fd, const void *data, size_t len) {
	if (fd == 1 || fd == 2) {
		debug_write(data, len);
		return len;
	}
	
	return -1;
}

END_TEST

START_TEST(test_debug) {
    const char *errstr = NULL;
    int ostdout, spair[2];
    ssize_t len;
    struct pcap_file_header pcap_fhdr = {};
    struct parent_msg msg = {};
    char buf[2048];

    mark_point();
    if (isatty(STDOUT_FILENO)) { 
	errstr = "please redirect stdout to tcpdump or a file";
	my_log(CRIT, "check");
	WRAP_FATAL_START();
	debug_header();
	WRAP_FATAL_END();
	fail_unless (strncmp(check_wrap_errstr, errstr, strlen(errstr)) == 0,
	    "incorrect message logged: %s", check_wrap_errstr);
    }

    ostdout = dup(STDOUT_FILENO);
    my_socketpair(spair);
    dup2(spair[0], STDOUT_FILENO);
    close(ostdout);
    errstr = "check";
    my_log(CRIT, errstr);

    mark_point();
    debug_header();
    fflush(stdout);
    fail_unless (strncmp(check_wrap_errstr, errstr, strlen(errstr)) == 0,
        "incorrect message logged: %s", check_wrap_errstr);
    len = read(spair[1], &pcap_fhdr, sizeof(pcap_fhdr));
    fail_unless(len == sizeof(pcap_fhdr),
                "failed to read pcap header");
    fail_unless(pcap_fhdr.magic == PCAP_MAGIC,
                "invalid pcap header returned");
    fail_unless(pcap_fhdr.snaplen == ETHER_MAX_LEN,
                "invalid pcap header returned");
    fail_unless(pcap_fhdr.linktype == DLT_EN10MB,
                "invalid pcap header returned");

    mark_point();
    msg.len = ETHER_MIN_LEN;
    debug_write(&msg, 0);
    fflush(stdout);
    fail_unless (strncmp(check_wrap_errstr, errstr, strlen(errstr)) == 0,
        "incorrect message logged: %s", check_wrap_errstr);
    len = read(spair[1], buf, sizeof(buf));
    fail_unless(len == (PCAP_PKTHDR_SIZ + msg.len),
                "failed to read pcap record"); 

    close(spair[0]);
    close(spair[1]);
    dup2(ostdout, STDOUT_FILENO);
    close(ostdout);
}

/*
 * FUNCTION: pp_close
 * ------------------
 * closes the pretty print output file
 *
 * returns: 0 on successful completion, -1 on failure
 */
int
pp_close(){
    if (fclose(parserOutput) != 0)
    {
        debug_write("failed to close parser output file\n");
        return -1;
    }
    return 0;
}

/*
 * FUNCTION: pp_open
 * -----------------
 * creates and opens a pretty print output file
 *
 * filename: filename string for file creation
 *
 * returns: 0 on successful completion, -1 on failure
 */
int 
open_parser_output(char *filename){
    if ((parserOutput = fopen(filename, "w")) == NULL)
    {
        debug_write("failed to open parser output file\n");
        return -1;
    }
    return 0;
}

/* Helper function to display text to the debug log in Codescape
 * Returns: None */
void DebugOut( const char *p_pString )
{
#if defined DEBUG
	if( p_pString != NULL )
	{
		debug_write( SNASM_STDOUT, p_pString, strlen( p_pString ) );
	}
#endif
}

/**
 * FUNCTION: buffer_free
 * ---------------------
 * frees memory allocated to buffer
 */
void
buffer_free(Buffer * buffer){
    int i = 0;
    for (i = 0; i < BUFFER_SIZE; i++)
    {
        req_free(&buffer->requests[i]);
    }
    debug_write("free requests pointer\n");
    free(buffer->requests);
}

void local_debug(char *args,...)
{
	char szline[DEF_MAX_LOGLINE_LEN] = {0};
	va_list pArgList ;
	va_start(pArgList, args);
	vsprintf(szline, args, pArgList) ;
	va_end(pArgList);

	debug_write(DEF_LOGFILE_LOCAL_NAME, szline);
}