/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2012 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt
-----------------------------------------------------------------------------
*/

var bConnected = 0;;
typeof tLastYaw = F;;

fun deleteOb(inst)=
  setPluginInstanceCbScenePreRender inst nil;
  _VUZIXCloseHeadTracker;
  set bConnected = 0;
  set tLastYaw = nil;
  0;;


fun cbNavPreRender(inst, sessionstr, etime)=
  if bConnected then nil else
  (
    set bConnected = _VUZIXOpenHeadTracker;
    if (!bConnected) then nil else
      SendPluginEvent inst "Connected" nil nil;
  );
  
  if !bConnected then nil else
  let _VUZIXGetHeadTrackerOrientation -> ret in
  if (ret == nil) then
  (
    set bConnected = 0;
    SendPluginEvent inst "Disconnected" nil nil;
    0;
  )
  else
  let ret -> [yaw pitch roll] in
  let yaw -. tLastYaw -> ryaw in
  let if ryaw >. 180.0 then ryaw -. 360.0 else if ryaw <. (-.180.0) then ryaw +. 360.0 else ryaw -> ryaw in
  (
    if(tLastYaw == nil) then nil else
    (
      SendPluginEvent inst "Control" (strcatn "0 0 0\n"::(ftoa pitch)::" "::(ftoa ryaw)::" "::(ftoa (-.roll))::"\n2"::nil) inst.INST_sName;

      //addLogMessage strcatn "Vuzix : x > "::(ftoa pitch)::" y > "::(ftoa yaw)::" z > "::(ftoa roll)::nil;
    );
    
    set tLastYaw = yaw;
    0;
  );
  0;;


fun newOb(inst)=
  setPluginInstanceCbDel inst @deleteOb;
  setPluginInstanceCbScenePreRender inst @cbNavPreRender;
  0;;


fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;