#include <inttypes.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <avr/io.h>

#include "uart.h"
#include "debug.h"
#include "common.h"
#include "protocol.h"

#define UART_PORT 0

void debug_dummy(char *fmt, ...) {}

void debug_init()
{
#ifdef DEBUG
#warning Compiling with debug-support!
  uart_init(UART_PORT);
  uart_baudrate(UART_PORT, 25, 0, 0, 0);
  uart_stopbits(UART_PORT, 1);
  uart_databits(UART_PORT, 8);
  uart_parity(UART_PORT, 'N');

  static FILE mystdout = FDEV_SETUP_STREAM(debug_putchar, NULL,
                                              _FDEV_SETUP_WRITE);
  stdout = &mystdout;
#endif
}

int debug_putchar(char c, FILE *stream)
{
#ifdef DEBUG
  if (c == '\n')
    uart_putchar(UART_PORT, '\r');
  uart_putchar(UART_PORT, c);
#endif
  return 1;
}

void debug_deinit(void)
{
#ifdef DEBUG
  uart_deinit(UART_PORT);
#endif
}

unsigned char debug_AsciiToHex(unsigned char high, unsigned char low)
{
  unsigned char new;

  // check if lower equal 9 ( ascii 57 )
  if(high <= 57) // high is a number
    high = high -48;
  else // high is a letter
    high = high -87;

  high = high << 4;
  high = high & 0xF0;
 
  // check if lower equal 9 ( ascii 57 )
  if(low <= 57) // high is a number
    low = low -48;
  else // high is a letter
    low = low -87;
  low = low & 0x0F;
 
  new = high | low;
 
  return new;
}

void debug_SendHex(unsigned char hex)
{
#ifdef DEBUG
  unsigned char high,low;
  // get highnibble
  high = hex & 0xF0;
  high = high >> 4;
 
  // get lownibble
  low = hex & 0x0F;
 
  if(high<=9)
    uart_putchar(UART_PORT, high+48);
  else
    uart_putchar(UART_PORT, high+87);
 
  if(low<=9)
    uart_putchar(UART_PORT, low+48);
  else
    uart_putchar(UART_PORT, low+87);

  //_debug_write("\r\n");
#endif
}