#include "main_functions.hpp"

/******************  声明  *******************************/
// 4个基站坐标
vec3d anchorArray[4];
// 1个测试坐标
vec3d tagArray[1];
// tag与4个坐标的距离 mm
int Range_deca[4];

/******************  声明  *******************************/

void printTestPos()
{

  int result = 0;

  vec3d report;

  result = GetLocation(&report, 1, &anchorArray[0], &Range_deca[0]);

  if (result == -1)
  {
    // error
    printf("error\r\n");
    return;
  }

  printf("result = %d\r\n\n", result);
  printf("tag.x=%.3f\r\ntag.y=%.3f\r\ntag.z=%.3f\r\n", report.x, report.y, report.z);
}

void printTestDis()
{
  for (int i = 0; i < 4; i = i + 1)
  {
    float dis_test = calc2pos(anchorArray[i], tagArray[0]);
    dis_test = dis_test * 1000;
    printf("Range_deca[%d] = %.3f;\r\n", i, dis_test);
  }
}

void setup()
{

  // 4个基站的坐标 uint:m
  anchorArray[0].x = 0.000; // anchor0.x uint:m
  anchorArray[0].y = 0.000; // anchor0.y uint:m
  anchorArray[0].z = 0.500; // anchor0.z uint:m

  anchorArray[1].x = 6.000; // anchor1.x uint:m
  anchorArray[1].y = 0.000; // anchor1.y uint:m
  anchorArray[1].z = 0.500; // anchor1.z uint:m

  anchorArray[2].x = 6.000; // anchor2.x uint:m
  anchorArray[2].y = 6.000; // anchor2.y uint:m
  anchorArray[2].z = 0.500; // anchor2.z uint:m

  anchorArray[3].x = 0.000; // anchor3.x uint:m
  anchorArray[3].y = 6.000; // anchor3.y uint:m
  anchorArray[3].z = 2.500; // anchor3.z uint:m

  // 标签坐标 uint:m
  tagArray[0].x = 1.000;
  tagArray[0].y = 0.000;
  tagArray[0].z = 0.5;

  // 标签分别到4个基站的距离 uint:mm
  Range_deca[0] = 1000.000;
  Range_deca[1] = 5000.000;
  Range_deca[2] = 7810.250;
  Range_deca[3] = 6403.125;

  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(100);
  Serial.println();
  Serial.println();
  printf("printTestDis:\r\n");
  printTestDis();
  Serial.println();
  Serial.println("printTestPos");
  printTestPos();
}

void loop()
{

  delay(1000); // this speeds up the simulation
}