component hal_mydro;

option userspace;
option singleton yes;
option userinit yes;
option extra_cleanup;

pin in float xpos;
pin in float ypos;
pin in float zpos;
license "GPL";
author "[email protected]";
;;

#include <termios.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>

#define		B_Z0	5
#define		B_Y0	11
#define		B_X0	16


//                               0     1     2     3     4     5     6     7     8     9     -     E    FULL
unsigned char decode7seg[13] = {0xEE, 0x82, 0xDC, 0xD6, 0xB2, 0x76, 0x7E, 0xC2, 0xFE, 0xF6, 0x10, 0x7C, 0xff};


int fd;
unsigned char buffer[64];
struct termios options;
int i,j,l;
double x_val, y_val, z_val, k;
double t;
int ioerror = 0;
char* dev="/dev/ttyACM0";

static void aaauser_init(int argc, char **argv) {
  printf ("lol rofl\n");
  printf ("lol =%d\n",argc);
//  printf ("0 =%c\n",argv[0]);
  printf ("lol =%d\n",argc);
}

static void user_init(int argc, char **argv) {
  printf("hal_mydro: Trying to open (%s)\n",dev);

  fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
  if (fd <0) {
    printf("hal_mydro: Error opening (%s)\n",dev);
    exit(-EBUSY); 
  } else {
    printf("hal_mydro: Open successfull (%s)\n",dev);

    // open successful
    fcntl(fd, F_SETFL, 0);
    /* get the current options */
    tcgetattr(fd, &options);
    cfsetispeed(&options, B57600);
    cfsetospeed(&options, B57600);
    /* set raw input, 1 second timeout */
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_oflag &= ~OPOST;
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 10;
 
    options.c_cflag &= ~PARENB; /* Clear parity enable */
    options.c_iflag &= ~INPCK; /* Enable parity checking */
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    options.c_cflag &= ~CRTSCTS;
    // options.c_iflag &= ~(IXON | IXOFF | IXANY); /* no flow control */
    options.c_oflag &= ~(IXON | IXOFF | IXANY); /* no flow control */
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST; /* No output processing */
    options.c_oflag &= ~ONLCR; /* Don't convert linefeeds */ 
    /* Miscellaneous stuff */
    options.c_cflag |= (CLOCAL | CREAD); /* Enable receiver, set local */
    /* Linux seems to have problem with the following ??!! */
    options.c_cflag |= (IXON | IXOFF); /* Software flow control */
    options.c_lflag = 0; /* no local flags */ 
    options.c_cflag |= HUPCL; /* Drop DTR on close */
    /* Setup non blocking, return on 1 character */
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 1;
    /* Clear the line */
    tcflush(fd,TCIFLUSH);

    /* Update the options and do it NOW */
    if (tcsetattr(fd,TCSANOW,&options) != 0) {
      printf("configure port failed\n");
      exit(-EBUSY);
    }

    // get version schreiben
    buffer[0] = 0xAA;
    buffer[1] = 0x01;
    buffer[3] = 0x00;
    buffer[2] = buffer[0] ^ buffer[1] ^ buffer[3];
    write(fd,buffer,4);
    usleep(10000);

    // antwort lesen
    i = read(fd,buffer,64);
    printf("hal_mydro: VERSION: Got %d bytes :",i);
    for (j = 0; j<i; j++) {
      printf(" 0x%02x ",buffer[j]);
    }
    printf("\n");


    // count bytes schreiben
    buffer[0] = 0xAA;
    buffer[1] = 0x13;
    buffer[3] = 0x00;
    buffer[2] = buffer[0] ^ buffer[1] ^ buffer[3];
    write(fd,buffer,4);
    usleep(10000);

    i = read(fd,buffer,64);
    printf("hal_mydro: NBYTES: Got %d bytes :",i);
    for (j = 0; j<i; j++) {
      printf(" 0x%02x ",buffer[j]);
    }
    printf("\n");
  }
}


void user_mainloop() {
  printf("hal_mydro: entered mainloop...\n");
  while (ioerror == 0) {
    x_val = (double)xpos;
    y_val = (double)ypos;
    z_val = (double)zpos;
    /*x_val += M_PI;
    y_val = 100-x_val;
    z_val = y_val/3;*/

    // Header
    buffer[0] = 0xAA;
    buffer[1] = 0x10;
    buffer[2] = 0x00;
    buffer[3] = 0x12;

    // Z
    l=B_Z0;
    k = fabs(z_val);
    for(i=2; i>-3;i--) {
      t = k;
      j = (int)(k/pow(10,i));
      k = k - j* pow(10,i); 
      //printf("Z: i=%d j=%d, t=%4.2f k=%4.2f \n",i,j,t,k);
      if (j > 9) { j = 11;}
      buffer[l++] = decode7seg[j];
    }
    // Minus-Zeichen?
    if (z_val < 0) {
      if (buffer[B_Z0] == decode7seg[0]) {
        buffer[B_Z0] = decode7seg[10];
      } else {
        buffer[B_Z0] = decode7seg[11]; // Error
      }
    }
    buffer[7] += 1;  //Dezimalpunkt
    buffer[4] = 0xff;   //LEDs
  
    // Y
    l=B_Y0;
    k = fabs(y_val);
    for(i=2; i>-3;i--) {
      j = (int)(k/pow(10,i));
      k = k - j* pow(10,i); 
      //printf("Y: i=%d j=%d, k=%4.2f \n",i,j,k);
      if (j > 9) { j = 11;}
      buffer[l++] = decode7seg[j];
    }
    // Minus-Zeichen?
    if (y_val < 0) {
      if (buffer[B_Y0] == decode7seg[0]) {
        buffer[B_Y0] = decode7seg[10];
      } else {
        buffer[B_Y0] = decode7seg[11]; // Error
      }
    }
    buffer[13] += 1;  //Dezimalpunkt
    buffer[10] = 0;   //Unbenutzt

    // X
    l=B_X0;
    k = fabs(x_val);
    for(i=3; i>-3;i--) {
      j = (int)(k/pow(10,i));
      k = k - j* pow(10,i); 
      if (j > 9) { j = 11;}
      //printf("X: i=%d j=%d, k=%4.2f \n",i,j,k);
      buffer[l++] = decode7seg[j];
    }
    // Minus-Zeichen?
    if (x_val < 0) {
      if (buffer[B_X0] == decode7seg[0]) {
        buffer[B_X0] = decode7seg[10];
      } else {
        buffer[B_X0] = decode7seg[11]; // Error
      }
    }
    //Dezimalpunkt
    buffer[19] += 1;  


    // calculate checksum
    buffer[2] = buffer[0] ^ buffer[1];
    for (j=3; j < 22; j++) {
      buffer[2] ^= buffer[j];
    }

    // schreiben
    write(fd,buffer,22);
    
    // antwort lesen
    i = read(fd,buffer,64);
    if (i != 4) {
      printf("hal_mydro: Got %d bytes :",i);
      for (j = 0; j<i; j++) {
        printf(" 0x%02x ",buffer[j]);
      }
      printf("\n"); 
      ioerror = 1;
    }


  //Delay
  usleep(50*1000);
  } // end while
  printf("hal_mydro: exit mainloop\n");

} // end main_loop


EXTRA_CLEANUP() {
  if (fd >=0) {
    close(fd);
  }
}