/*-------------------------------------------------------*/
| MOTOR SPEED REG       | 0x00
| -------------------------------------------------------
| FRONT_LEFT_reg[0]        |  R/W  |  FRONT_LEFT SPEED
| FRONT_RIGHT_reg[1]       |  R/W  |  FRONT_RIGHT SPEED
| REAR_LEFT_reg[2]         |  R/W  |  REAR_LEFT SPEED
| REAR_RIGHT_reg[3]        |  R/W  |  REAR_RIGHT SPEED
/*-------------------------------------------------------

	Wire.beginTransmission(0x38);
	Wire.write(0x00);
	Wire.write(FRONT_LEFT SPEED);
	Wire.write(FRONT_RIGHT SPEED);
	Wire.write(REAR_LEFT SPEED);
	Wire.write(REAR_RIGHT SPEED);
	Wire.endTransmission();


/*-------------------------------------------------------*/
| RGB LED COLOR REG       | 0x10
| -------------------------------------------------------
| left/right_reg[0]  |  R/W  | left/right control
|             | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
|             | R | R | R | R | R | R | R | SELECT |
|             | -SELECT: 0 FRONT_LEFT RGB LED
|             |          1 FRONT_RIGHT RGB LED
| rgb_r_reg[1]  |  R/W  |  RED value
| rgb_g_reg[2]  |  R/W  |  Green value
| rgb_b_reg[3]  |  R/W  |  Blue value
/*-------------------------------------------------------

	Wire.beginTransmission(0x38);
	Wire.write(0x10);
	Wire.write(0);  // FRONT_LEFT RGB LED
	Wire.write(Red);
	Wire.write(Green);
	Wire.write(Blue);
	Wire.endTransmission();

	Wire.beginTransmission(0x38);
	Wire.write(0x10);
	Wire.write(1);  // FRONT_RIGHT RGB LED
	Wire.write(Red);
	Wire.write(Green);
	Wire.write(Blue);
	Wire.endTransmission();
 

/*
 * @Author: Sorzn
 * @Date: 2019-11-22 13:38:26
 * @LastEditTime: 2019-11-22 14:28:56
 * @Description: M5Stack Hat BUGC Lib
 * @FilePath: /M5StickC/examples/Hat/BUGC/bugC.cpp
 */

#include "bugC.h"

void BUGC::Init() {
    Wire.begin(SDA, SCL);
//  Wire.begin(0, 26);  // M5Stick-C

}

void BUGC::Write1Byte(uint8_t address, uint8_t Register_address, uint8_t data) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    Wire.write(data);
    Wire.endTransmission();
}

void BUGC::Write2Byte(uint8_t address, uint8_t Register_address,
                      uint16_t data) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    Wire.write(data >> 8);    // MSB
    Wire.write(data & 0xFF);  // LSB

    Wire.endTransmission();
}

void BUGC::WriteBytes(uint8_t address, uint8_t Register_address, uint8_t *data,
                      size_t size) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    for (int i = 0; i < size; i++) {
        Wire.write(*(data + i));
    }
    Wire.endTransmission();
}

uint8_t BUGC::ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count,
                        uint8_t *dest) {
    Wire.beginTransmission(address);  // Initialize the Tx buffer
    Wire.write(subAddress);           // Put slave register address in Tx buffer
    uint8_t i = 0;
    if (Wire.endTransmission(false) == 0 &&
        Wire.requestFrom(address, (uint8_t)count)) {
        while (Wire.available()) {
            dest[i++] = Wire.read();
        }
        return true;
    }
    return false;
}

//*******************************************************//

// if speed > 0; clockwise rotation
void BUGC::BugCSetSpeed(uint8_t pos, int8_t speed) {
    speed = (speed > 100) ? 100 : speed;
    speed = (speed < -100) ? -100 : speed;
    Write1Byte(BUGC_ADDR, pos, speed);
}

void BUGC::BugCSetAllSpeed(int8_t speed_0, int8_t speed_1, int8_t speed_2,
                           int8_t speed_3) {
    int8_t speed_out[4] = {speed_0, speed_1, speed_2, speed_3};
    for (uint8_t i = 0; i < 4; i++) {
        speed_out[i] = (speed_out[i] > 100) ? 100 : speed_out[i];
        speed_out[i] = (speed_out[i] < -100) ? -100 : speed_out[i];
    }
    WriteBytes(BUGC_ADDR, 0x00, (uint8_t *)speed_out, 4);
}

void BUGC::BugCSetColor(uint32_t color_left, uint32_t color_right) {
    uint8_t color_out[4];
    color_out[0] = 0;
    color_out[1] = (color_left & 0xff0000) >> 16;
    color_out[2] = (color_left & 0x00ff00) >> 8;
    color_out[3] = (color_left & 0x0000ff);
    WriteBytes(BUGC_ADDR, 0x10, color_out, 4);

    color_out[0] = 1;
    color_out[1] = (color_right & 0xff0000) >> 16;
    color_out[2] = (color_right & 0x00ff00) >> 8;
    color_out[3] = (color_right & 0x0000ff);
    WriteBytes(BUGC_ADDR, 0x10, color_out, 4);
}

/*
 * @Author: Sorzn
 * @Date: 2019-11-22 13:38:31
 * @LastEditTime: 2019-11-22 14:33:56
 * @Description: M5Stack Hat BUGC Lib
 * @FilePath: /M5StickC/examples/Hat/BUGC/bugC.h
 */

#ifndef __BUG_C_H
#define __BUG_C_H

#include "Arduino.h"
#include "Wire.h"

#define BUGC_ADDR 0x38

#define FRONT_LEFT  0
#define FRONT_RIGHT 1
#define REAR_LEFT   2
#define REAR_RIGHT  3

