/****************************************************************************/
/* 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)
   {
      // Manage ongoing moves
      if (_ezMove)
      {
         _output->data.motor_action = 1;
         if (EZ17Watch() == 0)
         {
            // Done with move
            if (_ezHome)
            {
               // If the move was to home, set the position
               _ez17->set_position(660);
               _ezMove = False; _output->data.motor_action = 0;
            }

            dprintf("Modtronix::EZ17Handler() - motor stopped\n");
            // Save the position to the startup script
            _ez17->save_position();
            _ezMove = False; _output->data.motor_action = 0;
         }
         else
         {
            dprintf("Modtronix::EZ17Handler() - motor moving\n");
         }
      }
   }
   else
   {
      Syslog::write("Modtronix::EZ17Handler() - got message %d", modmsg->_msg);
      switch (modmsg->_msg)
      {
         case ModtronixMsg::EZHome:
            dprintf("Modtronix::EZ17Handler() - Home stepper\n");
            _ezMove = _ezHome = True; _output->data.motor_action = 1;
            _ez17->home(); _ez17->home();
            break;

         case ModtronixMsg::EZFStop:
            dprintf("Modtronix::EZ17Handler() - Move to an fstop %.3f\n",
               modmsg->_int);
            _ezMove = True; _ezHome = False; _output->data.motor_action = 1;
            _ez17->goto_steps(EZ17FS2Steps(modmsg->_int));
            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_position = _ezPos;
   _output->write();
   
   return 0;
}

int Modtronix::EZ17FS2Steps(int fstop)
{
   // Decode the fstop
   //
   switch (fstop)
   {
      case 31:
         return 15;
      case 32:
         return 25;
      case 33:
         return 60;
      case 41:
         return 90;
      case 42:
         return 130;
      case 43:
         return 165;
      case 561:
         return 200;
      case 562:
         return 240;
      case 563:
         return 280;
      case 81:
         return 320;
      case 82:
         return 360;
      case 83:
         return 400;
      case 111:
         return 435;
      case 112:
         return 475;
      case 113:
         return 515;
      case 161:
         return 555;
      case 162:
         return 595;
      case 163:
         return 630;
      case 221:
         return 660;
      default:
         return -1;   // Unknown
   }
}

// Just check on the motor position
// If the position is not changing set _ezHome to False
//
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));
}