/*


 Servo Sends:
 [0][p3][p2][p1][p0][v2][v1][v0]
 [1][v9][v8][v7][v6][v5][v4][v3] 
 
 p0-p3 are paramiter number
 v0-v9 are value
 
 Paramiter numbers:
 0 servo position (returns position sensor value)
 1 servo current 
 2 device model   (returns 10 bit device model number)
 

 
 
 
 Servo Recieves:

 [0][c3][c2][c1][c0][v2][v1][v0]
 [1][v9][v8][v7][v6][v5][v4][v3]
 
 c0-c3 are command
 v0-v9 are value
 
cmd_num    command 
 0        listen (servo number) 256 = all                      {always obey command} // sticks through listen once
 1        ignore (servo number) 256 = all                      {always obey command} // overrides listen once
 2        One Time listen (servo number)                       {always obey command}
 3        set flags (flags)                                      { bitwise obey }
             0 enguage cached position                              {always obey command}
             1 turn servo on                                        {obey if listening}
             2 turn servo off                                       {obey if listening}
             3 set cmdpos to curpos                                 {obey if listening}
 4        set servo position (position)                        {obey if listening}
 5        set cached position (position)                       {obey if listening}
 6        get servo current  (servo number)                    {servo number} 
 7        get servo position (servo number)                    {servo number} 
 8        send device model  (servo number)                    {servo number}




*/


typedef enum servoFlags_e {
   ENGCACHE = 0,
   SERVOON  ,
   SERVOOFF , 
   CMD2CUR  
} servoFlags_t;

typedef struct servoBus_s {
  int busFD;   // bus file descriptor, aka serial port
  unsigned int lastGet; // the last servo data was requested from none = 256
} servoBus_t;



void Listen            ( servoBus_t this, uint servo );
void Ignore            ( servoBus_t this, uint servo );
void OneTimeListen     ( servoBus_t this, uint servo );
void ChainListen       ( servoBus_t this, uint startServo );

void SetFlags          ( servoBus_t this, servoFlags_t flags );
void SetServoPostion   ( servoBus_t this, uint position );
void SetCachedPosition ( servoBus_t this, uint position );

void GetServoCurrent   ( servoBus_t this, uint servo );
void GetServoPosition  ( servoBus_t this, uint servo );
void GetDeviceModel    ( servoBus_t this, uint servo );





void botcmd(unsigned char * buffer, unsigned int command, unsigned int argument){
   buffer[0] = (command << 3)   | (argument & 0x07);
   buffer[1] = ((argument >> 3 ) & 0x7F) | 0x80;
}




void Listen            ( servoBus_t this, uint servo ) { 
  unsigned char buffer[2];
  
  botcmd( buffer, 0, servo);
  write(this->busFD, buffer, 2);  
  return; 
}
 
void Ignore            ( servoBus_t this, uint servo ) { 
  unsigned char buffer[2];

  botcmd( buffer, 1, servo);
  write(this->busFD, buffer, 2);
  return; 
}
 
void OneTimeListen     ( servoBus_t this, uint servo ) { 
  unsigned char buffer[2];

  botcmd( buffer, 2, servo);
  write(this->busFD, buffer, 2);
  return; 
}
 
void ChainListen       ( servoBus_t this, uint startServo ) { 
  unsigned char buffer[2];
  
  OneTimeListen(startServo+512);
  return; 
}

void SetFlags          ( servoBus_t this, servoFlags_t flags ) { 
  unsigned char buffer[2];

  botcmd( buffer, 3, flags);
  write(this->busFD, buffer, 2);
  return; 
}
 
void SetServoPostion   ( servoBus_t this, uint position ) { 
  unsigned char buffer[2];

  botcmd( buffer, 4, position);
  write(this->busFD, buffer, 2);
  return; 
}
 
void SetCachedPosition ( servoBus_t this, uint position ) { 
  unsigned char buffer[2];

  botcmd( buffer, 5, servo);
  write(this->busFD, buffer, 2);
  return; 
}

void GetServoCurrent   ( servoBus_t this, uint servo ) { 
  unsigned char buffer[2];

  this->lastGet = servo;
  botcmd( buffer, 6, servo);
  write(this->busFD, buffer, 2);
  return; 
}
 
void GetServoPosition  ( servoBus_t this, uint servo ) { 
  unsigned char buffer[2];

  this->lastGet = servo;
  botcmd( buffer, 7, servo);
  write(this->busFD, buffer, 2);
  return; 
}
 
void GetDeviceModel    ( servoBus_t this, uint servo ) { 
  unsigned char buffer[2];

  this->lastGet = servo;
  botcmd( buffer, 8, servo);
  write(this->busFD, buffer, 2);
  return; 
}