context: /*
      Stereo vision for Surveyor robots
      Copyright (C) 2008 Bob Mottram
      fuzzgun@gmail.com
  
      This program is free software: you can redistribute it and/or modify
      it under the terms of the GNU General Public License as published by
      the Free Software Foundation, either version 3 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 General Public License for more details.
  
      You should have received a copy of the GNU General Public License
      along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
  
  using System;
  using System.Xml;
  using System.IO;
  using System.Threading;
  using System.Drawing;
  using sluggish.utilities;
  
  namespace surveyor.vision
  {
      public class SurveyorVisionStereo : BaseVisionStereo
      {
          private SurveyorVisionClient[] camera;
          private string host;
          private int[] port_number;	
  		public bool Pause;
  		
          #region "constructors"
          
          /// <summary>
          /// constructor
          /// </summary>
          /// <param name="host">host name or IP address</param>
          /// <param name="port_number_left">port number for the left camera</param>
          /// <param name="port_number_right">port number for the right camera</param>
          /// <param name="broadcast_port">port number on which to broadcast stereo feature data to other applications</param>
          /// <param name="fps">ideal frames per second</param>
          public SurveyorVisionStereo(
  		    string host,
              int port_number_left,
              int port_number_right,
              int broadcast_port,
              float fps) : base(broadcast_port, fps)
          {
              usage.Update("Create, SurveyorVisionStereo, Constructor");
              
              device_name = "Surveyor stereo camera";
              this.host = host;
              
              camera = new SurveyorVisionClient[2];
              for (int cam = 0; cam < 2; cam++)
              {
                  camera[cam] = new SurveyorVisionClient();
                  camera[cam].usage = usage;
                  camera[cam].grab_mode = SurveyorVisionClient.GRAB_MULTI_CAMERA;
  				camera[cam].cam_index = cam;
              }
              
              port_number = new int[2];
              port_number[0] = port_number_left;
              port_number[1] = port_number_right;            
          }
          
          #endregion
  		
  		#region "enabling and disabling embedded stereo"
  		
  		private bool enable_embedded = false;
  		private bool disable_embedded = true;
  		private bool rectification_enabled;
  		DateTime embedded_last_enabled_disabled = DateTime.Now;
  		
  		public void EnableEmbeddedStereo()
  		{
  			Console.WriteLine("Enabling embedded stereo");
              usage.Update("Enable embedded, SurveyorVisionStereo, EnableEmbeddedStereo");
              
  			embedded_last_enabled_disabled = DateTime.Now;
  			disable_embedded = false;
  			enable_embedded = true;	
  			rectification_enabled = !disable_rectification;
  			disable_rectification = true;
  		}
  		
  		public void DisableEmbeddedStereo()
  		{
  			Console.WriteLine("Disabling embedded stereo");
              usage.Update("Disable embedded, SurveyorVisionStereo, DisableEmbeddedStereo");
              
  			embedded_last_enabled_disabled = DateTime.Now;
  			disable_embedded = true;
  			enable_embedded = false;
  			disable_rectification = !rectification_enabled;
  		}
  		
  		#endregion
  
          #region "sending commands (typically for driving) to the Surveyor robot"
  
          /// <summary>
          /// send a command to the SRV robot 
          /// </summary>
          /// <param name="camera_index">index number of the camera (0 = left, 1 = right)</param>
          /// <param name="command">command to be sent</param>
          public void SendCommand(
              int camera_index,
              string command)
          {
              if (camera[camera_index] != null) {
                  usage.Update("Command: " + command + ", SurveyorVisionStereo, SendCommand");
                  camera[camera_index].send_command = command;
              }
          }
  
          #endregion
          
  		#region "state machine"
  		
  		private Bitmap bmp_state_left, bmp_state_right;
  		private int svs_state, prev_svs_state = -1;
  		public const int SVS_STATE_GRAB_IMAGES = 0;
  		public const int SVS_STATE_RECEIVE_IMAGES = 1;
  		public const int SVS_STATE_PROCESS_IMAGES = 2;
  		public const int EMBEDDED_TIMEOUT_SEC = 5;
  		private DateTime svs_state_last;
          private bool first_frame_request = true;
  				
  		public void update_state()
  		{
              usage.Update("Update entry state " + svs_state.ToString() + ", SurveyorVisionStereo, update_state");
              
  			if ((camera[0].Running) && 
  			    (camera[1].Running))
  			{
                  TimeSpan diff;
                  
  				switch(svs_state)
  				{
  				    case SVS_STATE_GRAB_IMAGES:
  				    {					    					
  					    int time_step_mS = (int)(1000 / fps);
  					    diff = DateTime.Now.Subtract(svs_state_last);
  					    if (diff.TotalMilliseconds > time_step_mS)
  					    {
  						    svs_state_last = DateTime.Now;
  						
  						    // enable embedded stereo
  						    TimeSpan diff2 = DateTime.Now.Subtract(embedded_last_enabled_disabled);
  						    if (enable_embedded)
  						    {
  							    if (diff2.TotalSeconds > EMBEDDED_TIMEOUT_SEC) enable_embedded = false;
  			                    camera[0].Embedded = true;
  				                camera[1].Embedded = true;			
  				                camera[0].EnableEmbeddedStereo();	
  							    //Console.WriteLine("Embedded stereo enabled");
  						    }
  						    // disable embedded stereo
  						    if (disable_embedded)
  						    {						    
  							    if (diff2.TotalSeconds > EMBEDDED_TIMEOUT_SEC) enable_embedded = false;
  			                    camera[0].Embedded = false;
  				                camera[1].Embedded = false;			
  				                camera[0].DisableEmbeddedStereo();	
  							    //Console.WriteLine("Embedded stereo disabled");
  						    }
  												
  			                // pause or resume grabbing frames from the cameras
  						    bool is_paused = false;
  			                if (correspondence != null)
  			                {
  			                    if ((!UpdateWhenClientsConnected) ||
  			                        ((UpdateWhenClientsConnected) && (correspondence.GetNoOfClients() > 0)))
  			                        is_paused = false;
  			                    else
  			                        is_paused = true;
  			                }
  						
  						    if ((Pause) || (is_paused)) Console.WriteLine("Paused");
  						
  						    if (((!is_paused) || (Pause)) && 
                                  ((diff2.TotalSeconds > EMBEDDED_TIMEOUT_SEC) || (first_frame_request)))
  						    {
  						        // request images                        
  						        camera[0].RequestFrame();
  						        camera[1].RequestFrame();
                                  if ((camera[0].send_command != null) && (camera[0].send_command != ""))
                                      svs_state = SVS_STATE_GRAB_IMAGES;
                                  else
                                  {
  						            svs_state = SVS_STATE_RECEIVE_IMAGES;
                                      if (first_frame_request)
                                      {
                                          Console.WriteLine("Frames requested");
                                          first_frame_request = false;
                                      }
                                  }
  						    }
  					    }
  					    break;
  				    }
  				    case SVS_STATE_RECEIVE_IMAGES:
  				    {
  					    // both frames arrived
  				        if ((camera[0].frame_arrived) &&
  				            (camera[1].frame_arrived))
  					    {
  	                        bmp_state_left = (Bitmap)camera[0].current_frame;
  	                        bmp_state_right = (Bitmap)camera[1].current_frame;
  		                    if ((bmp_state_left != null) && 
  		                        (bmp_state_right != null))
  						    {
  							    // proceed to process the images
  							    svs_state = SVS_STATE_PROCESS_IMAGES;
  					    	}
  						    else
  						    {
  							    // images were invalid - try again
  							    svs_state = SVS_STATE_GRAB_IMAGES;
  						    }
  					    }
  					    else
  					    {
  				            int timeout_mS = 1000;
  	                        diff = DateTime.Now.Subtract(svs_state_last);
  						    if (diff.TotalMilliseconds > timeout_mS)
  						    {
  							    // timed out - request images again
  
gt: 							    Console.WriteLine("Timed out waiting for images");