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

Copyright (c) 2015 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 PlugOSensor = [
  POSENS_inst                 : PInstance,
  POSENS_father               : SO3_OBJECT,
  POSENS_iFatherMode          : I,
  POSENS_initpos              : [[F F F] [F F F F]],
  POSENS_qOffset              : [F F F F],
  POSENS_tLastY               : F,  
  POSENS_bUseMag              : I,
  POSENS_bLocal               : I,
  POSENS_iTick                : I,
  POSENS_bControl             : I,
  POSENS_bOrientation         : I,
  POSENS_bPitch               : I,
  POSENS_bYaw                 : I,
  POSENS_bRoll                : I
          
] mkPlugOSensor;;


/*------------------ Delete SpacePoint Fusion object -------------------*/
fun deleteOb(inst, obstr)=
  _SetSensorEnable SENSOR_TYPE_ORIENTATION 0;
  if (!obstr.POSENS_bUseMag) then nil else 
    _SetSensorEnable SENSOR_TYPE_MAGNETIC_FIELD 0;
  
  // Stop event firing
  setPluginInstanceCbScenePreRender inst nil;
  
  setPluginInstanceCbCameraChange inst nil;
  // Reset attached object position
  let obstr.POSENS_initpos -> [pos quat] in
  (
    SO3ObjectSetPosition obstr.POSENS_father pos;
    SO3ObjectSetOrientation obstr.POSENS_father quat;
  );
  0;;


fun cbNavPreRender(inst, sessionstr, etime, obstr)=
  let if etime <= 1000 then 1000 else etime -> etime in
  let _GetDeviceQuaternionOrientation (etime / 1000) -> quat in
  let SO3MathsQuatToEulerDegreePYR quat -> [pitch yaw roll] in
  if (quat == nil) then nil else
  (
    if (obstr.POSENS_tLastY == nil) then nil else
    let obstr.POSENS_tLastY -> 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
      if (!obstr.POSENS_bControl) then nil else
      (
        let ftoa pitch -> sax in
        let ftoa ryaw -> say in
        let ftoa roll -> saz in
          SendPluginEvent obstr.POSENS_inst "Control" (strcatn "0 0 0\n"::sax::" "::say::" "::saz::"\n2"::nil) obstr.POSENS_inst.INST_sName;
      );
    );
    
    let SO3MathsQuatSubstract obstr.POSENS_qOffset quat -> oquat in
    let SO3MathsQuatToEulerDegreePYR oquat -> [opitch oyaw oroll] in
    let if ((obstr.POSENS_iFatherMode == 1) || ((SO3ObjectGetType obstr.POSENS_father) == SO3_TYPE_CAMERA)) then SO3MathsQuatAdd SO3MathsQuatAdd SO3MathsQuatAdd oquat [0.0 1.0 0.0 0.0] [-.1.0 0.0 0.0 0.0] [0.0 0.0 1.0 0.0] else oquat -> rquat in // INVERT Z AXIS IF 
    (
      // LOCAL ORIENTATION HANDLING
      if (obstr.POSENS_bLocal) then
        SO3ObjectSetOrientation obstr.POSENS_father rquat
      // GLOBAL POSITION HANDLING
      else
        SO3ObjectSetGlobalOrientation obstr.POSENS_father rquat;
    
      if (!obstr.POSENS_bOrientation) then nil else
        SendPluginEvent obstr.POSENS_inst "Orientation" (strcatn (ftoa opitch)::" "::(ftoa oyaw)::" "::(ftoa oroll)::nil) obstr.POSENS_inst.INST_sName;
      
      if (!obstr.POSENS_bPitch) then nil else
        SendPluginEvent obstr.POSENS_inst "Pitch" (ftoa opitch) obstr.POSENS_inst.INST_sName;
      if (!obstr.POSENS_bYaw) then nil else
        SendPluginEvent obstr.POSENS_inst "Yaw" (ftoa oyaw) obstr.POSENS_inst.INST_sName;
      if (!obstr.POSENS_bRoll) then nil else
        SendPluginEvent obstr.POSENS_inst "Roll" (ftoa oroll) obstr.POSENS_inst.INST_sName;
    );
    
    //disable mag sensor it's used only for init
    //if (!obstr.POSENS_bUseMag || obstr.POSENS_tLastY == nil) then nil else
    //  _SetSensorEnable SENSOR_TYPE_MAGNETIC_FIELD 0;
    
    set obstr.POSENS_tLastY = yaw;
  );
  0;;


/*----------------------- Callbacks ------------------------*/


