/*
-----------------------------------------------------------------------------
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

-----------------------------------------------------------------------------
*/


/*-------------------- Global variables --------------------*/
struct Fusion = [
  FUSION_inst                 : PInstance,
  FUSION_object               : ObjFusion,
  FUSION_iDataType            : I,
  FUSION_iLastButton          : I,
  FUSION_iLastStatus          : I,
  FUSION_father               : SO3_OBJECT,
  FUSION_iFatherMode          : I,
  FUSION_initpos              : [[F F F] [F F F F]],
  FUSION_tLastOrientation     : [F F F F],
  FUSION_tLastControl         : F,
  FUSION_bLocal               : I 
          
] mkFusion;;


/*------------------ Delete SpacePoint Fusion object -------------------*/
fun deleteOb(inst, fusionStr)=
  setPluginInstanceCbScenePreRender inst nil;
  
  // Destroy Fusion instance
	_CloseFusionDevice fusionStr.FUSION_object;
	
  setPluginInstanceCbCameraChange inst nil;
  
  let fusionStr.FUSION_initpos -> [pos quat] in
  (
    SO3ObjectSetPosition fusionStr.FUSION_father pos;
    SO3ObjectSetOrientation fusionStr.FUSION_father quat;
  );
0;;


fun cbNavPreRender(inst, sessionstr, etime, fusionStr)=  
  if (fusionStr.FUSION_tLastOrientation == nil) then nil else
  //let SO3MathsQuatAdd fusionStr.FUSION_tLastOrientation [0.0 1.0 0.0 0.0] -> quat in
  let fusionStr.FUSION_tLastOrientation -> quat in
  let SO3MathsQuatToEulerDegreePYR quat -> [pitch yaw roll] in
  (
    if(fusionStr.FUSION_tLastControl == nil) then nil else
    let fusionStr.FUSION_tLastControl -> ly in
    let yaw -. ly -> ryaw in
    let if ryaw >. 180.0 then ryaw -. 360.0 else if ryaw <. (-.180.0) then ryaw +. 360.0 else ryaw -> ryaw in
    (
      let ftoa pitch -> sax in
      let ftoa ryaw -> say in
      let ftoa roll -> saz in
        SendPluginEvent fusionStr.FUSION_inst "Control" (strcatn "0 0 0\n"::sax::" "::say::" "::saz::"\n2"::nil) fusionStr.FUSION_inst.INST_sName;
    );
    
    set fusionStr.FUSION_tLastControl = yaw;
  );
  0;;


/*----------------------- Callbacks ------------------------*/
/* New orientation data */
fun cbNewOrientationData(fusion, fusionStr, acc, quat)=
  let acc -> [accX accY accZ] in
  (
    if (fusionStr.FUSION_iFatherMode == 2) then
    (
    	//TODO manage physic for navigation get relativ Yaw pitch roll
      let if (fusionStr.FUSION_iFatherMode == 1) then SO3MathsQuatAdd quat [0.0 1.0 0.0 0.0] else quat -> rquat in
      if (fusionStr.FUSION_bLocal) then
        SO3ObjectSetOrientation fusionStr.FUSION_father rquat
      else
        SO3ObjectSetGlobalOrientation fusionStr.FUSION_father rquat;  
    )
    else
    (
      let if (fusionStr.FUSION_iFatherMode == 1) then SO3MathsQuatAdd quat [0.0 1.0 0.0 0.0] else quat -> rquat in
      if (fusionStr.FUSION_bLocal) then
        SO3ObjectSetOrientation fusionStr.FUSION_father rquat
      else
        SO3ObjectSetGlobalOrientation fusionStr.FUSION_father rquat;  
    );
    
    SendPluginEvent fusionStr.FUSION_inst "Orientation accel" strbuild ((ftoa accX)::(ftoa accY)::(ftoa accZ)::nil)::nil nil;
    let SO3MathsQuatToEulerYXZ quat -> [x y z] in
      SendPluginEvent fusionStr.FUSION_inst "Orientation" strcatnSep (ftoa (SO3MathsRadianToDegree x))::(ftoa (SO3MathsRadianToDegree y))::(ftoa (SO3MathsRadianToDegree z))::nil " " nil;
    
    set fusionStr.FUSION_tLastOrientation = quat;
  );
0;;

/* New raw data */
fun cbNewRawData(fusion, fusionStr, mag, acc, gyr, res)=
  let mag -> [magX magY magZ] in
  let acc -> [accX accY accZ] in
  let gyr -> [gyrX gyrY gyrZ] in
  let res -> [res1 res2] in
  (
    SendPluginEvent fusionStr.FUSION_inst "Raw mag" strbuild ((itoa magX)::(itoa magY)::(itoa magZ)::nil)::nil nil;
    SendPluginEvent fusionStr.FUSION_inst "Raw accel" strbuild ((itoa accX)::(itoa accY)::(itoa accZ)::nil)::nil nil;
    SendPluginEvent fusionStr.FUSION_inst "Raw gyr" strbuild ((itoa gyrX)::(itoa gyrY)::(itoa gyrZ)::nil)::nil nil;
    SendPluginEvent fusionStr.FUSION_inst "Raw res" strbuild ((itoa res1)::(itoa res2)::nil)::nil nil;
  );
