/* A simple SocketCAN example */

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

int can_port, gps_port, cur_port;

int open_can_port(const char *port)
{
    struct ifreq ifr;
    struct sockaddr_can addr;

    /* open socket */
    can_port = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if(can_port < 0)
    {
        return (-1);
    }

    addr.can_family = AF_CAN;
    strcpy(ifr.ifr_name, port);

    if (ioctl(can_port, SIOCGIFINDEX, &ifr) < 0)
    {

        return (-1);
    }

    addr.can_ifindex = ifr.ifr_ifindex;

    fcntl(can_port, F_SETFL, O_NONBLOCK);

    if (bind(can_port, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    {

        return (-1);
    }

    return 0;
}

int send_can_port(struct can_frame *frame)
{
    int retval;
   retval = write(can_port, frame, sizeof(struct can_frame));
    if (retval != sizeof(struct can_frame))
    {
        return (-1);
    }
    else
    {
        return (0);
    }
}

unsigned createMask(unsigned a, unsigned b)
{
   unsigned i, r = 0;
   for (i=a; i<=b; i++)
       r |= 1 << i;

   return r;
}

/* this is just an example, run in a thread */
void read_ports()
{
   struct can_frame frame_rd;
   int recvbytes = 0;
   int i;
   int num = 0;

   unsigned seq_mask  = createMask(0,8);
   unsigned data_mask = createMask(9,13);
   unsigned src_mask  = createMask(14,17);
   while(1)
   {
      char buffer[200];
      struct timeval timeout = {1, 0};
      unsigned seq_num,data_id,src_id;
      fd_set readSet;
      FD_ZERO(&readSet);
      FD_SET(can_port, &readSet);
      FD_SET(gps_port, &readSet);
      FD_SET(cur_port, &readSet);

      if (select((cur_port + 1), &readSet, NULL, NULL, &timeout) >= 0)
      {
         if (FD_ISSET(can_port, &readSet))
         {
//            printf("%08x %08x %08x\n", seq_mask, data_mask, src_mask);
            seq_num = data_id = src_id = 0;
            recvbytes = read(can_port, &frame_rd, sizeof(struct can_frame));
            if(recvbytes)
            {
               seq_num = seq_mask & frame_rd.can_id;
               data_id = data_mask & frame_rd.can_id; data_id >>= 9;
               src_id  = src_mask & frame_rd.can_id;  src_id  >>= 14;
//               printf("%d: dlc = %d, srcID = %d, dataID = %d, seq# = %d", num++, frame_rd.can_dlc, src_id, data_id, seq_num);

#if 0
               char *id = (char*)&frame_rd.can_id;
               for(i=0;i<4;i++)
               {
                  printf("%2x ", id[i]);
               }

               printf(" data = ");
               for(i=0;i<frame_rd.can_dlc;i++)
               {
                  printf("%2x ", frame_rd.data[i]);
               }

#endif
//		         printf("\n"); 
            }
         }
         if (FD_ISSET(gps_port, &readSet))
         {
            int n = read(gps_port, buffer, sizeof(buffer)-1);
            if (n > 4)
            {
               buffer[n] = '\0';
//               printf("Got GPS data: ");
//               printf("%d bytes: %s\n", n, buffer);
            }
         }
         if (FD_ISSET(cur_port, &readSet))
         {
            printf("Got Current data: ");
            int n = read(cur_port, buffer, sizeof(buffer)-1);
            printf("%d bytes\n", n);
            int i;
            for (i = 0; i < n; i++)
               printf("N1[%d]:%2x ", i, buffer[i]);
            printf("\n");
         }
      }
   }

}

int close_port()
{
    close(can_port);
    return 0;
}

int main(void)
{
   open_can_port("can0");

   gps_port = open("/dev/ttyS1", O_RDONLY | O_NOCTTY);
   if (gps_port == -1)
   {
      /* Could not open the port. */
      perror("gps_port: Unable to open /dev/ttyS1 - ");
   }

   cur_port = open("/dev/ttyS0", O_RDONLY | O_NOCTTY);
   if (cur_port == -1)
   {
      /* Could not open the port. */
      perror("cur_port: Unable to open /dev/ttyS0 - ");
   }

   read_ports();
   return 0;
}