/*
    Description: Use GoPLUS Module for four-channel servo test and three-channel ADC test.
*/
#include <M5Stack.h>
#include <Wire.h>
#include "GoPlus.h"

GoPlus  goPlus;

#define X_LOCAL 40
#define Y_LOCAL 30

#define X_OFFSET 160
#define Y_OFFSET 23

// Print the header for a display screen
void header(const char *string, uint16_t color)
{
    M5.Lcd.fillScreen(color);
    M5.Lcd.setTextSize(1);
    M5.Lcd.setTextColor(TFT_MAGENTA, TFT_BLUE);
    M5.Lcd.fillRect(0, 0, 320, 30, TFT_BLUE);
    M5.Lcd.setTextDatum(TC_DATUM);
    M5.Lcd.drawString(string, 160, 3, 4); 
    
}

void Motor(void){
    goPlus.Motor_write_speed(MOTOR_NUM0, (uint8_t)0x80, 250);
    goPlus.Motor_write_speed(MOTOR_NUM1, (uint8_t)0x80, 250);
    delay(500);
    goPlus.Motor_write_speed(MOTOR_NUM0, (uint8_t)0x80, 0);
    goPlus.Motor_write_speed(MOTOR_NUM1, (uint8_t)0x00, 0);
    delay(500);

    goPlus.Motor_write_speed(MOTOR_NUM1, (uint8_t)0x00, 250);
    goPlus.Motor_write_speed(MOTOR_NUM0, (uint8_t)0x00, 250);
    delay(500);
    goPlus.Motor_write_speed(MOTOR_NUM0, (uint8_t)0x80, 0);
    goPlus.Motor_write_speed(MOTOR_NUM1, (uint8_t)0x00, 0);
    delay(500);
}

void setup() {
  // put your setup code here, to run once:
   M5.begin();
   M5.Power.begin();
   goPlus.begin();
   header("G O P L U S",TFT_BLACK);
   M5.Lcd.setTextColor(TFT_MAGENTA, TFT_BLACK);

   M5.Lcd.setCursor(X_LOCAL, Y_LOCAL, 2);
   M5.Lcd.printf("Read analog\n"); 

  M5.Lcd.setCursor(X_LOCAL+X_OFFSET, Y_LOCAL, 2);
  M5.Lcd.printf("Test Servo\n"); 

  M5.Lcd.setCursor(X_OFFSET-60, Y_LOCAL+Y_OFFSET*5, 2);
  M5.Lcd.printf("Test MoTor\n"); 

  M5.Lcd.setTextColor(TFT_WHITE, TFT_BLACK);
   pinMode(13,OUTPUT);
}
uint8_t a = 20;
uint16_t adInValue[6] = {0};
int led = 0;
void loop() {
    // put your main code here, to run repeatedly:
    digitalWrite(13, led);
    goPlus.Servo_write_angle(SERVO_NUM0,  a); delay(100);
    goPlus.Servo_write_angle(SERVO_NUM1 , a); delay(100);
    goPlus.Servo_write_angle(SERVO_NUM2 , a); delay(100);
    goPlus.Servo_write_angle(SERVO_NUM3 , a); delay(100);
  
    if (a > 180) {
      a = 10;
    }
    a = a + 170;

    M5.Lcd.fillRect(X_LOCAL+X_OFFSET, Y_LOCAL+Y_OFFSET, 100, 23, TFT_BLACK);
    M5.Lcd.setCursor(X_LOCAL+X_OFFSET, Y_LOCAL+Y_OFFSET, 2);
    M5.Lcd.printf("Angle:%d",a);


  
    adInValue[0] = goPlus.hub1_d_read_value(HUB_NUM0);
    delay(50);
    M5.Lcd.fillRect(X_LOCAL, Y_LOCAL+Y_OFFSET*1, 100, 23, TFT_BLACK);
    M5.Lcd.setCursor(X_LOCAL, Y_LOCAL+Y_OFFSET*1, 2);
    M5.Lcd.printf("A0:%d",adInValue[0]); 

    adInValue[1] = goPlus.hub2_d_read_value(HUB_NUM0);
    delay(50);
    M5.Lcd.fillRect(X_LOCAL, Y_LOCAL+Y_OFFSET*2, 100, 23, TFT_BLACK);
    M5.Lcd.setCursor(X_LOCAL, Y_LOCAL+Y_OFFSET*2, 2);
    M5.Lcd.printf("A1:%d",adInValue[1]); 

    adInValue[2] = goPlus.hub3_d_read_value(HUB_NUM0);
    delay(50);
    M5.Lcd.fillRect(X_LOCAL, Y_LOCAL+Y_OFFSET*3, 100, 23, TFT_BLACK);
    M5.Lcd.setCursor(X_LOCAL, Y_LOCAL+Y_OFFSET*3, 2);
    M5.Lcd.printf("A2:%d",adInValue[2]); 

    led = !led;
    Motor(); 
}