/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/ln/laos-drot/src/Service/LaosDrotServiceWorker.h 15070 2018-01-26 22:16:34Z mathar $ */

#pragma once

#include <Basda/Mocca/NServiceWorkerCore.h>
#include <Basda/Device.h>
#include <Nice/Util/Completion.h>
#include <Sharknir/Drot/SharknirDrotServiceWorkerSkeleton.h>
#include <Ltcs/Iif/ServiceWorkerInterface.h>
#include <LbtoIIFInterface.h>
#include <Nice/Property.h>
#include <config.h>
#include <ostream>
#include <iostream>
#include <list>
#include <algorithm>
#include <boost/math/special_functions/round.hpp>

#include <deque>

namespace Sharknir
{

enum EDrotModes
{
    DROT_MODE_PUPIL,
    DROT_MODE_FIELD_FIXED,
    DROT_MODE_FIELD_FREE
};

/** @wince 2014-08-22
 * @author kittmann */
class DrotServiceWorker : public Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>
{
struct DeviceAsyncImpl : Basda::DeviceAsync
{
    virtual int asyncWait(const Nice::Time & _t);
    virtual int asyncStatus(const std::string & _s, int _d);
    DrotServiceWorker * m_worker;
};
DeviceAsyncImpl m_deviceAsync;

private:

struct ControlFlags
{
    bool bLogMisalignment;
    bool bLogDerotationPositions;
    bool bTraceDebug;
} m_ControlFlags;

void createTrajectory(const Nice::Date & _startDate);

void getReady(double _startPosition, std::string _unit);

//            void status();

/** @brief the interface to ask the IIF for derotator polynomials or parallactic angle.  */
Ltcs::IifServiceWorkerInterfacePtr m_iif;

/** @brief A vector with the coordinate transformation values. (eg. degree to steps) */
Nice::MapStringPoint m_transformation;

/** @brief projected start time of the derotator polynomial  */
Nice::Date m_startDate;
/** @brief value of first derotator polynomial  */
double m_startAngle;

/** Time interval for the duration of one derotator polynomial (seconds)  */
double m_samplingRate;

/** Number of polynomial slices from the IIF  */
unsigned int m_numberOfPolynomials;
/**   */
double m_frequency;
/**   */
unsigned int m_polynomialOrder;
/**   */
unsigned int m_faultTolerance;
/**   */
unsigned int m_samplesPerSegment;
/** The position in absenc where the zenith is in the vertical direction of the scicam */
double m_referencePos;
/** The error in degree of the narrow slit from the vertical measred counterclockwise */
double m_slitNarrowPos;
/** The error in degree of the wider slit from the vertical measred counterclockwise */
double m_slitWidePos;

/**  @brief number of motor steps to rotate by one radian  */
double m_stepsPerRad;

/** @brief number of motor steps to rotate by one degree  */
double m_stepsPerDegree;

/** @brief accumulated angle since trajectory start in radian  */
double m_ang_rad;

/** @brief accumulated angle since trajectory start in steps  */
double m_ang_steps;

/** direction of motor steps relative to derotator polynomials.
 * Includes a factor 1/2 to slow down axle for the K-mirrors.  */
float m_direction;

Nice::NPoint m_positionSequence;
Nice::JointPtr m_resolutionProperty;
Nice::Date m_derotStartDate;
std::vector<Nice::Date> m_positionTimes;
std::vector<double> m_derotPositions;

int m_absencLead;
pthread_t m_absenc_tid;
int m_SimulatedAbsoluteEncoder;
int m_SimulatedDestination;
static void * absenc_thread(void * pMe);

int m_travelRangeMin;
int m_travelRange;
int m_microsteps;
// int m_travelRangeABSENC;
double m_absencX, m_absencY;

void resetPosition();
void resetPosition(double to_be_set);
void initConfig();

EDrotModes m_drotMode;


std::deque<int> m_CBqueue;

// this member keeps trak of the steps position obtained by transformiung the
// absolute encoder, without performing modulo operation with travel range.
// it is used to transform steps into ABSENC, whose value should mimic
// absolute encoder value.
// double m_stepsFromAbsoluteEncoder;

void callback_check_reached_position(const Nice::Completion& _c);    //,
/* const Sharknir::DrotServiceWorkerInterfacePtr& _service, */
/* const long int _cmdPosition); */

void SaveDerotationLog();

public:
DrotServiceWorker ( const Nice::JointPtr & _config, const Basda::MoccaNDevicePtr & _device );

virtual ~DrotServiceWorker();

/** @brief This method initiate and open the interface.  */
virtual void init();

/** @brief This method activate the device.  */
virtual void activate();

/** @brief Change the absolute position of all motors.
 *
 * The motor will not move, but the current absolute position will be
 * set to the given position value.
 * The new position value can only be inside the travel range.
 *
 * @param _unit the unit in which the position value should be converted
 * @param _position a vector with the new absolute position of all motors */
virtual void setPosition(double _position, std::string _unit);

virtual void moveToHome();

bool deltaCorrection0(/*double _pos, std::string _unit*/);
bool deltaCorrection(double _pos, std::string _unit);

virtual void setDerotationParameters1(int numPolynomials, double samplingRate, double frequency);
virtual void setDerotationParameters2(int polynomialOrder, int faultTolerance, int samplesPerSegment);
virtual void setDerotationParameters3(double debug1, double debug2, double debug3);
virtual void setDerotationParameters4(double debug4, double debug5, double debug6);

virtual void moveAbsolute(double _position, std::string _unit);
virtual void moveAbsoluteWait(  const Nice::Time & _wait = Nice::Time());

virtual void moveRelative(double _position, std::string _unit);
virtual void moveRelativeWait(  const Nice::Time & _wait = Nice::Time());

virtual void idleOnline();

virtual void changeDerotationWithAbsoluteOffset(const Nice::Date & _startPositionAbsolute, double _startPosition, std::string _unit);

virtual void changeDerotationWithRelativeOffset(const Nice::Date & _startPositionRelative, double _startPosition, std::string _unit);

// virtual Nice::SeqPoint getKFPCoordinates(const lbto::SeqPos2 backupCat) ;
virtual Nice::SeqPoint getKFPCoordinates();
virtual lbto::SeqPos2 getPresetStars();
//bool waitWhileSlewing(const Nice::Time & tout);
bool isSlewing();
virtual std::string getSHARKNIRstation();
virtual std::string getMode();

virtual double getParallacticAngle();

/* virtual std::string checkForNamedPosition(); */
virtual double getAbsoluteEncoderPosition();
virtual double getPosition(std::string _unit="STEPS");
virtual double getPosition0(std::string _unit="STEPS");

virtual int sharkDerotatorBacklashProcedure();
virtual int OsStartDerotationTest(double startPosDegree, double endPosDegree, double speedDegreeSec);
virtual void startDerotation(const Nice::Date & _startDate, double _startPosition, std::string _unit);
virtual int OsStartDerotation(const Nice::Date & _startDate, double _positionAngle, int _correction, bool _autoStart);
virtual int OsWaitDerotation(int seconds);
// virtual int OsExtimateDerotation(const Nice::Date & _startDate, double _startPosition, std::string _unit, bool _autoStart);
void OsExtimateDerotation(Nice::Date & _startDate, double _positionAngle, int _correction, bool _autoStart, int & _finalSize, double & _effStartPositionSteps);
int OsExtimateStartingPosition(const Nice::Date & _startDate, double _positionAngle, int _correction, bool _autoStart);
bool m_bDerotationRunning;
Nice::SeqDouble OsGetDerotationProfile(const Nice::Date & start_date, int number_of_polynomials, float time_interval);


virtual void startDerotationHere(const Nice::Date & _startDate);


virtual double convertPosition(double _position, bool _absolute, std::string _unitFrom, std::string _unitTo);

virtual void transformationFactorChanged(const Nice::Any & _a);

double coordinateConversionFromSteps(double _value, std::string _unit); // Same as coordinatetransformation to be called internally
virtual double coordinateTransformation(double _value, std::string _unit); // Transformation from STEPS (or maybe CFG.POSITION.UNIT)
virtual double coordinateTransformation0(double _value, std::string _unit);

virtual double coordinateTransformationInverse(double _value, std::string _unit); //Transformation to STEPS (or maybe CFG.POSITION.UNIT)

virtual double coordinateTranslateInverse(double _value, std::string _unit);

virtual double coordinateTranslate(double _value, std::string _unit);

virtual std::string getRA();

Nice::Time timeLeft();
Nice::Time timeLeft(int frequency, double _startPosition, std::string _unit, bool trunc);

};

typedef IceUtil::Handle<Sharknir::DrotServiceWorker> DrotServiceWorkerPtr;
typedef Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton> drotBaseClassType;

}

namespace Basda
{
template<> Nice::Point Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, Sharknir::DrotServiceWorkerSkeleton>::getDefaultTransformation();
}
