#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <assert.h>
#include <string.h>

#define DIR_TO 0
#define DIR_FROM 1

void dump(int dir, unsigned char *pkt, int len)
{
  int i;
  fprintf(stderr, dir == DIR_TO ? ">>" : "<<");
  for(i = 0; i<len; i++)
    fprintf(stderr, " %02x", pkt[i]);
  fprintf(stderr, "\n");  
}

void print_packet(unsigned char *pkt)
{
  int pt = pkt[0] & 0x3f;
  switch(pt)
  {
    case 0:
      printf("-\n");
      break;
      
    case 1:
    {
      double dist, azi, incl, roll;
      int idist, iazi, iincl, iroll;
      
      idist = ((pkt[0] & 0x40) ? 1 : 0) << 16;
      idist |= pkt[1];
      idist |= pkt[2] << 8;
      
      iazi  = pkt[3];
      iazi |= pkt[4] << 8;
      
      iincl = pkt[5];
      iincl = pkt[6] << 8;
      
      iroll = pkt[7] << 8;
      
      dist = idist; azi = iazi; incl = iincl; roll = iroll;
      
      dist /= 1000;
      azi  *= 360.0 / 65536.0;
      incl *= 360.0 / 65536.0;
      roll *= 360.0 / 65536.0;
      
      if(incl > 180)
        incl -= 360.0; 
      
      printf("N\t%c\t%.3f\t%.1f\t%.1f\t%.1f\n", pkt[0] & 0x80 ? '*' : '-', dist, azi, incl, roll);
      break;
    }
    case 2:
    case 3:
    {
      int x = pkt[1] | (pkt[2] << 8), 
          y = pkt[3] | (pkt[4] << 8),
          z = pkt[5] | (pkt[6] << 8);

      if(x > 32767) x -= 65536;
      if(y > 32767) y -= 65536;
      if(z > 32767) z -= 65536;

      printf("C%c\t%c\t%d\t%d\t%d\n", pt == 2 ? 'A' : 'M',
            pkt[0] & 0x80 ? '*' : '-', x, y, z);
      break;
    }
    default:
      printf("?\t%02x\t%02x\t%02x\t%02x\t%02x\t%02x\t%02x\t%02x\n", pkt[0], pkt[1], pkt[2], pkt[3], pkt[4], pkt[5], pkt[6], pkt[7]);
      break;
  }
}