class BUGC {
   public:
    void Init();
    /**
     * @description: Set BugC single motor speed
     * @param pos: 0 ~ 3
     * @param speed: -100 ~ 100
     * @return:
     */
    void BugCSetSpeed(uint8_t pos, int8_t speed);

    /**
     * @description: Set BugC motor speed
     * @param speed_0: -100 ~ 100
     * @param speed_1: -100 ~ 100
     * @param speed_2: -100 ~ 100
     * @param speed_3: -100 ~ 100
     * @return:
     */
    void BugCSetAllSpeed(int8_t speed_0, int8_t speed_1, int8_t speed_2,
                         int8_t speed_3);

    /**
     * @description: Set BugC sk6812 color
     * @param color_left: (r << 16) | (g << 8) | b
     * @param color_right: (r << 16) | (g << 8) | b
     * @return:
     */
    void BugCSetColor(uint32_t color_left, uint32_t color_right);

   private:
    void Write1Byte(uint8_t address, uint8_t Register_address, uint8_t data);
    void Write2Byte(uint8_t address, uint8_t Register_address, uint16_t data);
    void WriteBytes(uint8_t address, uint8_t Register_address, uint8_t* data,
                    size_t size);
    uint8_t ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count,
                      uint8_t* dest);
};

#endif

#include "bugC.h"

BUGC BugC;
#define BUGC_ADDR       0x38
#define BUGC_LED_REG    0x10
#define BUGC_LED_LEFT   0x00
#define BUGC_LED_RIGHT  0x01
#define WAIT   20

void color(int velocity) {

	uint8_t R, G, B;

	velocity = velocity % 255;

	if (velocity < 85) {
		R = 255 - velocity * 3;
		G = 0;
		B = velocity * 3;
	} else if (velocity < 170 ) {
		velocity -= 85;
		R = 0;
		G = velocity * 3;
		B = 255 - velocity * 3;
	} else {
		velocity -= 170;
		R = velocity * 3;
		G = 255 - velocity * 3;
		B = 0;
	}

	Wire.beginTransmission(BUGC_ADDR);
	Wire.write(BUGC_LED_REG);
	Wire.write(BUGC_LED_LEFT);
	Wire.write(R);
	Wire.write(G);
	Wire.write(B);
	Wire.endTransmission();

	Wire.beginTransmission(BUGC_ADDR);
	Wire.write(BUGC_LED_REG);
	Wire.write(BUGC_LED_RIGHT);
	Wire.write(R);
	Wire.write(G);
	Wire.write(B);
	Wire.endTransmission();

	delay(WAIT);
}

void shou_led() {
	int i = 0;
	while(1) {
		color(i);
		if ((++i) == 255) i = 0;
	}
}

void stop() {
	BugC.BugCSetColor(0x100000, 0x100000);  // RED
	BugC.BugCSetAllSpeed(0, 0, 0, 0);
	delay(1000);
}

void forward(float speed) {
	int8_t leftF  = 100 * speed;
	int8_t rightF = -60 * speed;
	int8_t leftR  = 100 * speed;
	int8_t rightR = -60 * speed;
	BugC.BugCSetColor(0x000010, 0x000010); // BLUE
	BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR);
	delay(1000);
}

void back(float speed) {
	int8_t leftF  = -90 * speed;
	int8_t rightF =  80 * speed;
	int8_t leftR  = -90 * speed;
	int8_t rightR =  80 * speed;
	BugC.BugCSetColor(0x101000, 0x101000); // YELLOW
	BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR);
	delay(1000);
}

void forward_left(float speed) {
	int8_t leftF  =    0 * speed;
	int8_t rightF = -100 * speed;
	int8_t leftR  =    0 * speed;
	int8_t rightR =  -50 * speed;
	BugC.BugCSetColor(0x001000, 0x000000); // GREEN
	BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR);
	delay(1000);
}

void forward_right(float speed) {
	int8_t leftF  = 100 * speed;
	int8_t rightF =   0 * speed;
	int8_t leftR  =  50 * speed;
	int8_t rightR =   0 * speed;
	BugC.BugCSetColor(0x000000, 0x001000); // GREEN
	BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR);
	delay(1000);
}

void back_left(float speed) {
	int8_t leftF  =    0 * speed;
	int8_t rightF =   50 * speed;
	int8_t leftR  =    0 * speed;
	int8_t rightR =  100 * speed;
	BugC.BugCSetColor(0x001000, 0x000000); // GREEN
	BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR);
	delay(1000);
}

void back_right(float speed) {
	int8_t leftF  =  -50 * speed;
	int8_t rightF =    0 * speed;
	int8_t leftR  = -100 * speed;
	int8_t rightR =    0 * speed;
	BugC.BugCSetColor(0x000000, 0x001000); // GREEN
	BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR);
	delay(1000);
}

void rotate_left(float speed) {
	int8_t all = -30 * speed;
	BugC.BugCSetColor(0x001000, 0x000000); // GREEN
	BugC.BugCSetAllSpeed(all, all, all, all);
	delay(1000);
}

void rotate_right(float speed) {
	int8_t all = 30 * speed;
	BugC.BugCSetColor(0x000000, 0x001000); // GREEN
	BugC.BugCSetAllSpeed(all, all, all, all);
	delay(1000);
}

void setup() {

	delay(10000);

	BugC.Init();

	stop();
	forward(0.8);
	back(0.8);
	rotate_left(0.8);
	rotate_right(0.8);
	forward_left(0.8);
	forward_right(0.8);
	back_left(0.8);
	back_right(0.8);
	stop();

	while (1);
}

void loop() {}