0;;

/* Button pressed */
fun cbButton(fusion, fusionStr, buttons)=
  // Left button released
  if ((fusionStr.FUSION_iLastButton & 1) && !(buttons & 1)) then
    SendPluginEvent fusionStr.FUSION_inst "Left button released" nil nil
  else nil;
  
  // Right button released
  if ((fusionStr.FUSION_iLastButton & 2) && !(buttons & 2)) then
    SendPluginEvent fusionStr.FUSION_inst "Right button released" nil nil
  else nil;
  
  // Left button pressed
  if buttons & 1 then
  (
    SendPluginEvent fusionStr.FUSION_inst "Left button" nil nil;
  )
  
  // Right button pressed
  else if buttons & 2 then
  (
    SendPluginEvent fusionStr.FUSION_inst "Right button" nil nil;
  )
  else nil;
  
  // Update last button pressed
  set fusionStr.FUSION_iLastButton = buttons;
0;;

/* Update camera control */
fun cbChangeCamera(inst, viewstr, sessionstr, camera, fusionStr)=
  if (camera != V3DgetDefaultCamera c3dXsession) then nil else
  let V3DgetCameraByType sessionstr camera fusionStr.FUSION_iFatherMode -> nfather in
  let SO3ObjectGetPosition nfather -> cpos in
  let SO3ObjectGetOrientation nfather -> cquat in
  (
    let fusionStr.FUSION_initpos -> [pos quat] in
    (
      SO3ObjectSetPosition fusionStr.FUSION_father pos;
      SO3ObjectSetOrientation fusionStr.FUSION_father quat;
    );
    
    set fusionStr.FUSION_father = nfather; 
    set fusionStr.FUSION_initpos = [cpos cquat];
  );
0;;
  
  
/* Device connected */
fun cbConnected(fusion, fusionStr)=
  _fooS (loc "OS3DFUSION_0001");
  _fooS strcat (strcat (loc "OS3DFUSION_0010") " ") (itoa (fusionStr.FUSION_iLastStatus));
  
  if (fusionStr.FUSION_iLastStatus != 1) then
    SendPluginEvent fusionStr.FUSION_inst "Connected" nil nil
  else nil;
  
  // Update connection status
  set fusionStr.FUSION_iLastStatus = 1;
  setPluginInstanceCbScenePreRender fusionStr.FUSION_inst mkfun4 @cbNavPreRender fusionStr;
0;;

/* Device disconnected */
fun cbDisconnected(fusion, fusionStr)=
  _fooS (loc "OS3DFUSION_0002");
  _fooS strcat  (strcat (loc "OS3DFUSION_0011") " ") (itoa (fusionStr.FUSION_iLastStatus));
  
  setPluginInstanceCbScenePreRender fusionStr.FUSION_inst nil;
  set fusionStr.FUSION_tLastOrientation = nil;
  
  if (fusionStr.FUSION_iLastStatus != 2) then
    SendPluginEvent fusionStr.FUSION_inst "Disconnected" nil nil
  else nil;
  
  // Update connection status
  set fusionStr.FUSION_iLastStatus = 2;
0;;


/*--------------- Create new SpacePoint Fusion instance ----------------*/
fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in
  let atoi (getPluginInstanceParam inst "local") -> local in
  let if (local == nil) then 1 else local -> local in
  
  // Retrieve selected data type
  let atoi (getPluginInstanceParam inst "dataType") -> dataType in
  
  // Open Fusion device
  let _OpenFusionDevice _channel dataType -> fusion in
  //_fooS (strcat (strcat (loc "OS3DFUSION_0012") " ") (itoa dataType));
  if (fusion == nil) then nil else
  (
    // Set the SpacePoint Fusion global structure
    let V3DgetObjectByName c3dXsession objname -> father in
    let V3DgetObjectTypeByName objname -> iobjmode in
    let SO3ObjectGetPosition father -> cpos in
    let SO3ObjectGetOrientation father -> cquat in    
    let mkFusion [inst fusion dataType 0 0 father iobjmode [cpos cquat] nil nil local] -> fusionStr in
    (
      if !iobjmode then nil else
        setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera fusionStr;
        
      // Define the callback to call when the plugin instance is deleted
      setPluginInstanceCbDel inst mkfun2 @deleteOb fusionStr;
      
      // TODO: Associate callbacks to the current Fusion instance
      _CBFusionOrientationData fusion @cbNewOrientationData fusionStr;
      _CBFusionRawData fusion @cbNewRawData fusionStr;
      _CBFusionButton fusion @cbButton fusionStr;
      _CBFusionConnected fusion @cbConnected fusionStr;
      _CBFusionDisconnected fusion @cbDisconnected fusionStr;
    );
  );
0;;


/*------------------- Initialize plugIT --------------------*/
fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
0;;