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

GoPlus2  goPlus;

#define X_LOCAL 40
#define Y_LOCAL 30

#define X_OFFSET 160
#define Y_OFFSET 23
#define rrmt_item32_timemout_us 9500

int _hub1, hub1 = 0;
rmt_item32_t signals[1024];

size_t received = 0;




int flag = 0;
int num = 0;

void header(const char *string, uint16_t color)
{
    M5.Lcd.fillScreen(color);
    M5.Lcd.setTextSize(1);
    M5.Lcd.setTextColor(TFT_WHITE, TFT_BLACK);
    M5.Lcd.fillRect(0, 0, 320, 30, TFT_BLACK);
    M5.Lcd.setTextDatum(TC_DATUM);
    M5.Lcd.drawString(string, 160, 3, 4);   
}

void Motor()
{   
    while(num == 0){
      M5.Lcd.fillRect(0, 40, 320, 100, TFT_BLACK);
      M5.Lcd.setCursor(20, 40, 4);
      M5.Lcd.println("Motor Test");
      M5.Lcd.setCursor(20, 60, 4);
      M5.Lcd.println("Motor Speed: -127");
      goPlus.Motor_write_speed(MOTOR_NUM1, -127);
      goPlus.Motor_write_speed(MOTOR_NUM0, -127);
      delay(1000);
      goPlus.Motor_write_speed(MOTOR_NUM1, 0);
      goPlus.Motor_write_speed(MOTOR_NUM0, 0);
      delay(1000);
      M5.Lcd.fillRect(0, 60, 320, 180, TFT_BLACK);
      M5.Lcd.setCursor(20, 60, 4);
      M5.Lcd.println("Motor Speed: 127");
      goPlus.Motor_write_speed(MOTOR_NUM1, 127);
      goPlus.Motor_write_speed(MOTOR_NUM0, 127);
      delay(1000);
      goPlus.Motor_write_speed(MOTOR_NUM1, 0);
      goPlus.Motor_write_speed(MOTOR_NUM0, 0);
      delay(1000);
    }

}

void Servo()
{   
    while(num == 1){
      M5.Lcd.fillRect(0, 40, 320, 100, TFT_BLACK);
      M5.Lcd.setCursor(20, 40, 4);
      M5.Lcd.println("Servo Test");
      M5.Lcd.setCursor(20, 60, 4);
      M5.Lcd.println("Servo Angle: 0");    
      goPlus.Servo_write_angle(SERVO_NUM0,  0); 
      goPlus.Servo_write_angle(SERVO_NUM1 , 0); 
      goPlus.Servo_write_angle(SERVO_NUM2 , 0);
      goPlus.Servo_write_angle(SERVO_NUM3 , 0); 
      delay(1000);
      M5.Lcd.fillRect(0, 60, 320, 100, TFT_BLACK);
      M5.Lcd.setCursor(20, 60, 4);
      M5.Lcd.println("Pluse Width: 2000");
      goPlus.Servo_write_plusewidth(SERVO_NUM0_PW, 2500); 
      goPlus.Servo_write_plusewidth(SERVO_NUM1_PW, 2500);
      goPlus.Servo_write_plusewidth(SERVO_NUM2_PW, 2500); 
      goPlus.Servo_write_plusewidth(SERVO_NUM3_PW, 2500); 
      delay(1000);
  }
}

void Port_B()
{   
  M5.Lcd.fillRect(0, 40, 320, 100, TFT_BLACK);
  while(num == 2){ 
      
      M5.Lcd.setCursor(0, 40, 4);
       hub1 = goPlus.hub1_a_read_value(HUB1_R_ADDR);
      if(abs(hub1 - _hub1) > 3){
        M5.Lcd.fillRect(160, 40, 70, 20, TFT_BLACK);       
      }
      _hub1 = hub1;
      M5.Lcd.printf("HUB1 Analog: %d \r\n", hub1);
      int hub2 = goPlus.hub2_d_read_value(HUB2_R_O_ADDR);
      M5.Lcd.printf("HUB2 Digital: %d\r\n", hub2);
      if(M5.BtnC.wasPressed()){
        flag = !flag;
        goPlus.hub3_wire_value(HUB3_W_ADDR, flag);
      }
      M5.update();
      M5.Lcd.printf("HUB3 State: %d\r\n", flag);
     
    }
}



