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

Copyright (c) 2016 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

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

/*
  Version : 1.0
  First version : 05/29/2016
  Author : Bourineau Bastien
*/

struct PlugGPIO9685 = [
  GPIO9685_inst                : PInstance,
  GPIO9685_iDeviceId           : I,
  GPIO9685_iFrequency          : I,
  GPIO9685_bServos             : I,
  GPIO9685_device              : ObjGpioPCA9685
  
  ]mkPlugGPIO9685;;


var MAX_PWM = 4096;;

fun mapData(input, vmin, vmax, tmin, tmax)=
 (input -. vmin) *. (tmax -. tmin) /. (vmax -. vmin) +. tmin;;


fun getAngle(value, obstr)=
  let if (value <. 0.0) then
        0.0
      else if (value >. 180.0) then
        180.0
      else
        value
  -> value in
  let 1000.0 /. (itof obstr.GPIO9685_iFrequency) -> cycleMs in
  let (itof MAX_PWM) /. cycleMs -> ms in
  let mapData (value *. 2.0) 0.0 180.0 ms (ms *. 2.0) -> cval in
    ftoi cval;;


/*! \brief Callback on instance destruction
  *
  *  <b>Prototype:</b> fun [PInstance PlugGPIO9685] I
  *
  *  \param PInstance : destroyed plugIT instance
  *  \param PlugGPIO9685 : gpio structure
  *   
  *  \return I : 0
  **/
fun deleteOb(inst, obstr)=
  _dsGPIOpca9685 obstr.GPIO9685_device;
  set obstr.GPIO9685_device = nil;
  0;;


/*! \brief Callback on "Set angle" dms action
  *
  *  change servo angle
  *
  *  <b>Prototype:</b> fun [PInstance DMI S S I PlugGPIO9685] I
  *
  *  \param PInstance : plugIT instance
  *  \param DMI : DMS module who call the action (not used)
  *  \param S : name of the launched action
  *  \param S : data posted in DMS action link
  *  \param I : reply flag (not used)
  *  \param PlugGPIO9685 : gpio structure
  * 
  *  \return I : 0
  **/
fun cbSetValue(inst, from, action, param, rep, obstr, ipos) =
  let if obstr.GPIO9685_bServos then getAngle (atof param) obstr else atoi param -> val in
  //let addLogMessage itoa val -> _ in
    _GPIOpca9685Write obstr.GPIO9685_device ipos val;
  0;;


/*! \brief Callback on "Enable" dms action
  *
  *  Enable servo
  *
  *  <b>Prototype:</b> fun [PInstance DMI S S I PlugGPIO9685] I
  *
  *  \param PInstance : plugIT instance
  *  \param DMI : DMS module who call the action (not used)
  *  \param S : name of the launched action
  *  \param S : data posted in DMS action link
  *  \param I : reply flag (not used)
  *  \param PlugGPIO9685 : gpio structure
  * 
  *  \return I : 0
  **/
fun cbEnable(inst, from, action, param, reply, obstr)=
  if (obstr.GPIO9685_device != nil) then nil else
  (
    set obstr.GPIO9685_device = _crGPIOpca9685 _channel obstr.GPIO9685_iDeviceId 300 obstr.GPIO9685_iFrequency;
    _GPIOpca9685Reset obstr.GPIO9685_device;
  );
  0;;


fun cbSetFrequency(inst, from, action, param, reply, obstr)=
  set obstr.GPIO9685_iFrequency = atoi param;
  _GPIOpca9685SetFrequency obstr.GPIO9685_device obstr.GPIO9685_iFrequency;
  0;;


/*! \brief Callback on "Disable" dms action
  *
  *  Disable servo
  *
  *  <b>Prototype:</b> fun [PInstance DMI S S I PlugGPIO9685] I
  *
  *  \param PInstance : plugIT instance
  *  \param DMI : DMS module who call the action (not used)
  *  \param S : name of the launched action
  *  \param S : data posted in DMS action link
  *  \param I : reply flag (not used)
  *  \param PlugGPIO9685 : gpio structure
  * 
  *  \return I : 0
  **/
fun cbDisable(inst, from, action, param, reply, obstr)=
  _dsGPIOpca9685 obstr.GPIO9685_device;
  set obstr.GPIO9685_device = nil;
  0;;


/*! \brief Callback on new plugIT instance
  *
  *  Read the parameters from editor values and init GPIO pin
  *
  *  <b>Prototype:</b> fun [PInstance] I
  *
  *  \param PInstance : plugIT instance
  *   
  *  \return I : 0
  **/
fun newOb(inst)=
  let getPluginInstanceParam inst "i2cdevice" -> i2cdevice in
  let if i2cdevice == nil then 0x40 else htoi i2cdevice -> i2cdevice in
  let atoi (getPluginInstanceParam inst "frequency") -> frequency in
  let if frequency == nil then 50 else frequency -> frequency in
  let atoi (getPluginInstanceParam inst "forServos") -> forServos in
  let if forServos == nil then 1 else forServos -> forServos in
  let atoi (getPluginInstanceParam inst "init") -> init in
  let if init == nil then 1 else init -> init in

  let mkPlugGPIO9685 [inst i2cdevice frequency forServos nil] -> obstr in
  (
    if (!init) then nil else
     cbEnable inst nil nil nil nil obstr;
    
    let 0 -> i in
  	while (i < 16) do
  	(
    	PluginRegisterAction inst (strcat "Set value on " (itoa i+1)) mkfun6 mkfun7 @cbSetValue i obstr;
    	set i = i + 1;
  	);
    
    PluginRegisterAction inst "Enable" mkfun6 @cbEnable obstr;
    PluginRegisterAction inst "Disable" mkfun6 @cbDisable obstr;
    PluginRegisterAction inst "Set frequency" mkfun6 @cbSetFrequency obstr;
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
  );
  0;;


/*! \brief Global plugIT function to initialize the plugIT callbacks
  *
  *  <b>Prototype:</b> fun [S] I
  *
  *  \param S : plugIT file path
  *  
  *  \return I : 0
  **/
fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;