// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2017/08/30
//  filename:   NeoM8N.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet integrant le recepteur gps ublox NeoM8N
//
//
/*********************************************************************/
#ifdef ARMV7A

#include "NeoM8N.h"
#include <SerialPort.h>
#include <FrameworkManager.h>
#include <string.h>

using std::string;
using namespace flair::core;

namespace flair {
namespace sensor {

NeoM8N::NeoM8N(string name,
             SerialPort *serialport,NmeaGps::NMEAFlags_t NMEAFlags,uint8_t priority)
    : NmeaGps(name, NMEAFlags), Thread(getFrameworkManager(), name, priority) {
  this->serialport = serialport;
  serialport->SetRxTimeout(100000000);
  
  SetIsReady(true);
}

NeoM8N::~NeoM8N() {
  SafeStop();
  Join();
}

void NeoM8N::Run(void) {
  
  //turn off all messages
  {
    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0,0};
    for(int i=0;i<0x100;i++) {
      tx[5]=i;//msg id
      SendUbx(tx,sizeof(tx));
    }
  }
  //10Hz output
  {
    char tx[]={0x06, 0x08, 0x06, 0, 0x64, 0, 0x01, 0, 0, 0};
    SendUbx(tx,sizeof(tx));
  }
  
  if ((NMEAFlags & GGA) != 0) {
    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0,0x01};
    SendUbx(tx,sizeof(tx));
  }
  if ((NMEAFlags & VTG) != 0) {
    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x05,0x01};
    SendUbx(tx,sizeof(tx));
  }
  if ((NMEAFlags & GST) != 0) {
    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x07,0x01};
    SendUbx(tx,sizeof(tx));
  }
  if ((NMEAFlags & GSA) != 0) {
    char tx[]={0x06, 0x01, 0x03, 0, 0xf0, 0x02,0x01};
    SendUbx(tx,sizeof(tx));
  }
  
  /** Debut running loop **/
  WarnUponSwitches(true);

  Sync();
  
  while (!ToBeStopped()) {
    char response[1024] = {0};
    int size = 0;
    while (!ToBeStopped()) {
      ssize_t read = serialport->Read(&response[size], 1);
      if (read < 0) {
        Thread::Err("erreur Read (%s)\n", strerror(-read));
      }

      if (response[size] == 0x0a ) break;
      size++;
      if (size==sizeof(response)) break;
    }
    if (size!=sizeof(response)) {
      size++;
      parseFrame(response, size);
    } else {
      Thread::Warn("frame too long for buffer\n");
    }
  }
  /** fin running loop **/
  WarnUponSwitches(false);
}

void NeoM8N::SendUbx(char* buf,size_t length) {
  char header[2]={0xb5,0x62};
  int written=serialport->Write(header, sizeof(header));
  if (written< 0) {
      Thread::Err("erreur Write (%s)\n", strerror(-written));
  }
  
  written=serialport->Write(buf, length);
  if (written< 0) {
      Thread::Err("erreur Write (%s)\n", strerror(-written));
  }
  
  unsigned char checksum[2]={0,0};
  for(int i=0;i<length;i++) {
     checksum[0] = checksum[0] + buf[i];
     checksum[1] = checksum[1] + checksum[0];
  }
  written=serialport->Write(checksum, sizeof(checksum));
  if (written< 0) {
      Thread::Err("erreur Write (%s)\n", strerror(-written));
  }
}

void NeoM8N::Sync(void) {
  char data = 0;
  ssize_t read = 0;

  // attente fin trame
  while (data != 0x0a && !ToBeStopped()) {
    read = serialport->Read(&data, 1);
    SleepMS(10);

    if (read < 0) {
      Thread::Err("erreur Read (%s)\n", strerror(-read));
    }
  }
}

} // end namespace sensor
} // end namespace flair

#endif