void rx_channel_init()
{
    rmt_config_t rmt_rx;
    rmt_rx.channel                        = RMT_CHANNEL_0;
    rmt_rx.gpio_num                       = GPIO_NUM_35;
    rmt_rx.clk_div                        = 80;
    rmt_rx.mem_block_num                  = 4;
    rmt_rx.rmt_mode                       = RMT_MODE_RX;
    rmt_rx.rx_config.filter_en            = false;
    rmt_rx.rx_config.filter_ticks_thresh  = 0;
    rmt_rx.rx_config.idle_threshold       = 5000;

    rmt_config(&rmt_rx);
    rmt_driver_install(rmt_rx.channel, 1000, 0);
    
}

void tx_channel_init() {
  rmt_config_t rmt_tx;
  rmt_tx.rmt_mode = RMT_MODE_TX;
  rmt_tx.channel = RMT_CHANNEL_4;
  rmt_tx.gpio_num = GPIO_NUM_5;
  rmt_tx.mem_block_num = 4;
  rmt_tx.clk_div = 80;
  rmt_tx.tx_config.loop_en = false;
  rmt_tx.tx_config.carrier_duty_percent = 50;
  rmt_tx.tx_config.carrier_freq_hz = 38000;
  rmt_tx.tx_config.carrier_level = RMT_CARRIER_LEVEL_HIGH;
  rmt_tx.tx_config.carrier_en = 1;
  rmt_tx.tx_config.idle_level = RMT_IDLE_LEVEL_LOW;
  rmt_tx.tx_config.idle_output_en = true;
  rmt_config(&rmt_tx);
  rmt_driver_install(rmt_tx.channel, 0, 0);
}


void rmt_rx_task() {
  RingbufHandle_t rb = NULL;
  rmt_get_ringbuf_handle(RMT_CHANNEL_0, &rb);
  rmt_rx_start(RMT_CHANNEL_0, 1);

  size_t rx_size = 0;
  M5.Lcd.setCursor(20, 40, 4);
  M5.Lcd.println("wait ir signal...");
  rmt_item32_t *item = (rmt_item32_t*)xRingbufferReceive(rb, &rx_size, 3000);
  rmt_rx_stop(RMT_CHANNEL_0);
 if(!item) {
    M5.Lcd.println("no data received");
    return;
  }
  M5.Lcd.print("received items: ");
  M5.Lcd.println(rx_size);

  memcpy(signals, item, sizeof(rmt_item32_t) * rx_size);
  for (int i = 0; i < rx_size; ++i) {
    signals[i].level0 = ~signals[i].level0;
    signals[i].level1 = ~signals[i].level1;
  }
  received = rx_size;
  vRingbufferReturnItem(rb, (void*)item);
  M5.Lcd.println("recv done");
  rmt_rx_stop(RMT_CHANNEL_0);
  rmt_tx_task();
}

void rmt_tx_task() {
  M5.Lcd.println("send...");
  M5.Lcd.println(received);
  rmt_write_items(RMT_CHANNEL_4, signals, received, false);
  rmt_wait_tx_done(RMT_CHANNEL_4, 2000);
  M5.Lcd.println("send done");

}

void IR()
{   
    
    while(num == 3) {
      M5.Lcd.fillRect(0, 40, 320, 200, TFT_BLACK);
      rmt_rx_task();
    }
}

void doTask()
{
  if(num == 4){
    num = 0;
  }else{
    num++;
  }
  Serial.println(num);
}

void setup() {

   M5.begin();
   goPlus.begin();
   delay(100);
   rx_channel_init();
   tx_channel_init();

//   goPlus.hub1_set_io(HUB1_R_O_ADDR, 1); //set digital_output to digital_input
//   goPlus.hub2_set_io(HUB2_R_O_ADDR, 1); //set digital_output to digital_input
//   goPlus.hub3_set_io(HUB3_R_O_ADDR, 1); //set digital_output to digital_input

   header("GoPlus 2", TFT_BLACK);
   M5.Lcd.setTextColor(TFT_GREEN, TFT_BLACK);
   attachInterrupt(digitalPinToInterrupt(38), doTask, RISING);

}

void loop() {
   Servo();
   Motor();
   Port_B();
   IR();
  
//    int val1 = goPlus.hub1_d_o_read_value(HUB1_R_O_ADDR);  //read digtial_input
//    int val2 = goPlus.hub2_d_o_read_value(HUB2_R_O_ADDR);  //read digtial_input
//    int val3 = goPlus.hub3_d_o_read_value(HUB3_R_O_ADDR);  //read digtial_input
//    Serial.println(val1);
//    Serial.println(val2);
//    Serial.println(val3);

}