/**@file CamView/RT_EXEC.cpp:
  Real time executive for camera viewer. */

#include <wx/wxprec.h>
#include <wx/confbase.h>
#ifndef WX_PRECOMP
 #include <wx/wx.h>
#endif

#include "../library/hardware/timer.h"
#include "../library/wxTools.h"
#include "../library/hardware/OsSupp.h"

#include "RT_EXEC.h"


const int LEFT_INITIALISATION=800;
const int RET2HORIZONTAL=520;

const double k=1.0/(RET2HORIZONTAL-LEFT_INITIALISATION);
const double q=(double)RET2HORIZONTAL/(RET2HORIZONTAL-LEFT_INITIALISATION);

const int _k=-(LEFT_INITIALISATION-RET2HORIZONTAL);
const int _q=-RET2HORIZONTAL;


/*          +----------+
 * Video -->|          |                  +-----+ 
 * Req ---->|controller|--->Controller--->|scale|-->Output
 *          |          |  ->Dummy         +-----+ 
 *          +----------+ 
 */


CamRtExec MRT;  ///< global object that contains camera real time variables


#define MIN_ITERATE_CAMERA_DEVICES 5


/** Set main period of real time */
void CamRtExec::SetPeriodMs(unsigned NewPeriod)
{
unsigned n;

  if(NewPeriod!=ControlPeriod)
    {    
    if(NewPeriod>20) 
      {
      n = NewPeriod/10;
      period = 10;
      ControlPeriod = n*period;
      }
    else
      {      
      period=ControlPeriod=NewPeriod;
      }

//    ExternalPID.Ts = ControlPeriod/1000.0;
//    ExternalPID.LastChange++;

    if(TimerRunning()) 
        {
	Stop();
	Start(period); 
        }
    }
}


/** Read a period of controller. This period differs from timer,
  because acceleration of stepper motors must be calculated faster. */
int CamRtExec::GetPeriodMs(void)
{
  return ControlPeriod;
}


/** Initialise real time executive for BallAndPlate */
CamRtExec::CamRtExec(void):
  ControlMode(CTR_BEFORE_STARTUP)
{  
  VideoX=VideoY=NULL;

  Sensors=NULL;
  CalibratePlate=NULL;

  ControllerX.SetDouble(0);
  ControllerY.SetDouble(0);
}


