#include "NxWMS.h"

NxWMSDev::NxWMSDev()
{
  pthread_mutex_init(&rlock, NULL);
  for(int i=0; i<3; i++) off_accs[i] = 0.0;
  for(int i=0; i<9; i++) accs_cmat_[i] = 0.0;
  for(int i=0; i<3; i++) accs_cmat_[i*4] = 1.0;
  m_wirelessflag = false;

  fd=NULL;
}

NxWMSDev::~NxWMSDev()
{
  CloseNxwms();
}

void NxWMSDev::InitializeNxwms(unsigned char prdct_id, unsigned char tid, int freq)
{
  id = tid;
  prdct_id_ = prdct_id;
  int wflag;
  if(m_wirelessflag==true) wflag=1;
  else wflag=0;

  while( stop_measure_with_check(fd, prdct_id_, id, wflag) == 0 ){
    printf("can't stop\n @ stop_measure");
    usleep(100*1000);
  }

  int tryn = 0;
  while (set_measurefrequency(fd, prdct_id_, id, freq, wflag) == 0){
    printf("can't connect @ set_measurefrequency\n");
    usleep(100 * 1000);
    if (tryn % 10 == 0) EndMeasure(); //これが無いと前回継続の時に進まない
  }
  while ( set_measuretime(fd, prdct_id_, id, 255, 59, 59, wflag) == 0 ){
    printf("can't connect @ set_measuretime\n");
    usleep(100 * 1000);
  }

  BeginMeasure();


  std::cerr << "Please set sensor at the right direction, and don't move!" << std::endl;
  // CalibAccs();
  CalibGyros();
}

void NxWMSDev::FinalizeNxwms(void)
{
  EndMeasure();
}

void NxWMSDev::BeginMeasure(void)
{
  int wflag;
  if(m_wirelessflag==true) wflag=1;
  else wflag=0;

  start_measure(fd, prdct_id_, id, MONITORMODE, wflag);
}

void NxWMSDev::EndMeasure(void)
{
  int wflag;
  if(m_wirelessflag==true) wflag=1;
  else wflag=0;

  stop_measure(fd, prdct_id_, id, wflag);
}

void NxWMSDev::CalibGyros(void)
{
  //calib
  int ret;
  int count=0;
  int wflag;
  if(m_wirelessflag==true) wflag=1;
  else wflag=0;

  int init_loop = 100;
  for(int i=0; i<init_loop; i++) get_sensordata(fd, prdct_id_, 2, accs, gyros, mags, wflag);


  for(int i=0; i<3; i++){ave_gyros[i]=0;}

  pthread_mutex_lock(&rlock);
  for(int i=0; i<CALIB_NUM; ){
    ret = get_sensordata(fd, prdct_id_, 2, accs, gyros, mags, wflag);
    if(ret==1){
      i++;
      for(int j=0; j<5; j++){
      	ave_gyros[0] += gyros[3*j+0];
      	ave_gyros[1] += gyros[3*j+1];
      	ave_gyros[2] += gyros[3*j+2];
      	count++;
      }

      // for(int j=0; j<3; j++){
      // 	ave_gyros[j] += gyros[j];
      // }
      // count++;

      std::cerr << ".";
    }}
  pthread_mutex_unlock(&rlock);
  if(count!=0){
    for(int i=0; i<3; i++) ave_gyros[i] /= (double) count;
  }
  std::cerr << "calib num is " << count << std::endl;
  std::cerr << "gyro offset is " << ave_gyros[0] << " "
	    << ave_gyros[1] << " " << ave_gyros[2] << std::endl;
}

void NxWMSDev::CalibAccs(void)
{
  //calib
  int i, ret;
  int count=0;
  int wflag;
  if(m_wirelessflag==true) wflag=1;
  else wflag=0;

  for(i=0; i<3; i++){off_accs[i]=0;}

  for(i=0; i<CALIB_NUM; i++){
    ret = get_sensordata(fd, prdct_id_, 0, accs, gyros, mags, wflag);

    if(ret==1){
      off_accs[0] += accs[0];
      off_accs[1] += accs[1];
      off_accs[2] += accs[2];
      count++;
    }
    usleep(5000);//20ms
    std::cerr << ".";
  }
  std::cerr << "calib count = " << count << std::endl;
  for(i=0; i<3; i++) off_accs[i] /= count;

  // x->0.0, y->0.0, z->-1.0
  off_accs[2]+=1.0;
}

void NxWMSDev::GetSensordata(int mode)
{
  int wflag;
  if(m_wirelessflag==true) wflag=1;
  else wflag=0;

  mode_ = mode;
  if(mode_== 2) packet_n_ = 5;
  else packet_n_ = 1;

  pthread_mutex_lock(&rlock);
  double tmpaccs[15];
  get_sensordata(fd, prdct_id_, mode_, tmpaccs, gyros, mags, wflag);

  for(int j=0; j<packet_n_; j++){
    int idx = 3*j;
    for(int i=0; i<3; i++){
      tmpaccs[idx+i]=   tmpaccs[idx+i] - off_accs[i];
      gyros[idx+i]=  gyros[idx+i] - ave_gyros[i];
    }}

  for(int i=0; i<packet_n_; i++){
    int idx = 3*i;
    accs[idx+0] = accs_cmat_[0] * tmpaccs[idx+0] + accs_cmat_[1] * tmpaccs[idx+1] + accs_cmat_[2] * tmpaccs[idx+2];
    accs[idx+1] = accs_cmat_[3] * tmpaccs[idx+0] + accs_cmat_[4] * tmpaccs[idx+1] + accs_cmat_[5] * tmpaccs[idx+2];
    accs[idx+2] = accs_cmat_[6] * tmpaccs[idx+0] + accs_cmat_[7] * tmpaccs[idx+1] + accs_cmat_[8] * tmpaccs[idx+2];
  }

  pthread_mutex_unlock(&rlock);
}

void NxWMSDev::CopyAccs(double *accsv){
  pthread_mutex_lock(&rlock);
  for(int i=0;i<packet_n_*3;i++) accsv[i] = accs[i];
  pthread_mutex_unlock(&rlock);
}

void NxWMSDev::CopyGyros(double *gyrosv){
  pthread_mutex_lock(&rlock);
  for(int i=0;i<packet_n_*3;i++) gyrosv[i] = gyros[i];
  pthread_mutex_unlock(&rlock);
}

void NxWMSDev::CopyMags(double *magsv){
  pthread_mutex_lock(&rlock);
  for(int i=0;i<packet_n_*3;i++) magsv[i] = mags[i];
  pthread_mutex_unlock(&rlock);
}

void NxWMSDev::OpenNxwms(std::string devname)
{
  if(fd==NULL)
    fd = open_nxwms((char*)devname.c_str());
}

void NxWMSDev::CloseNxwms(void)
{
  EndMeasure();
  close_nxwms(fd);
}

void NxWMSDev::SetWirelessFlag(int onoff)
{
  if (onoff==1) m_wirelessflag = true;
  else m_wirelessflag = false;
    
}

void NxWMSDev::setAccsOffset(double *accs_offset){
  for(int i=0; i<3; i++) off_accs[i] = accs_offset[i];
}

void NxWMSDev::setAccsConvMat(double *accs_cmat){
  for(int i=0; i<9; i++) accs_cmat_[i] = accs_cmat[i];
}
