/* A simple giotto program controlling a 2DOF hovercraft.

Copyright (c) 2004 The Regents of the University of California.
All rights reserved.

Permission is hereby granted, without written agreement and without
license or royalty fees, to use, copy, modify, and distribute this
software and its documentation for any purpose, provided that the above
copyright notice and the following two paragraphs appear in all copies
of this software.

IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF
THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.

THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
CALIFORNIA HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,
ENHANCEMENTS, OR MODIFICATIONS.
*/

//////////////////////////////////////////////////////////////////////////
//// Hovercraft Example
/**
A basic Giotto program for controlling a 2 degree of freedom hovercraft
simulator. The program computes the position of the hovercraft, and the
moves the hovercraft towards the target destination and orientation.

This example is part of the Giotto Tutorial documented in 
../doc/GiottoTutorial.pdf.  It illustrates the final stage of 
development of the tutorial example.

To run the example:
0. Start up Giotto with java giotto.gdk.Start
1. Be sure that the "make Java" checkbox is checked
2. Press the "Compile" button
3. Under the "E code" menu, select "Run E code"
4. When the "E code interpreter" window comes up, select "Simulate"
5. When the "Hovercraft" window comes up, target the hovercraft
by left mousing in the white area at the bottom of the window.

@author M.A.A. Sanvido
@version hovercraft05.giotto,v 1.5 2004/09/29 00:03:37 cxh Exp
@since Giotto 1.0.1
*/

/////////////////////////////////////////////////////////////////////////
////                         sensors                                 ////

sensor
    real_port  positionX   uses GetPosX;
    real_port  positionY   uses GetPosY;
    real_port  angle       uses GetPosA;
    real_port  targetX     uses GetTargX;
    real_port  targetY     uses GetTargY;
    real_port  targetAngle uses GetTargA;

/////////////////////////////////////////////////////////////////////////
////                         actuators                               ////

actuator
    real_port     rightJet uses PutRightJet;
    real_port     leftJet  uses PutLeftJet;

/////////////////////////////////////////////////////////////////////////
////                        output ports                             ////

output
    real_port     turn            := real_zero;
    real_port     thrust          := real_zero;
    real_port     errorX          := real_zero;
    real_port     errorY          := real_zero;
    real_port     errorAngle      := real_zero;
    real_port     targetDirection := real_zero;
    bool_port     openWindow      := HovercraftWindow;

/////////////////////////////////////////////////////////////////////////
////                         tasks                                   ////

/** Compute the 'turn' and 'thrust' acceleration values in order
 *  to reach the target position. The inputs
 *  represent the distance from the target (errX, errY), the direction (dir)
 *  of the target, and the error in the heading of the hovercraft (errAngle).
 *  This task generates only thrust, i.e. turn = 0;
 */
task forwardTask(real_port errX, real_port errY, real_port errAngle, real_port dir)
     output (turn, thrust) state () {
    schedule Forward(errX, errY, errAngle, dir, turn, thrust)
}

/** Reset the 'turn' and 'thrust' to 0.
 */
task idleTask() output (turn, thrust) state () {
    schedule Idle(turn, thrust)
}

/** Computes the control error, i.e. the distance and angles
*   from the actual hovercraft position and orientation to the
*   target position and orientation.
*/
task errorTask(real_port posX, real_port posY, real_port posA,
               real_port tgtX, real_port tgtY, real_port tgtA)
     output (errorX, errorY, errorAngle, targetDirection) state () {
    schedule Error(posX, posY, posA, tgtX, tgtY, tgtA,
                        errorX, errorY, errorAngle, targetDirection)
}

task turnTowardsTargetTask(real_port eX, real_port eY, real_port eA, real_port eA2) output (turn, thrust) state () {
   schedule TurnTowardsTarget(eX, eY, eA, eA2, turn, thrust)
}

task turnToTargetTask(real_port eX, real_port eY, real_port eA, real_port eA2) output (turn, thrust) state () {
   schedule TurnToTarget(eX, eY, eA, eA2, turn, thrust)
}