/* Update camera control */
fun cbChangeCamera(inst, viewstr, sessionstr, camera, obstr)=
  let V3DgetCameraByType sessionstr camera obstr.POSENS_iFatherMode -> nfather in
  let SO3ObjectGetPosition nfather -> cpos in
  let SO3ObjectGetOrientation nfather -> cquat in
  (
    let obstr.POSENS_initpos -> [pos quat] in
    (
      SO3ObjectSetPosition obstr.POSENS_father pos;
      SO3ObjectSetOrientation obstr.POSENS_father quat;
    );
    
    set obstr.POSENS_father = nfather; 
    set obstr.POSENS_initpos = [cpos cquat];
  );
  0;;

  
fun cbSetZero(inst, from, action, param, rep, obstr)=
  set obstr.POSENS_qOffset = _GetDeviceQuaternionOrientation 1;
  0;;
  

fun cbSetOffset(inst, from, action, param, reply, obstr)=
  if (!strcmp "" (strtrim param)) || param == nil then nil else
  (
    let hd (strextr param) -> lp in
    let atof (nth_list lp 0) -> ax in
    let atof (nth_list lp 1) -> ay in
    let atof (nth_list lp 2) -> az in
    let SO3MathsEulerPYRToQuat [ax ay az] -> qoffset in
    if (qoffset == nil) then nil else
      set obstr.POSENS_qOffset = qoffset;
  );
  0;;


fun cbEnable(inst, from, action, param, rep, obstr)=
  _SetSensorEnable SENSOR_TYPE_ORIENTATION 1;
  if (!obstr.POSENS_bUseMag) then nil else
    _SetSensorEnable SENSOR_TYPE_MAGNETIC_FIELD 1;
    
  setPluginInstanceCbScenePreRender inst mkfun4 @cbNavPreRender obstr;
  0;;
  

fun cbDisable(inst, from, action, param, rep, obstr)=
  _SetSensorEnable SENSOR_TYPE_ORIENTATION 0;
  if (!obstr.POSENS_bUseMag) then nil else 
    _SetSensorEnable SENSOR_TYPE_MAGNETIC_FIELD 0;
    
  setPluginInstanceCbScenePreRender inst nil;
  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
  let atoi (getPluginInstanceParam inst "usemag") -> usemag in
  let if (usemag == nil) then 0 else usemag -> usemag in
  
  let atof (getPluginInstanceParam inst "angx") -> ax in
  let atof (getPluginInstanceParam inst "angy") -> ay in
  let atof (getPluginInstanceParam inst "angz") -> az in

  let if ax == nil then 0.0 else ax -> ax in
  let if ay == nil then 0.0 else ay -> ay in
  let if az == nil then 0.0 else az -> az in
  let SO3MathsEulerPYRToQuat [ax ay az] -> qoffset in
  
  // 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 (IsInEditor inst) || IsEventLinked inst "Control" -> bcontrol in
  let (IsInEditor inst) || IsEventLinked inst "Orientation" -> borientation in
  let (IsInEditor inst) || IsEventLinked inst "Pitch" -> bpitch in
  let (IsInEditor inst) || IsEventLinked inst "Yaw" -> byaw in
  let (IsInEditor inst) || IsEventLinked inst "Roll" -> broll in
  let mkPlugOSensor [inst father iobjmode [cpos cquat] qoffset nil usemag local 0 bcontrol borientation bpitch byaw broll] -> obstr in
  (
    if !(_IsSensorAvailable SENSOR_TYPE_ORIENTATION) then
    (
      SendPluginEvent inst "Sensor not found" nil nil;
      0;
    )
    else
    (
      _SetSensorEnable SENSOR_TYPE_ORIENTATION 1;
      if (!obstr.POSENS_bUseMag) then nil else 
        _SetSensorEnable SENSOR_TYPE_MAGNETIC_FIELD 1;
      
      setPluginInstanceCbScenePreRender inst mkfun4 @cbNavPreRender obstr;
      
      PluginRegisterAction inst "Enable" mkfun6 @cbEnable obstr;
      PluginRegisterAction inst "Disable" mkfun6 @cbDisable obstr;
      PluginRegisterAction inst "Set offset to orientation" mkfun6 @cbSetZero obstr;
      PluginRegisterAction inst "Set offset" mkfun6 @cbSetOffset obstr;
      
      if !iobjmode then nil else
        setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera obstr;
      
      SendPluginEvent inst "Sensor found" nil nil;
    );
    
    // Define the callback to call when the plugin instance is deleted
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
  );
  0;;


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