#include <TFT_eSPI.h> // Hardware-specific library
#include <ESP32Servo.h>

//声明TFT_ESPI对象
//本示例采用直接写屏方式
TFT_eSPI lcd;
TFT_eSPI &tft = lcd;
//声明舵机控制对象
Servo myServo;

//初始化显示
void init_lcd()
{
  tft.init();
  tft.setRotation(1); //屏幕旋转方向设置，0-3分别表示屏幕4个旋转方向
  tft.fillScreen(TFT_BLACK);
}
//显示雷达半径标线
void drawRadarCircle()
{
  //标线间距60像素，采用双线方式加粗显示
  //内环
  tft.drawCircle(159, 223, 60, TFT_GREEN);
  tft.drawCircle(159, 223, 59, TFT_GREEN);
  //中环
  tft.drawCircle(159, 223, 120, TFT_GREEN);
  tft.drawCircle(159, 223, 119, TFT_GREEN);
  //外环
  tft.drawCircle(159, 223, 180, TFT_GREEN);
  tft.drawCircle(159, 223, 179, TFT_GREEN);
  //清除轴线以外的圆形
  tft.fillRect(0, 224, 320, 16, TFT_BLACK);

}
//显示雷达角度标线
void drawAngleLine(int sx, int sy, int rad) {
  int i;
  int dx, dy;
  //每30度画一根直线作为标线
  for (i = 0; i <= 180; i += 30) {
    dx = rad * cos(i * 3.141592 / 180) + sx;
    dy = sy - rad * sin(i * 3.141592 / 180);
    tft.drawLine(sx, sy, dx, dy, TFT_GREEN);
  }
}

//显示雷达扫描线
void drawScanLine(int sx, int sy, int angle, int index) {
  int dx, dy;

  dx = 240 * cos(angle * 3.141592 / 180) + sx;
  dy = sy - 240 * sin(angle * 3.141592 / 180);
  if (angle % 30 == 0)
    return;
  tft.drawLine(sx, sy, dx, dy, tft.color565(0, 240 - index * 12, 0));

}
//显示目标点
void drawScanPoint(int sx, int sy, int angle, float distance)
{
  int dx, dy;

  int rad = distance * 3; // 20厘米显示在60像素位置，40厘米显示在120像素位置...
  if (distance == 0)
    return;
  dx = rad * cos(angle * 3.141592 / 180) + sx;
  dy = sy - rad * sin(angle * 3.141592 / 180);

  //如果距离小于80厘米(240像素)则在绿线之外画红线表示目标
  if (rad < 240)
  {
    tft.fillCircle(dx, dy, 3, tft.color565(0, 255, 0));
  }
}

//超声探头距离读取
float readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);  // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns
  // the sound wave travel time in microseconds
  return round(0.034002 / 2 * pulseIn(echoPin, HIGH) * 10) / 10;
}

//显示当前角度文本
void drawAngleText(int angle, float distance) {
  tft.setTextSize(2);
  //角度文本显示
  tft.setCursor(10, 226);
  tft.fillRect(10, 226, 180, 16, TFT_BLACK);
  tft.setTextColor(TFT_YELLOW);
  tft.print("Angle:");
  tft.print(angle);
  //距离文本显示
  tft.setCursor(150, 226);
  tft.fillRect(150, 226, 160, 16, TFT_BLACK);
  tft.setTextColor(TFT_GREEN);
  tft.print("Distance:");
  tft.print(distance);
  //内外环标识文本显示
  tft.setTextSize(1);
  tft.setCursor(200, 210);
  tft.setTextColor(TFT_GREEN);
  tft.print("20cm     40cm    60cm");
  //角度标线标识文本显示
  tft.setTextSize(1);
  tft.setCursor(0, 120);
  tft.setTextColor(TFT_GREEN);
  tft.print("30");
  tft.setCursor(60, 50);
  tft.print("60");
  tft.setCursor(157, 25);
  tft.print("90");
  tft.setCursor(240, 50);
  tft.print("120");
  tft.setCursor(300, 120);
  tft.print("150");
}

float distance_list[60];
int distance_index = 0;
void setup() {
  Serial.begin(115200);
  init_lcd();
  //初始化舵机对象连接
  myServo.attach(13);
  //初始化显示
  drawRadarCircle();
  drawAngleLine(159, 223, 240);
  for (int i = 0; i < 60; i ++)
  {
    distance_list[i] = 0;
  }


}
//雷达初始角度为0度
int servo_angle = 0;
//雷达初始方向为正向，扫描间距为3度
int servo_dir = 1;


void loop() {
  int i;
  //移动雷达角度
  servo_angle += servo_dir * 3;
  distance_index += servo_dir;
  //如果雷达角度大于180度，则反向旋转，并清屏重新绘制
  if (servo_angle >= 180)
  {
    distance_index = 59;
    servo_angle = 179;
    servo_dir = -1;
    tft.fillScreen(TFT_BLACK);
    drawRadarCircle();
    drawAngleLine(159, 223, 240);
  }
  //如果雷达角度小于0度，则正向旋转，并清屏重新绘制
  if (servo_angle < 0)
  {
    distance_index = 0;
    servo_angle = 0;
    servo_dir = 1;
    tft.fillScreen(TFT_BLACK);
    drawRadarCircle();
    drawAngleLine(159, 223, 240);
  }
  //设定舵机角度
  myServo.write(servo_angle);
  //读取当前超声探头距离
  float distance = readUltrasonicDistance(12, 14);
  distance_list[distance_index] = distance;
  //绘制扫描线，将扫描线角度镜像以使显示方向与舵机方向一致
  drawScanLine(159, 223, 180 - servo_angle, 0);
  if (servo_dir > 0)
  {
    for (int i = servo_angle, j = 0; i > 0; i -= 3, j++)
    {
      if (j == 20)
        break;
      drawScanLine(159, 223, 180 - i, j);

    }
  }
  if (servo_dir < 0)
  {
    for (int i = servo_angle, j = 0; i < 180; i += 3, j ++)
    {
      if (j == 20)
        break;
      drawScanLine(159, 223, 180 - i, j);

    }
  }
  for (i = 0; i < 60; i++)
  {
    drawScanPoint(159, 223, 180 - i * 3, distance_list[i]);

  }
  //重新绘制角度标线以免被扫描线完全覆盖
  drawAngleLine(159, 223, 240);
  drawAngleText(servo_angle, distance);
  delay(10); 
}