/** Scan all cameras available. */
CameraDevice *SelectCamDriver(bool ForceSelect, int CamNo=1, CameraDevice *OldCamDriver=NULL)
{
wxString PreferredDrv;
wxString str;
BazeCameraDriver *CamFactory;
int drv;
int PreselectedCamera=-1;
CameraDevice *CamDriver=NULL, *GoodCamDriver=NULL;
wxArrayString strings;
int Width=DEFAULT_VIDEO_WIDTH;	  ///< Default camera video resolution - width
int Height=DEFAULT_VIDEO_HEIGHT;  ///< Default camera video resolution - height
int Depth=24;			  ///< Default camera depth

  wxConfigBase *pConfig = wxConfigBase::Get();
  if(pConfig)
    {
    if(CamNo==1)
       pConfig->SetPath("/camera1/");
    else
       pConfig->SetPath("/camera2/");
    PreferredDrv = pConfig->Read(_T("CameraDriver"),"");
    Width = pConfig->Read("width", DEFAULT_VIDEO_WIDTH);
    Height = pConfig->Read("height", DEFAULT_VIDEO_HEIGHT);
    Depth = pConfig->Read("depth", Depth);
    pConfig->SetPath("../");
    }
  CamFactory=BazeCameraDriver::GetFirst();

  while(CamFactory!=NULL)
    {
    drv=0;
    CamDriver = CamFactory->GetDriver(drv); 
    while(CamDriver!=NULL || drv<MIN_ITERATE_CAMERA_DEVICES)
      {
      if(CamDriver!=NULL)
        {        
        GoodCamDriver = CamDriver;
        str.Printf(wxT("#%d "),drv);        
        str = str + CamFactory->GetName();
        str = str + CamDriver->GetName();

        if(!ForceSelect && PreferredDrv==str) 
	  {
          CamDriver->SelectVideoMode(Width,Height,Depth);
	  if(CamDriver!=OldCamDriver)
	    {
	    if(OldCamDriver!=NULL)
              {       
              if(OldCamDriver->UssageCount>0)
	         OldCamDriver->UssageCount--;
              //if(OldCamDriver->UssageCount<=0)
                 //OldCamDriver->Stop(true);		//disconnect graph for unused camera
              }
            if(CamDriver!=NULL)
              CamDriver->UssageCount++;
	    }
	  return CamDriver;
	  }

        strings.Add(str);
        if(OldCamDriver==CamDriver)
          PreselectedCamera = strings.GetCount()-1;
        }
      drv++;
      CamDriver = CamFactory->GetDriver(drv); 
      }
    CamFactory = CamFactory->GetNext();
    } 

  if(strings.GetCount()<=1)
    {
    CamDriver = GoodCamDriver;
    }

  if(strings.GetCount()>1 || (ForceSelect)) // && strings.GetCount()>=1))
    {
    int result = wxGetSingleChoiceIndex(_("Select camera:"),
	   _("Available Camera Devices"), PreselectedCamera, strings, NULL //wxFindSuitableParent()
	   );
    if(result<0)	//cancel was triggered
      {
      if(ForceSelect) return OldCamDriver; //return last choice after cancel
      return NULL;	
      }	
    else
      {
      int drivers=0;
      CamFactory = BazeCameraDriver::GetFirst();  
      while(CamFactory!=NULL && drivers<=result)  // find n'th driver - yes it is somehow clumsy
        {
        drv=0;
        while((CamDriver=CamFactory->GetDriver(drv))!=NULL)
          {	  
          drivers++;
          drv++;
	  if(result==drivers-1)
	    {
	    if(pConfig)
	      {
	      if(CamNo==1)
	        pConfig->Write(_T("/camera1/CameraDriver"),strings[drivers-1]);
	      else
		pConfig->Write(_T("/camera2/CameraDriver"),strings[drivers-1]);
	      }
	    goto ExitPoint;
            }
          }
        CamFactory = CamFactory->GetNext();
        }
      }
    }

ExitPoint:
  if(CamDriver)
    {
    if(CamDriver!=OldCamDriver)
      {
      if(OldCamDriver!=NULL)
        {       
        if(OldCamDriver->UssageCount>0)
	  OldCamDriver->UssageCount--;
        if(OldCamDriver->UssageCount<=0)    
          OldCamDriver->Stop(true);		//disconnect graph for unused camera
        }
      if(CamDriver!=NULL)
        CamDriver->UssageCount++;
      }

    CamDriver->SelectVideoMode(Width,Height,Depth);
    if(labs(CamDriver->GetHeight())==0 || labs(CamDriver->GetWidth())==0)
      {
      wxMessageBox(_("Selected camera does not work properly!"),APP_NAME, wxOK|wxICON_EXCLAMATION);
      return NULL;
      }
    }
  return CamDriver;
}


/** Select some available HW driver. If more driver exists, ask user which to use. */
void CamRtExec::Hook2Drv(int SelectType)
{
wxArrayString strings;
wxString str;
wxString PreferredDrv;

  if(SelectType & SEL_CAM)
    {    
    try
      {
      CT.Camera = SelectCamDriver(SelectType&SEL_FORCE,1,CT.Camera);      
      }
    catch(std::exception ex)
      {
      wxMessageBox(_(ex.what()), APP_NAME, wxOK|wxICON_EXCLAMATION);
      CT.Camera = NULL;
      }
    catch(...)
      {
      CT.Camera = NULL;
      }
    }  

  if(SelectType & SEL_CAM2)
    {    
    try
      {
      CT2.Camera = SelectCamDriver(SelectType&SEL_FORCE,2,CT2.Camera);
      }
    catch(std::exception ex)
      {
      wxMessageBox(_(ex.what()),APP_NAME, wxOK|wxICON_EXCLAMATION);
      CT2.Camera = NULL;
      }
    catch(...)
      {
      CT2.Camera = NULL;
      }
    }

  if(CT.Camera!=NULL)
    {
    threshold=CT.GetInput(2);

    CT.Camera->Start();
    }
  if(CT2.Camera!=NULL && CT2.Camera!=CT.Camera)
    {
    //threshold=CT.GetInput(2);

    CT2.Camera->Start();
    }

}


/** Do one calculation step of real time network. */
void CamRtExec::Tick(void)
{
  TickCount+=period;

  //try {CT.Execute();}
  //catch(...) {}  /// \todo: emit any warning
}


/** Release all resources used by real time executive. */
void CamRtExec::DismantleDrivers(void)
{   //Please note, that all drivers might be already destructed!  
  if(CT.Camera) 
    {
    threshold=
      VideoX=VideoY=NULL;	//Release all camera related RT points - just for safety
    //delete CT.Camera;		//delete is omitted, it should be handled from class factory
    CT.Camera=NULL;    
    }
}



/** Release all resources used by real time executive. */
CamRtExec::~CamRtExec()
{   //Please note, that all drivers might be already destructed!
  DismantleDrivers();  
}
