/*
-----------------------------------------------------------------------------
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 PlugGPIOmotorDriver = [
  GPIOIN_iPin1               : ObjGpioPin,
  GPIOIN_iPin2               : ObjGpioPin,
  GPIOIN_iPin3               : ObjGpioPin,
  GPIOIN_iPin4               : ObjGpioPin,
  GPIOIN_iSpeed1             : I,
  GPIOIN_iSpeed2             : I
  
  ]mkPlugGPIOinput;;

/*! \brief Callback on instance destruction
  *
  *  <b>Prototype:</b> fun [PInstance PlugGPIOinput] I
  *
  *  \param PInstance : destroyed plugIT instance
  *  \param PlugGPIOinput : gpio structure
  *   
  *  \return I : 0
  **/
fun deleteOb(inst, obstr)=
  _dsGPIOpin obstr.GPIOIN_iPin1;
  _dsGPIOpin obstr.GPIOIN_iPin2;
  _dsGPIOpin obstr.GPIOIN_iPin3;
  _dsGPIOpin obstr.GPIOIN_iPin4;
  0;;


fun cbSetMotor1Speed(inst, from, action, param, reply, obstr)=
  let atoi param -> speed in
  (
    //addLogMessage strcat "m1 : " itoa speed;
    set obstr.GPIOIN_iSpeed1 = speed;
    if (speed == 0) then
    (
      _GPIOpwmWrite obstr.GPIOIN_iPin1 0;
      _GPIOpwmWrite obstr.GPIOIN_iPin2 0;
    )
    else if (speed > 0) then
    (
      _GPIOpwmWrite obstr.GPIOIN_iPin1 abs speed;
      _GPIOpwmWrite obstr.GPIOIN_iPin2 0;
    )
    else
    (
      _GPIOpwmWrite obstr.GPIOIN_iPin1 0;
      _GPIOpwmWrite obstr.GPIOIN_iPin2 abs speed;
    );
  );
  0;;


fun cbSetMotor2Speed(inst, from, action, param, reply, obstr)=
  let atoi param -> speed in
  (
    //addLogMessage strcat "m2 : " itoa speed;
    set obstr.GPIOIN_iSpeed2 = speed;
    if (speed == 0) then
    (
      _GPIOpwmWrite obstr.GPIOIN_iPin3 0;
      _GPIOpwmWrite obstr.GPIOIN_iPin4 0;
    )
    else if (speed > 0) then
    (
      _GPIOpwmWrite obstr.GPIOIN_iPin3 abs speed;
      _GPIOpwmWrite obstr.GPIOIN_iPin4 0;
    )
    else
    (
      _GPIOpwmWrite obstr.GPIOIN_iPin3 0;
      _GPIOpwmWrite obstr.GPIOIN_iPin4 abs speed;
    );
  );
  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 atoi (getPluginInstanceParam inst "pin1") -> pin1 in
  let if pin1 == nil then 6 else pin1 -> pin1 in
  let atoi (getPluginInstanceParam inst "pin2") -> pin2 in
  let if pin2 == nil then 13 else pin2 -> pin2 in
  let atoi (getPluginInstanceParam inst "pin3") -> pin3 in
  let if pin3 == nil then 19 else pin3 -> pin3 in
  let atoi (getPluginInstanceParam inst "pin4") -> pin4 in
  let if pin4 == nil then 26 else pin4 -> pin4 in
  let atoi (getPluginInstanceParam inst "speed1") -> speed1 in
  let if speed1 == nil then 0 else speed1 -> speed1 in
  let atoi (getPluginInstanceParam inst "speed2") -> speed2 in
  let if speed2 == nil then 0 else speed2 -> speed2 in
  let mkPlugGPIOinput [nil nil nil nil speed1 speed2] -> obstr in
  (
    set obstr.GPIOIN_iPin1 = _crGPIOpin _channel pin1 GPIO_PWM_OUTPUT;
    set obstr.GPIOIN_iPin2 = _crGPIOpin _channel pin2 GPIO_PWM_OUTPUT;
    set obstr.GPIOIN_iPin3 = _crGPIOpin _channel pin3 GPIO_PWM_OUTPUT;
    set obstr.GPIOIN_iPin4 = _crGPIOpin _channel pin4 GPIO_PWM_OUTPUT;
    
    cbSetMotor1Speed inst nil nil nil itoa speed1 obstr;
    cbSetMotor2Speed inst nil nil nil itoa speed2 obstr;
        
    PluginRegisterAction inst "Set motor 1 speed" mkfun6 @cbSetMotor1Speed obstr;
    PluginRegisterAction inst "Set motor 2 speed" mkfun6 @cbSetMotor2Speed 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;;