#include "GoPlus.h"

GoPlus::GoPlus() {}

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

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

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

uint16_t GoPlus::hub1_a_read_value(uint8_t reg) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(HUB1_ADDR | reg);
    Wire.endTransmission();

    uint8_t RegValue_L, RegValue_H;

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

    return RegValue_L;
}

uint16_t GoPlus::hub1_d_read_value(uint8_t reg) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(HUB1_ADDR | reg);
    Wire.endTransmission();

    uint8_t RegValue_L, RegValue_H;

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

    return (RegValue_H << 8) | RegValue_L;
}

uint16_t GoPlus::hub2_d_read_value(uint8_t reg) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(HUB2_ADDR | reg);
    Wire.endTransmission();

    uint8_t RegValue_L, RegValue_H;

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

    return (RegValue_H << 8) | RegValue_L;
}

uint16_t GoPlus::hub2_a_read_value(uint8_t reg) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(HUB2_ADDR | reg);
    Wire.endTransmission();

    uint8_t RegValue_L, RegValue_H;

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

    return RegValue_L;
}

uint16_t GoPlus::hub3_d_read_value(uint8_t reg) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(HUB3_ADDR | reg);
    Wire.endTransmission();

    uint8_t RegValue_L, RegValue_H;

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

    return (RegValue_H << 8) | RegValue_L;
}

uint16_t GoPlus::hub3_a_read_value(uint8_t reg) {
    Wire.beginTransmission(GOPLUS_ADDR);
    Wire.write(HUB3_ADDR | reg);
    Wire.endTransmission();

    uint8_t RegValue_L, RegValue_H;

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

    return RegValue_L;
}

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

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

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