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

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

struct PlugVRPNStr = [
  PVRPN_inst                : PInstance,
  PVRPN_device              : ObjVRPN,
  PVRPN_sHost               : S,
  PVRPN_sName               : S,
  PVRPN_bAnalog             : I,
  PVRPN_bButtons            : I,
  PVRPN_bTracker            : I,
  PVRPN_bAnalSep            : I,
  PVRPN_nbAnalValues        : I,
  PVRPN_lSensors            : [[I [SO3_OBJECT I I I I [[F F F] [F F F]][[F F F] [F F F F]]]] r1]
  
  ]mkPlugVRPNStr;;

proto stopDevice = fun [PlugVRPNStr] I;;

fun lFtolS(l)=
  let nil -> ln in
  (
    let sizelist l -> size in
    let 0 -> i in
    while (i < size) do
    (
      set ln = (ftoa nth_list l i)::ln;
      set i = i + 1;
    );
    revertlist ln;
  );;


fun deleteOb(inst, obstr)=
  setPluginInstanceCbCameraChange inst nil;
  
  stopDevice obstr;
  
  //reset sensor objets position
  let sizelist obstr.PVRPN_lSensors -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVRPN_lSensors i -> [_ [obj _ _ _ _ _ [pos quat]]] in
    (
      SO3ObjectSetPosition obj pos;
      SO3ObjectSetOrientation obj quat;
    );
    set i = i + 1;
  );
  0;;


fun cbAnalogData(vrpnobj, obstr, ldata)=
  if (obstr.PVRPN_bAnalSep) then
  (
    let sizelist ldata -> size in
    let 0 -> i in
    while (i < size) do
    (
      SendPluginEvent obstr.PVRPN_inst strcat "Analog" (itoa i) ftoa (nth_list ldata i) nil;
      
      set i = i + 1;
    );
  )
  else
  (
    SendPluginEvent obstr.PVRPN_inst "Analog data" (strcatnSep lFtolS ldata " ") nil;
  );
  
  if (obstr.PVRPN_nbAnalValues < 6) then nil else
  (
    let nth_list ldata 0 -> x in
    let nth_list ldata 1 -> y in
    let nth_list ldata 2 -> z in
    
    let nth_list ldata 3 -> ax in
    let nth_list ldata 4 -> ay in
    let nth_list ldata 5 -> az in
    (
      SendPluginEvent obstr.PVRPN_inst "Control" strcatn (ftoa x)::" "::(ftoa -.z)::" "::(ftoa y)::"\n"::(ftoa -.ax)::" "::(ftoa -.ay)::" "::(ftoa -.az)::nil obstr.PVRPN_inst.INST_sName;
    );
  );
  0;;


/*
fun cbButtonData(vrpnobj, obstr, lbtn)=
  let sizelist lbtn -> size in
  let 0 -> i in
  while (i < size) do
  (
    SendPluginEvent obstr.PVRPN_inst strcatn "Button"::(itoa i)::" state"::nil itoa (nth_list lbtn i) nil;
    
    set i = i + 1;
  );
  0;;
*/

fun cbButtonState(vrpnobj, obstr, btn, state)=
  let if state then " pressed" else " released" -> nstate in
    SendPluginEvent obstr.PVRPN_inst strcatn "Button"::(itoa btn)::nstate::nil nil nil;
  0;;


