#include "GoPlus2.h"


GoPlus2::GoPlus2(){

}

void GoPlus2::begin(){
  Wire.begin();
}

void GoPlus2::Servo_write_angle(uint8_t number, uint8_t angle) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(SERVO_ADDR | number);
    Wire.write(angle);
    Wire.endTransmission();
}

void GoPlus2::Servo_write_plusewidth(uint8_t number,  uint16_t width) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(PLUSE_ADDR | number);
    uint8_t buff_out[2];
    buff_out[0] = width >> 8;
    buff_out[1] = width & 0xff;
    Wire.write(buff_out[0]);
    Wire.write(buff_out[1]);
    Wire.endTransmission();
}

void GoPlus2::Motor_write_speed(uint8_t number, int motor_speed) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(MOTOR_ADDR | number);
    Wire.write(motor_speed);
    Wire.endTransmission();
}

uint16_t GoPlus2::hub1_d_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_DIGITAL | reg);
  Wire.endTransmission();
  
  uint8_t RegValue_L;

  Wire.requestFrom(GOPLUS_ADDR,1);
  while(Wire.available()){
    RegValue_L = Wire.read();
  }
  return RegValue_L;   
} 


uint16_t GoPlus2::hub1_a_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_ANALOG | reg);
  Wire.endTransmission();
  
  uint8_t RegValue_L,RegValue_H;

  Wire.requestFrom(GOPLUS_ADDR,2);
  while(Wire.available()){
    RegValue_H = Wire.read();
    RegValue_L = Wire.read();
  }
  
  return (RegValue_H<<8) | RegValue_L;   
} 

uint16_t GoPlus2::hub2_a_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_ANALOG | reg);
  Wire.endTransmission();
  
  uint8_t RegValue_L,RegValue_H;

  Wire.requestFrom(GOPLUS_ADDR,2);
  while(Wire.available()){
    RegValue_H = Wire.read();
    RegValue_L = Wire.read();
  }
  
  return (RegValue_H<<8) | RegValue_L;   
} 

uint16_t GoPlus2::hub2_d_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_DIGITAL | reg);
  Wire.endTransmission();
  
  uint8_t RegValue_L;

  Wire.requestFrom(GOPLUS_ADDR,1);
  while(Wire.available()){
    RegValue_L = Wire.read();
  }
  
  return RegValue_L;   
} 

uint16_t GoPlus2::hub3_a_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_ANALOG | reg);
  Wire.endTransmission();
  
  uint8_t RegValue_L,RegValue_H;

  Wire.requestFrom(GOPLUS_ADDR,2);
  while(Wire.available()){
    RegValue_H = Wire.read();
    RegValue_L = Wire.read();
  }
  
  return (RegValue_H<<8) | RegValue_L;   
} 



uint16_t GoPlus2::hub3_d_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_DIGITAL | reg);
  Wire.endTransmission();
  
  uint8_t RegValue_L;

  Wire.requestFrom(GOPLUS_ADDR,1);
  while(Wire.available()){ 
    RegValue_L = Wire.read();
  }
  
  return RegValue_L;   
} 


void GoPlus2::hub1_wire_value(uint8_t reg,uint8_t value){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_WIRE | reg);
  Wire.write(value & 0xff);
  Wire.endTransmission(); 
} 

void GoPlus2::hub2_wire_value(uint8_t reg,uint8_t value){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_WIRE | reg);
  Wire.write(value & 0xff);
  Wire.endTransmission(); 
} 

void GoPlus2::hub3_wire_value(uint8_t reg,uint8_t value){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_WIRE | reg);
  Wire.write(value & 0xff);
  Wire.endTransmission(); 
} 


void GoPlus2::hub1_set_io(uint8_t reg, uint8_t value){
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_IO_SET | reg);
  Wire.write(value & 0xff);
  Wire.endTransmission(); 
}

void GoPlus2::hub2_set_io(uint8_t reg, uint8_t value){
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_IO_SET | reg);
  Wire.write(value & 0xff);
  Wire.endTransmission(); 
}

void GoPlus2::hub3_set_io(uint8_t reg, uint8_t value){
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_IO_SET | reg);
  Wire.write(value & 0xff);
  Wire.endTransmission(); 
}



uint8_t GoPlus2::hub1_d_o_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_O_DIGITAL | reg);
  Wire.endTransmission();
  
  uint8_t RegValue;

  Wire.requestFrom(GOPLUS_ADDR,1);
  while(Wire.available()){ 
    RegValue = Wire.read();
  }
  
  return RegValue;   
} 


uint8_t GoPlus2::hub2_d_o_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_O_DIGITAL | reg);
  Wire.endTransmission();
  
  uint8_t RegValue;

  Wire.requestFrom(GOPLUS_ADDR,1);
  while(Wire.available()){ 
    RegValue = Wire.read();
  }
  
  return RegValue;   
} 

uint8_t GoPlus2::hub3_d_o_read_value(uint8_t reg){
  
  Wire.beginTransmission(GOPLUS_ADDR);
  Wire.write(HUB_READ_O_DIGITAL | reg);
  Wire.endTransmission();
  
  uint8_t RegValue;

  Wire.requestFrom(GOPLUS_ADDR,1);
  while(Wire.available()){ 
    RegValue = Wire.read();
  }
  
  return RegValue;   
}