#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <fcntl.h>
#include <termios.h>

void Init_ServoController(int *fd){
  struct termios options;
  
  *fd = open("/dev/ttyACM1", O_RDWR | O_NOCTTY);
  if ((*fd) == -1)    {
    fprintf(stderr, "Error Opening Device");
    exit(1);
  }
  
  tcgetattr((*fd), &options);
  options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
  options.c_oflag &= ~ONLCR;
  options.c_oflag &= ~OPOST;
  tcsetattr((*fd), TCSANOW, &options);
}


void Set_Position(int fd, int Channel, float Value){
  char Command[4];
  int i;
  
  Command[0] = 0x84;
  Command[1] = Channel-1;
  i = 1500 + (8200-1500)*(Value/180.);
  Command[2] = i&0x7F;
  Command[3] = (i>>7)&0x7F;    
  if (write(fd,Command,4) == EOF)      {
    fprintf(stderr, "Error Writing Command");
    exit(1);
  }
  return;
}



int main(){
  int ServoController;  // Tall som refererer til hvilke servokontrolleren
  float Servo1_Pos; // Posisjon til servo 1 : skal ha verdi i område 0-180
  float Servo2_Pos; // Posisjon til servo 2 : skal ha verdi i område 0-180
  float t;          // Tiden
  int i;            // Tellevariabel
  
  Init_ServoController(&ServoController);
  
  // Går i en løkke over 1000 tidssteg :
  for(i=0;i<1000;i=i+1){
    // tiden for dette tidssteget
    t = i*10./1000;
    
    // Posisjon til servo1 for dette tidssteget
    Servo1_Pos = 180.*sin(3.1415*i/1000);
    // Posisjon til servo1 for dette tidssteget
    Servo2_Pos = 180.*i/1000;     
    
    // Gi beskjed om dette til servo-conktrolleren :
    Set_Position(ServoController, 1, Servo1_Pos);
    Set_Position(ServoController, 2, Servo2_Pos);
    
    // Skriv ut til skjermen :
    printf("%d %f\n", i, Servo1_Pos);
    // Sov et visst antall mikrosekunder :
    usleep(10000); // Sov 0.01 seund
  }
  
  return;
}