fun cbTrackerData(vrpnobj, obstr, id, pos, quat)=
  let switch obstr.PVRPN_lSensors id -> tdata in
  let tdata -> [obj coordspace accel velocity _ lastpos _] in
  (
    let pos -> dpos in
    let quat -> dquat in
    (
      if (coordspace < 2) then nil else
      (
        let pos -> [x y z] in
        let (V3DgetDefaultViewport (V3DgetSessionView c3dXsession)) -> viewportstr in
        let SO3ViewportGetCamera viewportstr.V3D_viewport -> camera in
        let SO3ObjectGetGlobalPosition camera -> cpos in
        let SO3ObjectGetGlobalOrientation camera -> cquat in
        let SO3MathsQuatGetDirection cquat [x y (-.z)] -> npos in
        (
          set dpos = (addVectorF npos cpos);
          set dquat = SO3MathsQuatAdd cquat quat;
        );
      );
      
      if (obj == nil) then nil else
      (
        if (coordspace == 1) then
        (
          SO3ObjectSetPosition obj dpos;
          SO3ObjectSetOrientation obj dquat;
        )
        else
        (
          SO3ObjectSetGlobalPosition obj dpos;
          SO3ObjectSetGlobalOrientation obj dquat;
        );
      );
      
      let SO3MathsQuatToEulerDegreeYZX quat -> [roll yaw pitch] in
      (
        if (lastpos == nil) then nil else
        let lastpos -> [rpos [rpitch ryaw rroll]] in
        (
          let dpos -> [px py pz] in
          let SO3MathsQuatToEulerXYZ dquat -> [ax ay az] in
          let strcatn (strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " ")::"\n"::(strcatnSep (ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree az))::nil " ")::nil -> spos in
            SendPluginEvent obstr.PVRPN_inst strcatn "Sensor"::(itoa id)::" position"::nil spos nil;
          
          let yaw -. ryaw -> 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 pitch -. rpitch -> rpitch in
          let if rpitch >. 180.0 then rpitch -. 360.0 else if rpitch <. (-.180.0) then rpitch +. 360.0 else rpitch -> rpitch in
          let roll -. rroll -> rroll in
          let if rroll >. 180.0 then rroll -. 360.0 else if rroll <. (-.180.0) then rroll +. 360.0 else rroll -> rroll in
          (
            let subVectorF dpos rpos -> [px py pz] in
            // last "\n 1" define a coords control, not simple 0 to 1 data
            let strcatn (ftoa px)::" "::(ftoa py)::" "::(ftoa pz)::"\n"::(ftoa rpitch)::" "::(ftoa ryaw)::" "::(ftoa rroll)::"\n"::"4"::nil -> spos in
              SendPluginEvent obstr.PVRPN_inst strcatn "Sensor"::(itoa id)::" control"::nil spos (strcat obstr.PVRPN_inst.INST_sName (itoa id));
          );
        );
        
        mutate tdata <- [_ _ _ _ _ [dpos [pitch yaw roll]] _];
      );
    );
  );
  0;;


fun cbTrackerVel(vrpnobj, obstr, id, pos, quat)=
  let switch obstr.PVRPN_lSensors id -> [obj coordspace accel velocity _ _ _] in
  if (!velocity) then nil else
  (
    let pos -> [px py pz] in
    let SO3MathsQuatToEulerXYZ quat -> [ax ay az] in
    let strcatn (strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " ")::"\n"::(strcatnSep (ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree az))::nil " ")::nil -> spos in
      SendPluginEvent obstr.PVRPN_inst strcatn "Sensor"::(itoa id)::" velocity"::nil spos nil;
  );
  0;;


fun cbTrackerAcc(vrpnobj, obstr, id, pos, quat)=
  let switch obstr.PVRPN_lSensors id -> [obj coordspace accel velocity _ _ _] in
  if (!accel) then nil else
  (
    let pos -> [px py pz] in
    let SO3MathsQuatToEulerXYZ quat -> [ax ay az] in
    let strcatn (strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " ")::"\n"::(strcatnSep (ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree az))::nil " ")::nil -> spos in
      SendPluginEvent obstr.PVRPN_inst strcatn "Sensor"::(itoa id)::" acceleration"::nil spos nil;
  );
  0;;


fun loadSensors(obstr)=  
  let 0 -> i in
  while (i < 256) do
  (
    let getPluginInstanceParam obstr.PVRPN_inst (strcat "sensor_" (itoa i)) -> id in
    if (id == nil) then nil else
    (
      let getPluginInstanceParam obstr.PVRPN_inst (strcat "sensorObj_" (itoa i)) -> obj in
      let atoi getPluginInstanceParam obstr.PVRPN_inst (strcat "sensorSpace_" (itoa i)) -> coordspace in
      let atoi getPluginInstanceParam obstr.PVRPN_inst (strcat "sensorAccel_" (itoa i)) -> accel in
      let atoi getPluginInstanceParam obstr.PVRPN_inst (strcat "sensorVelocity_" (itoa i)) -> velocity in
      
      let V3DgetObjectByName c3dXsession obj -> trackobj in
      let [(SO3ObjectGetPosition trackobj) (SO3ObjectGetOrientation trackobj)] -> restoreobj in
      let V3DgetObjectTypeByName obj -> objmode in
      (
        set obstr.PVRPN_lSensors = [atoi id [trackobj coordspace accel velocity objmode nil restoreobj]]::obstr.PVRPN_lSensors;
      );
      0;
    );
      
    set i = i + 1;
  );
  set obstr.PVRPN_lSensors = revertlist obstr.PVRPN_lSensors;
  0;;


