/****************************************************************************/
/* Copyright (c) 2014 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : EZ17 controller handler integrated into the Modtronix driver  */
/* Filename : EZMod.cc                                                      */
/* Author   : Henthorn                                                      */
/* Project  : i2MAP                                                         */
/* Version  : 1.0                                                           */
/* Created  : 06/03/2015                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include "Syslog.h"
#include "Modtronix.h"
#include "ModtronixMsg.h"
#include "ModtronixOutput.h"
#include "ModtronixLog.h"
#include "EZ17.h"

// Entry point for Modtronix EZ17 commands and management.
//
int Modtronix::EZ17Handler(ModtronixMsg::Message *modmsg)
{
   Boolean debug = False;

   if ( !_ez17 || !EZOn() )
   {
      dprintf("Modtronix::EZ17Handler() - motor i/f: %x, motor power: %d\n",
         _ez17, EZOn());
      return 0;
   }


   // Handle a new message or manage a job
   //
   if (NULL == modmsg)    // Handle on-going move, if applicable
   {
      // Manage ongoing moves
      if (_ezMove)
      {
         // Is the motor moving?
         // 
         if (EZ17Watch() == 0)
         {
            // Done with move
            // 
            _ezMove = False;

            // Has the motor stopped at the expected location?
            // 
            if (_expectedPos != _ezPos)
               Syslog::write("EZMod - Move finished, position is not %d.", _expectedPos);


            // Special if this was a move to the home position
            // 
            if (_ezHome)
            {
               _ezHome = False;           // Done, no longer homing

               if (0 !=_ezPos)
                  Syslog::write("EZMod - homing finished, position is not 0");

               _ez17->set_position(660);  // Set EZ17 stepper count
               _ezPos = 660;              // and Modtronix position

               _irisClosed = True;        // Home position is virtually a closed iris
            }

            // Always save the position in the EZ Stepper to use when it powers-up
            // 
            _ez17->save_position();

            dprintf("Modtronix::EZ17Handler() - motor stopped. Iris closed? %d\n",
               _irisClosed);
         }
         else
         {
            // Motor is still moving
            // 
            _ezMove = True;
            _irisClosed = False;
            dprintf("Modtronix::EZ17Handler() - motor moving\n");
         }
      }
   }
   else
   {
      Syslog::write("Modtronix::EZ17Handler() - got message %d %d %f",
         modmsg->_msg, modmsg->_int, modmsg->_float);

      // Check to see if the EZ17 is 'ready' for a move.
      // These function calls introduce a small delay that seems
      // to be required before executing move commands in quick
      // succession. A negative return value indicates the EZ17 is not
      // ready. Readiness is not required to attempt the command.
      // 
      if (_ez17->read_position(10) < 0) 
         Syslog::write("Modtronix::EZ17Handler() - EZ17 not ready for a move?");

      switch (modmsg->_msg)
      {
         case ModtronixMsg::EZHome:
            dprintf("Modtronix::EZ17Handler() - Home stepper\n");
            _ezMove = _ezHome = True;
            _expectedPos = 0;
            _ez17->home();
            break;

         case ModtronixMsg::EZFStop:
            dprintf("Modtronix::EZ17Handler() - Move to an fstop %.3f\n",
               modmsg->_int);
            _ezMove = True; _ezHome = False;
            _expectedPos = EZ17FS2Steps(modmsg->_int);
            _ez17->goto_steps(_expectedPos);
            break;

         case ModtronixMsg::EZAutoFS:
            dprintf("Modtronix::EZ17Handler() - Auto FS\n");
            EZ17FSLookup();
            break;

         default:
            dprintf("Modtronix::EZ17Handler() - Unknown message %d\n",
               modmsg->_msg);
            break;
      }
   }

//   EZ17Watch();
   _output->data.motor_action   = _ezMove;
   _output->data.motor_position = _ezPos;
   _output->write();

   return _ezMove;
}

#define FS31   15
#define FS32   25
#define FS33   60
#define FS41   90
#define FS42   130
#define FS43   165
#define FS561  200
#define FS562  240
#define FS563  280
#define FS81   320
#define FS82   360
#define FS83   400
#define FS111  435
#define FS112  475
#define FS113  515
#define FS161  555
#define FS162  595
#define FS163  630
#define FS221  660

int Modtronix::EZ17FS2Steps(int fstop)
{
   // Decode the fstop
   //
   switch (fstop)
   {
      case 31:
         return FS31;
      case 32:
         return FS32;
      case 33:
         return FS33;
      case 41:
         return FS41;
      case 42:
         return FS42;
      case 43:
         return FS43;
      case 561:
         return FS561;
      case 562:
         return FS562;
      case 563:
         return FS563;
      case 81:
         return FS81;
      case 82:
         return FS82;
      case 83:
         return FS83;
      case 111:
         return FS111;
      case 112:
         return FS112;
      case 113:
         return FS113;
      case 161:
         return FS161;
      case 162:
         return FS162;
      case 163:
         return FS163;
      case 221:
         return FS221;
      default:
         return -1;   // Unknown
   }
}

// Just check on the motor position. Return non-zero if position is changing.
//
int Modtronix::EZ17Watch()
{
   Boolean debug = False;

   int oldPos = _ezPos;
   if (EZOn() && _ez17)
   {
      oldPos = _ez17->last_known_position();
      _ezPos = _ez17->read_position();
      dprintf("Modtronix::EZ17Watch() - old:%d, new:%d\n", oldPos, _ezPos);
   }

   return (_ezPos - oldPos);
}

// TODO: Implement auto-iris setting depending on the light levels
//
float Modtronix::EZ17FSLookup()
{
   return 561;
}

Boolean Modtronix::EZOn()
{
   return (power_state(1));
}