#include <stdlib.h>
#include <stdint.h>
#include <string.h>

#include <sys/types.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <linux/spi/spidev.h>

#include "spi.h"
#include "servo.h"

static struct spi_message spi_message;
static struct spi_result  spi_result;

static int      spi_fd;
static uint8_t  spi_mode;
static uint32_t spi_speed;


void
spi_open(uint8_t mode, uint32_t speed) {
   spi_fd=open("/dev/spidev1.0",O_RDWR);
   if(spi_fd==-1) {
      perror("spidev1.0");
      exit(20);
   }
}

void
spi_close() {
   close(spi_fd);
}

void
spi_update_servos() {
   int i;
   spi_result.status=SPI_BUSY; // assume busy
   while(spi_result.status) {
      if((spi_result.status==SPI_ERR)||(spi_result.status=SPI_DONE)) {
         spi_message.cmd=CMD_FINISH;
         write(spi_fd,&spi_message,1);
      } else if((spi_result.status==SPI_WAIT)) {
         spi_message.cmd=CMD_CONTINUE;
         write(spi_fd,&spi_message,1);
      }
      read(spi_fd,&spi_result,1);
   }

   spi_message.cmd=CMD_SET_SERVOS;
   spi_message.length=sizeof(servo_pwm);
   memcpy(spi_message.data,servo_pwm,sizeof(servo_pwm));
   
   spi_message.checksum=0x00;
   for(i=0;i<spi_message.length;i++) {
      spi_message.checksum^=spi_message.data[i];
   }
   write(spi_fd,&spi_message,sizeof(spi_message));

   while(spi_result.status!=SPI_WAIT) read(spi_fd,&spi_result,1);
   spi_message.cmd=CMD_CONTINUE;
   write(spi_fd,&spi_message,1);
   read(spi_fd,&spi_result,1);
   while(spi_result.status==SPI_BUSY) read(spi_fd,&spi_result,1);

}

void
spi_set_led(uint8_t led) {
   int i;
   spi_result.status=SPI_BUSY; // assume busy
   while(spi_result.status) {
      if((spi_result.status==SPI_ERR)||(spi_result.status=SPI_DONE)) {
         spi_message.cmd=CMD_FINISH;
         write(spi_fd,&spi_message,1);
      } else if((spi_result.status==SPI_WAIT)) {
         spi_message.cmd=CMD_CONTINUE;
         write(spi_fd,&spi_message,1);
      }
      read(spi_fd,&spi_result,1);
   }

   spi_message.cmd=CMD_SET_LED;
   spi_message.length=1;
   spi_message.data[0]=led;
   
   spi_message.checksum=0x00;
   for(i=0;i<spi_message.length;i++) {
      spi_message.checksum^=spi_message.data[i];
   }
   write(spi_fd,&spi_message,sizeof(spi_message));

   while(spi_result.status!=SPI_WAIT) read(spi_fd,&spi_result,1);
   spi_message.cmd=CMD_CONTINUE;
   write(spi_fd,&spi_message,1);
   read(spi_fd,&spi_result,1);
   while(spi_result.status==SPI_BUSY) read(spi_fd,&spi_result,1);
   
}

void
spi_set_laser(uint8_t led) {
   int i;
   spi_result.status=SPI_BUSY; // assume busy
   while(spi_result.status) {
      if((spi_result.status==SPI_ERR)||(spi_result.status=SPI_DONE)) {
         spi_message.cmd=CMD_FINISH;
         write(spi_fd,&spi_message,1);
      } else if((spi_result.status==SPI_WAIT)) {
         spi_message.cmd=CMD_CONTINUE;
         write(spi_fd,&spi_message,1);
      }
      read(spi_fd,&spi_result,1);
   }

   spi_message.cmd=CMD_SET_LASER;
   spi_message.length=1;
   spi_message.data[0]=led;
   
   spi_message.checksum=0x00;
   for(i=0;i<spi_message.length;i++) {
      spi_message.checksum^=spi_message.data[i];
   }
   write(spi_fd,&spi_message,sizeof(spi_message));

   while(spi_result.status!=SPI_WAIT) read(spi_fd,&spi_result,1);
   spi_message.cmd=CMD_CONTINUE;
   write(spi_fd,&spi_message,1);
   read(spi_fd,&spi_result,1);
   while(spi_result.status==SPI_BUSY) read(spi_fd,&spi_result,1);
   
}