fun cbChangeCamera(inst, viewstr, sessionstr, camera, obstr)=
  let sizelist obstr.PVRPN_lSensors -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVRPN_lSensors i -> sensor in
    let sensor -> [_ [obj coordspace accel velocity mode rpos [pos quat]]] in
    if (!mode) then nil else
    let V3DgetCameraByType sessionstr camera mode -> nson in
    let [(SO3ObjectGetPosition nson) (SO3ObjectGetOrientation nson)] -> restoreobj in
    (
      SO3ObjectSetPosition obj pos;
      SO3ObjectSetOrientation obj quat;
      mutate sensor <- [_ [nson coordspace accel velocity mode rpos restoreobj]];
    );
    set i = i + 1;
  );
  0;;


fun stopDevice(obstr)=
  _DSvrpnDevice obstr.PVRPN_device;
  set obstr.PVRPN_device = nil;
  0;;


fun startDevice(obstr)=
  let _CRvrpnDevice _channel obstr.PVRPN_sHost obstr.PVRPN_sName -> vrpnobj in
    set obstr.PVRPN_device = vrpnobj;
  
  if (!obstr.PVRPN_bAnalog) then nil else
    _CBvrpnAnalogData obstr.PVRPN_device @cbAnalogData obstr;
  
  if (!obstr.PVRPN_bButtons) then nil else
  (
    //_CBvrpnButtonData obstr.PVRPN_device @cbButtonData obstr;
    _CBvrpnButtonState obstr.PVRPN_device @cbButtonState obstr;
  );
  
  if (!obstr.PVRPN_bTracker) then nil else
  (
    _CBvrpnTrackerData obstr.PVRPN_device @cbTrackerData obstr;
    _CBvrpnTrackerVelocityData obstr.PVRPN_device @cbTrackerVel obstr;
    _CBvrpnTrackerAccelData obstr.PVRPN_device @cbTrackerAcc obstr;
  );
  0;;


fun cbSetHost(inst, from, action, param, reply, obstr)=  
  stopDevice obstr;
  set obstr.PVRPN_sHost = param;
  startDevice obstr;
  0;;


fun cbSetName(inst, from, action, param, reply, obstr)=
  stopDevice obstr;
  set obstr.PVRPN_sName = param;
  startDevice obstr;
  0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "host") -> host in
  let if host == nil then "localhost" else host -> host in
  
  let (getPluginInstanceParam inst "name") -> name in
  let if name == nil then "" else name -> name in
  
  let atoi (getPluginInstanceParam inst "bAnalog") -> bAnalog in
  let if bAnalog == nil then 0 else bAnalog -> bAnalog in
  let atoi (getPluginInstanceParam inst "nbAnalogValues") -> nbAnalogValues in
  let if nbAnalogValues == nil then 1 else nbAnalogValues -> nbAnalogValues in
  let atoi (getPluginInstanceParam inst "bAnalogSeparateValues") -> bAnalogSeparateValues in
  let if bAnalogSeparateValues == nil then 0 else bAnalogSeparateValues -> bAnalogSeparateValues in
  
  let atoi (getPluginInstanceParam inst "bButton") -> bButton in
  let if bButton == nil then 0 else bButton -> bButton in
  let atoi (getPluginInstanceParam inst "nbButtons") -> nbButtons in
  let if nbButtons == nil then 1 else nbButtons -> nbButtons in
  
  let atoi (getPluginInstanceParam inst "bTracker") -> bTracker in
  let if bTracker == nil then 0 else bTracker -> bTracker in
  let mkPlugVRPNStr [inst nil host name bAnalog bButton bTracker bAnalogSeparateValues nbAnalogValues nil] -> obstr in
  (
    if (!bTracker) then nil else
    (
      loadSensors obstr;      
      setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera obstr;
    );
    
    startDevice obstr;    
    
    PluginRegisterAction inst "Set host" mkfun6 @cbSetHost obstr;
    PluginRegisterAction inst "Set name" mkfun6 @cbSetName obstr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
  );
  0;;


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