/***
 * This example expects the serial port has a loopback on it.
 *
 * Alternatively, you could use an Arduino:
 *
 * <pre>
 *  void setup() {
 *    Serial.begin(<insert your baudrate here>);
 *  }
 *
 *  void loop() {
 *    if (Serial.available()) {
 *      Serial.write(Serial.read());
 *    }
 *  }
 * </pre>
 */

#include <string>
#include <iostream>
#include <cstdio>

// OS Specific sleep
#ifdef _WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif

#include "serial/serial.h"
#include <chrono>

using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;

#include "JY901.h"
CJY901 JY901;

void my_sleep(unsigned long milliseconds) {
#ifdef _WIN32
      Sleep(milliseconds); // 100 ms
#else
      usleep(milliseconds*1000); // 100 ms
#endif
}

void enumerate_ports()
{
	vector<serial::PortInfo> devices_found = serial::list_ports();

	vector<serial::PortInfo>::iterator iter = devices_found.begin();

	while( iter != devices_found.end() )
	{
		serial::PortInfo device = *iter++;

		printf( "(%s, %s, %s)\n", device.port.c_str(), device.description.c_str(),
     device.hardware_id.c_str() );
	}
}

void print_usage()
{
	cerr << "Usage: test_serial {-e|<serial port address>} ";
    cerr << "<baudrate> [test string]" << endl;
}

int run(int argc, char **argv)
{
  if(argc < 2) {
	  print_usage();
    return 0;
  }

  // Argument 1 is the serial port or enumerate flag
  string port(argv[1]);

  if( port == "-e" ) {
	  enumerate_ports();
	  return 0;
  }
  else if( argc < 3 ) {
	  print_usage();
	  return 1;
  }

  // Argument 2 is the baudrate
  unsigned long baud = 0;
#if defined(WIN32) && !defined(__MINGW32__)
  sscanf_s(argv[2], "%lu", &baud);
#else
  sscanf(argv[2], "%lu", &baud);
#endif

  // port, baudrate, timeout in milliseconds
  serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));

  cout << "Is the serial port open?";
  if(my_serial.isOpen())
    cout << " Yes." << endl;
  else
    cout << " No." << endl;

  // Get the Test string
  int count = 0;
  string test_string;
  if (argc == 4) {
    test_string = argv[3];
  } else {
    test_string = "Testing.";
  }

  // Test the timeout, there should be 1 second between prints
  cout << "Timeout == 1000ms, asking for 1 more byte than written." << endl;
  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
  while (count < 200) {
    //size_t bytes_wrote = my_serial.write(test_string);

    //string result = my_serial.read(test_string.length()+14);
    std::vector<uint8_t> buff;
    my_serial.read(buff, 11);
    cout << "Iteration: " << count << ", Bytes read: ";
    cout << buff.size() << ", String read: " ;
    for (size_t i = 0; i < buff.size(); i++)
    {
      cout<< /*std::ios::dec <<*/ (int)(char)buff[i] <<"\t";
    }
    cout<< endl;
    count += 1;
  }
  std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
  std::cout<<"time: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;

  count=1000;
  int start_index=0,end_index=100;
  while(count-->0){
    std::vector<uint8_t> buff;
    my_serial.read(buff, 100);
    if (JY901.CopeSerialData((char *)buff.data(), start_index, end_index, 100))
    {

      printf("Acc:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[2] / 32768 * 16 * 9.8);
      printf("Gyro:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0);
      printf("Angle:\t%.3f\t %.3f\t %.3f\r\n", (float)JY901.stcAngle.Angle[0] / 32768 * M_PI, (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, (float)JY901.stcAngle.Angle[2] / 32768 * M_PI);
      std::cout << "----------------------" << std::endl;
    }
    // printf("Time:20%d-%d-%d %d:%d:%.3f\r\n",(short)JY901.stcTime.ucYear,(short)JY901.stcTime.ucMonth,
    //     (short)JY901.stcTime.ucDay,(short)JY901.stcTime.ucHour,(short)JY901.stcTime.ucMinute,(float)JY901.stcTime.ucSecond+(float)JY901.stcTime.usMiliSecond/1000);

    // printf("Acc:%.3f %.3f %.3f\r\n",(float)JY901.stcAcc.a[0]/32768*16,(float)JY901.stcAcc.a[1]/32768*16,(float)JY901.stcAcc.a[2]/32768*16);

    // printf("Gyro:%.3f %.3f %.3f\r\n",(float)JY901.stcGyro.w[0]/32768*2000,(float)JY901.stcGyro.w[1]/32768*2000,(float)JY901.stcGyro.w[2]/32768*2000);

    // printf("Angle:%.3f %.3f %.3f\r\n",(float)JY901.stcAngle.Angle[0]/32768*180,(float)JY901.stcAngle.Angle[1]/32768*180,(float)JY901.stcAngle.Angle[2]/32768*180);

    // printf("Mag:%d %d %d\r\n",JY901.stcMag.h[0],JY901.stcMag.h[1],JY901.stcMag.h[2]);

    // printf("Pressure:%lx Height%.2f\r\n",JY901.stcPress.lPressure,(float)JY901.stcPress.lAltitude/100);

    // printf("DStatus:%d %d %d %d\r\n",JY901.stcDStatus.sDStatus[0],JY901.stcDStatus.sDStatus[1],JY901.stcDStatus.sDStatus[2],JY901.stcDStatus.sDStatus[3]);

    // printf("Longitude:%ldDeg%.5fm Lattitude:%ldDeg%.5fm\r\n",JY901.stcLonLat.lLon/10000000,(double)(JY901.stcLonLat.lLon % 10000000)/1e5,JY901.stcLonLat.lLat/10000000,(double)(JY901.stcLonLat.lLat % 10000000)/1e5);

    // printf("GPSHeight:%.1fm GPSYaw:%.1fDeg GPSV:%.3fkm/h\r\n\r\n",(float)JY901.stcGPSV.sGPSHeight/10,(float)JY901.stcGPSV.sGPSYaw/10,(float)JY901.stcGPSV.lGPSVelocity/1000);

    usleep(5*1000);
  }

  return 0;
}

int main(int argc, char **argv) {
  try {
    return run(argc, argv);
  } catch (exception &e) {
    cerr << "Unhandled Exception: " << e.what() << endl;
  }
}