int main(int argc, char **argv)
{
  char * port = "/dev/rfcomm0";
  int s_addr = 0, e_addr = 0x7fff;
#define INTP_MEASUREMENT 1
#define INTP_HEX 2
#define INTP_READ 3
#define INTP_DEC 4
  int intp = INTP_MEASUREMENT, fd, addr, c, verbose = 0;
  char ** eptr = NULL;
  unsigned char pb[8];
  
  while((c = getopt(argc, argv, "p:s:e:mxhvrd")) != EOF)
  {
    switch(c)
    {
      case 'h':
      case '?':
        fprintf(stderr, "dxtool - simple distoX protocol implementation\n"
                        "Usage: dxtool [-p <port>] [-s <start addr>] [-e <end addr>] [-v] [-m | -x | -d | -r]\n\n"
                        "Available options:\n"
                        "  -p   Bluetooth port (default: /dev/rfcomm0)\n"
                        "  -v   Verbose operation (dump every sent/received packet)\n"
                        "\n"
                        "Operation modes:\n"
                        "  -m   Dump all stored measurements (default mode)\n"
                        "  -r   Wait for DistoX-initiated transmissions (new measurements)\n"
                        "  -x   Dump memory in hex digits\n"
                        "  -d   Dump memory in decimal\n"
                        "In -m/-x/-d modes use -s/-e options to specify start and end addresses (default: 0 ~ 0x7fff)\n"
                        "\n"
                        "Note 1: Before using dxtool, you need to power on the device and estabilish a SPP "
                        "connection, for example: sudo rfcomm connect /dev/rfcomm0 10:00:e8:be:a5:18 1\n\n"
                        "Note 2: During dxtool operation, it is neccessary to keep the device powered on"
                        " (ie. prevent the power-off timeout). For example, by pressing random keys"
                        " other than the measurement key.\n\n"
                        "Contact: Mateusz Golicz <mateusz.golicz@pza.org.pl>\n");
        return 1;
      case 'v':
        verbose = 1;
        break;
      case 'p':
        assert((port = strdup(optarg)));
        break;
      case 's':
        s_addr = strtol(optarg, &eptr, 0);
        if(*eptr)
        {
          fprintf(stderr, "Wrong start address: %s\n", optarg);
          return 1;
        }
        break;
      case 'e':
        e_addr = strtol(optarg, &eptr, 0);
        if(*eptr)
        {
          fprintf(stderr, "Wrong start address: %s\n", optarg);
          return 1;
        }
        break;

      case 'm':
        intp = INTP_MEASUREMENT;
        break;
      case 'x':
        intp = INTP_HEX;
        break;
      case 'r':
        intp = INTP_READ;    
        break;
      case 'd':
        intp = INTP_DEC;
        break;
    }
  }
  
  if((fd = open(port, O_RDWR)) == -1)
  {
    perror("open");
    return 1;
  }
  
  if(intp == INTP_READ)
  {
    unsigned char b[10];
    unsigned char c[10];
    int n = 0;
    
    while(read(fd, b, 8) == 8)
    {
      if(verbose) dump(DIR_FROM, b, 8);
      
      printf("%d\t", n++);
      print_packet(b);
      
      c[0] = (b[0] & 0x80) | 0x55;
      if(verbose) dump(DIR_TO, c, 1);
      if(write(fd, c, 1) != 1)
      {
        perror("write acknowledgement, 1 byte: write()");
        return 1;
      }
    }
    perror("read distoX initiated transmission, 8 bytes: read()");
    return 0;
  }
  
  for(addr = s_addr & 0xfffc; addr<e_addr; addr += 4)
  {
    unsigned char b[10];
    unsigned char c[10];
    b[0] = 0x38;
    b[1] = addr & 0xff;
    b[2] = (addr >> 8) & 0xff;
    
    if(verbose)
      dump(DIR_TO, b, 3);
    if(write(fd, b, 3) != 3)
    {
      perror("write command (3 bytes): write()");
      return 1;
    }
    
    if(read(fd, c, 8) != 8)
    {
      perror("read acknowledgement (8 bytes): read()");
      return 1;
    }
    if(verbose)
      dump(DIR_FROM, c, 8);
  
  
    if(c[0] != 0x38)
    {
      fprintf(stderr, "Invalid packet header, received 0x%02x, should be 0x38\n", c[0]);
      return 1;
    }
    
    if(c[1] != b[1] && c[2] != b[2])
    {
      fprintf(stderr, "Unexpected address in reply packet, received 0x%02x%02x, should be 0x%02x%02x\n", c[2], c[1], b[2], b[1]);
      return 1;
    }
    
    switch(intp)
    {
      case INTP_HEX:
        printf("%02x%02x: %02x %02x %02x %02x\n", c[2], c[1], c[3], c[4], c[5], c[6]);
        break;
      case INTP_DEC:
        printf("%d: %d %d %d %d\n", (c[2] << 8) | c[1], c[3], c[4], c[5], c[6]);
        break;
      case INTP_MEASUREMENT:        
        if(addr & 0x4)
        {
          pb[4] = c[3];
          pb[5] = c[4];
          pb[6] = c[5];
          pb[7] = c[6];
          printf("%04d\t", addr >> 3);
          print_packet(pb);
        }
        else
        {
          pb[0] = c[3];
          pb[1] = c[4];
          pb[2] = c[5];
          pb[3] = c[6];
        }
        break;
    }
    if(c[7] != 0)
    {
      fprintf(stderr, "Invalid packet tail, received 0x%02x, should be 0x00\n", c[7]);
      return 1;
    }
  }  
  
  close(fd);
  return 0;
}