/////////////////////////////////////////////////////////////////////////
////                    actuator drivers                             ////


driver leftMotor(turn, thrust) output (real_port left) {
    if constant_true() then ComputeLeftJetPower(turn, thrust, left)
}

driver rightMotor(turn, thrust) output (real_port right) {
    if constant_true() then ComputeRightJetPower(turn, thrust, right)
}

/////////////////////////////////////////////////////////////////////////
////                    input drivers                                ////

driver getPos (positionX, positionY, angle, targetX, targetY, targetAngle)
       output (real_port posX, real_port posY, real_port posA,
               real_port tgtX, real_port tgtY, real_port tgtA) {
    if constant_true() then
       copy_real_port6(positionX, positionY, angle, targetX, targetY, targetAngle,
                     posX, posY, posA, tgtX, tgtY, tgtA)
}

driver getErr (errorX, errorY, errorAngle, targetDirection)
       output (real_port errX, real_port errY, real_port dir, real_port errA) {
    if constant_true() then
       copy_real_port4(errorX, errorY, errorAngle, targetDirection,
                       errX, errY, errA, dir)
}

/////////////////////////////////////////////////////////////////////////
////                    mode switch drivers                          ////

driver goForward(errorX, errorY, errorAngle, targetDirection) output () {
    if GoForward(errorX, errorY, errorAngle, targetDirection) then dummy()
}

driver goIdle(errorX, errorY, errorAngle, targetDirection) output () {
    if GoIdle(errorX, errorY, errorAngle, targetDirection) then dummy()
}

driver goRotate(errorX, errorY, errorAngle, targetDirection) output () {
   if GoRotate(errorX, errorY, errorAngle, targetDirection) then dummy()
}

driver goPoint(errorX, errorY, errorAngle, targetDirection) output () {
   if GoPoint(errorX, errorY, errorAngle, targetDirection) then dummy()
}

/////////////////////////////////////////////////////////////////////////
////                         modes                                  ////

start idle {

   /////////////////////////////////////////////////////////////////////
    ////                     mode idle                               ////

    mode idle() period 1000 {
        actfreq 1 do leftJet(leftMotor);
        actfreq 1 do rightJet(rightMotor);
        exitfreq 1 do idle(goIdle);
        exitfreq 1 do rotate(goRotate);
        exitfreq 1 do forward(goForward);
        exitfreq 1 do point(goPoint);
        taskfreq 2 do errorTask(getPos);
        taskfreq 1 do idleTask();
    }

    //////////////////////////////////////////////////////////////////////
    ////                     mode forward                             ////

    mode forward() period 200 {
        actfreq 1 do leftJet(leftMotor);
        actfreq 1 do rightJet(rightMotor);
        exitfreq 1 do point(goPoint);
        exitfreq 1 do idle(goIdle);
        exitfreq 1 do rotate(goRotate);
        taskfreq 2 do errorTask(getPos);
        taskfreq 1 do forwardTask(getErr);
    }

    //////////////////////////////////////////////////////////////////////
    ////                     mode rotate                              ////

    mode rotate() period 200 {
        actfreq 1 do leftJet(leftMotor);
        actfreq 1 do rightJet(rightMotor);
        exitfreq 1 do point(goPoint);
        exitfreq 1 do idle(goIdle);
        exitfreq 1 do forward(goForward);
        taskfreq 2 do errorTask(getPos);
        taskfreq 1 do turnTowardsTargetTask(getErr);
    }

    //////////////////////////////////////////////////////////////////////
    ////                     mode point                               ////

    mode point() period 200 {
        actfreq 1 do leftJet(leftMotor);
        actfreq 1 do rightJet(rightMotor);
        exitfreq 1 do point(goPoint);
        exitfreq 1 do idle(goIdle);
        exitfreq 1 do rotate(goForward);
        exitfreq 1 do forward(goRotate);
        taskfreq 2 do errorTask(getPos);
        taskfreq 1 do turnToTargetTask(getErr);
    }
}