#if 0

#############################
##
## BAORadio Indi driver
## Franck RICHARD
## Mai 2010
##
#############################
#endif

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>
#include <unistd.h>
#include <time.h>
#include <memory>
#include <pthread.h>
#include <iostream>
#include <time.h>
#include <unistd.h>
#include <sys/time.h>

#include <config.h>

#include "indicom.h"

#include "Socket.h"

#include "BAO.h"

using namespace std;

auto_ptr<BAO> telescope(0);

const int POLLMS = 1;				// Period of update, 1 ms.

const char *mydev = "BAO";			// Name of our device.

const char *BASIC_GROUP    = "Main Control";		// Main Group
const char *OPTIONS_GROUP  = "Options";			// Options Group

/* Handy Macros */
//#define currentRA	EquatorialCoordsRN[0].value
//#define currentDEC	EquatorialCoordsRN[1].value
#define targetRA	EquatorialCoordsWN[0].value
#define targetDEC	EquatorialCoordsWN[1].value


static void ISPoll(void *);

static void retry_connection(void *);

/**************************************************************************************
**
***************************************************************************************/
void *pThreadSocket (void * arg)
{
    try
    {
        int pos = SocketsNumber;

        // Trouver éventuellement un emplacement disponible dans l'intervalle [1..SocketsNumber]

        /*
        for (int i=1; i<SocketsNumber; i++)
        {
        if (!Sockets[i].Connected)
        {
        pos = i;
        break;
        }
        }*/

        server.accept ( Sockets[pos].new_sock );

        Sockets[pos].IP=server.recupip(Sockets[pos].new_sock);

        Sockets[pos].Connected=true;

        if (pos == SocketsNumber ) SocketsNumber++;

        InitThreadOK=true;
    }
    catch ( SocketException& e )
    {
        //A activer pour vérif

        /*std::string oss;
        oss="pThreadSocket exception : " + e.description() + "\n";
        size_t size = oss.size() + 1;
        char* buffer = new char[size];
        strncpy(buffer, oss.c_str(), size);

        IDLog(buffer);

        delete [] buffer;*/
    }

    return NULL;
}


/**************************************************************************************
** Utilisation d'un thread pour éviter de bloquer l'execution du pilote
** avec la commande accept
***************************************************************************************/

void BAO::InitThread()
{
    if (pthread_create (&th1, NULL, pThreadSocket, NULL) < 0)
    {
        IDLog("pthread_create error for threadSocket\n");
    }

    pthread_join (th1, NULL);
}

/**************************************************************************************
** Initialisation du pilote BAO
***************************************************************************************/
void ISInit()
{
    static int isInit=0;

    if (isInit) return;

    if (telescope.get() == 0) telescope.reset(new BAO());

    isInit = 1;

    IEAddTimer (POLLMS, ISPoll, NULL);
}

/**************************************************************************************
**
***************************************************************************************/
void ISGetProperties (const char *dev)
{
    ISInit();
    telescope->ISGetProperties(dev);
}

/**************************************************************************************
**
***************************************************************************************/
void ISNewSwitch (const char *dev, const char *name, ISState *states, char *names[], int n)
{
    ISInit();
    telescope->ISNewSwitch(dev, name, states, names, n);
}

/**************************************************************************************
**
***************************************************************************************/
void ISNewText (const char *dev, const char *name, char *texts[], char *names[], int n)
{
    ISInit();
    telescope->ISNewText(dev, name, texts, names, n);
}

/**************************************************************************************
**
***************************************************************************************/
void ISNewNumber (const char *dev, const char *name, double values[], char *names[], int n)
{
    ISInit();
    telescope->ISNewNumber(dev, name, values, names, n);
}

/**************************************************************************************
**
***************************************************************************************/
void ISPoll (void *p)
{
    INDI_UNUSED(p);

    telescope->ISPoll();
    IEAddTimer (POLLMS, ISPoll, NULL);
}


/**************************************************************************************
**
***************************************************************************************/
void ISNewBLOB (const char *dev, const char *name, int sizes[], int blobsizes[], char *blobs[], char *formats[], char *names[], int n)
{
    INDI_UNUSED(dev);
    INDI_UNUSED(name);
    INDI_UNUSED(sizes);
    INDI_UNUSED(blobsizes);
    INDI_UNUSED(blobs);
    INDI_UNUSED(formats);
    INDI_UNUSED(names);
    INDI_UNUSED(n);
}

/**************************************************************************************
**
***************************************************************************************/
void ISSnoopDevice (XMLEle *root)
{
    INDI_UNUSED(root);
}

/**************************************************************************************
** Initialisation de la classe BAO
***************************************************************************************/
BAO::BAO()
{
    init_properties();

    ConnectSP.s = IPS_IDLE;

    lastSet        = -1;
    fd             = -1;
    simulation     = false;
    lastRA         = 0;
    lastDEC        = 0;
    currentSet     = 0;
    JJAnc          = 0.0;

    SocketsNumber  = 1;

    ActualisationTM1 = 15.0*60.0;

    ActualisationTM2 = 5.0;

    InitThreadOK=false;
    LecturePosition=false;
    Abort=false;
    Park=false;
    Goto=false;
    LastGotoOK=true;

    TrackingMode=1;

    for (int i=0; i<MAXHOSTNAME; i++)
    {
        Sockets[i].Connected=false;
        Sockets[i].IP="";
    }

    InitAntennes();


    IDLog("Initilizing from BAO device...\n");
    IDLog("Driver Version: 2010-11-10\n");

    //enableSimulation(true);
}

/**************************************************************************************
**
***************************************************************************************/
BAO::~BAO()
{

}

/**************************************************************************************
** Initialisation des boutons et des zones d'affichage dans la boîte de dialogue INDI
***************************************************************************************/
void BAO::init_properties()
{
    // Connection
    IUFillSwitch(&ConnectS[0], "CONNECT", "Connect", ISS_OFF);
    IUFillSwitch(&ConnectS[1], "DISCONNECT", "Disconnect", ISS_ON);
    IUFillSwitchVector(&ConnectSP, ConnectS, NARRAY(ConnectS), mydev, "CONNECTION", "Connection", BASIC_GROUP, IP_RW, ISR_1OFMANY, 60, IPS_IDLE);

    // Coord Set
    IUFillSwitch(&OnCoordSetS[0], "TRANSIT", "Transit", ISS_ON);
    IUFillSwitch(&OnCoordSetS[1], "TRACKING", "Tracking", ISS_OFF);
    IUFillSwitchVector(&OnCoordSetSP, OnCoordSetS, NARRAY(OnCoordSetS), mydev, "ON_COORD_SET", "On Set", BASIC_GROUP, IP_RW, ISR_1OFMANY, 0, IPS_IDLE);

    // Abort
    IUFillSwitch(&AbortSlewS[0], "ABORT", "Abort", ISS_OFF);
    IUFillSwitchVector(&AbortSlewSP, AbortSlewS, NARRAY(AbortSlewS), mydev, "ABORT_MOTION", "Abort", BASIC_GROUP, IP_RW, ISR_ATMOST1, 0, IPS_IDLE);

    // Park
    IUFillSwitch(&ParkS[0], "PARK", "Park", ISS_OFF);
    IUFillSwitchVector(&ParkSP, ParkS, NARRAY(ParkS), mydev, "", "Park", BASIC_GROUP, IP_RW, ISR_ATMOST1, 0, IPS_IDLE);

    // Object Name
    IUFillText(&ObjectT[0], "OBJECT_NAME", "Name", "--");
    IUFillTextVector(&ObjectTP, ObjectT, NARRAY(ObjectT), mydev, "OBJECT_INFO", "Object", BASIC_GROUP, IP_RW, 0, IPS_IDLE);


    // Equatorial Coords - SET
    IUFillNumber(&EquatorialCoordsWN[0], "RA", "RA  H:M:S", "%10.6m",  0., 24., 0., 0.);
    IUFillNumber(&EquatorialCoordsWN[1], "DEC", "Dec D:M:S", "%10.6m", -90., 90., 0., 0.);
    IUFillNumberVector(&EquatorialCoordsWNP, EquatorialCoordsWN, NARRAY(EquatorialCoordsWN), mydev, "EQUATORIAL_EOD_COORD_REQUEST" , "Equatorial JNow", BASIC_GROUP, IP_WO, 0, IPS_IDLE);

    // Equatorial Coords - READ
    //    IUFillNumber(&EquatorialCoordsRN[0], "RA", "RA  H:M:S", "%10.6m",  0., 24., 0., 0.);
    //    IUFillNumber(&EquatorialCoordsRN[1], "DEC", "Dec D:M:S", "%10.6m", -90., 90., 0., 0.);
    //    IUFillNumberVector(&EquatorialCoordsRNP, EquatorialCoordsRN, NARRAY(EquatorialCoordsRN), mydev, "EQUATORIAL_EOD_COORD" , "Equatorial JNow", BASIC_GROUP, IP_RO, 0, IPS_IDLE);

    // Geographic coord - SET
    IUFillNumber(&GeographicCoordsWN[0], "LAT", "Lat  D", "%10.6m",  -90., 90., 0., 0.);
    IUFillNumber(&GeographicCoordsWN[1], "LONG", "Long D", "%10.6m", 0., 360., 0., 0.);
    IUFillNumberVector(&GeographicCoordsWNP, GeographicCoordsWN, NARRAY(GeographicCoordsWN), mydev, "GEOGRAPHIC_COORD" , "Geographic coords", OPTIONS_GROUP, IP_WO, 0, IPS_IDLE);

    // Actualisation - SET
    IUFillNumber(&ActualisationN1[0], "DELAY", "Transit mode delay (Sec)", "%10.6m",  0., 3600., 0., 0.);
    IUFillNumberVector(&ActualisationNP1, ActualisationN1, NARRAY(ActualisationN1), mydev, "DELAY1" , "", OPTIONS_GROUP, IP_WO, 0, IPS_IDLE);

    IUFillNumber(&ActualisationN2[0], "DELAY", "Tracking mode delay (Sec)", "%10.6m",  0., 3600., 0., 0.);
    IUFillNumberVector(&ActualisationNP2, ActualisationN2, NARRAY(ActualisationN2), mydev, "DELAY2" , "", OPTIONS_GROUP, IP_WO, 0, IPS_IDLE);

    // Transit threshold
    //IUFillNumber(&SlewAccuracyN[0], "TransitRA",  "RA (arcmin)", "%10.6m",  0., 60., 1., 3.0);
    //IUFillNumber(&SlewAccuracyN[1], "TransitDEC", "Dec (arcmin)", "%10.6m", 0., 60., 1., 3.0);
    //IUFillNumberVector(&SlewAccuracyNP, SlewAccuracyN, NARRAY(SlewAccuracyN), mydev, "Transit Accuracy", "", OPTIONS_GROUP, IP_RW, 0, IPS_IDLE);

    // Tracking threshold
    //IUFillNumber(&TrackAccuracyN[0], "TrackingRA", "RA (arcmin)", "%10.6m",  0., 60., 1., 3.0);
    //IUFillNumber(&TrackAccuracyN[1], "TrackingDEC", "Dec (arcmin)", "%10.6m", 0., 60., 1., 3.0);
    //IUFillNumberVector(&TrackAccuracyNP, TrackAccuracyN, NARRAY(TrackAccuracyN), mydev, "Tracking Accuracy", "", OPTIONS_GROUP, IP_RW, 0, IPS_IDLE);

}

/**************************************************************************************
** Initialisation de la boîte de dialogue INDI (suite)
***************************************************************************************/
void BAO::ISGetProperties(const char *dev)
{

    if (dev && strcmp (mydev, dev))
        return;

    // Main Control
    IDDefSwitch(&ConnectSP, NULL);
    IDDefText(&ObjectTP, NULL);
    IDDefNumber(&EquatorialCoordsWNP, NULL);
    //IDDefNumber(&EquatorialCoordsRNP, NULL);
    IDDefNumber(&GeographicCoordsWNP, NULL);
    IDDefSwitch(&OnCoordSetSP, NULL);
    IDDefSwitch(&AbortSlewSP, NULL);
    IDDefSwitch(&ParkSP, NULL);

    // Options
    //IDDefNumber(&SlewAccuracyNP, NULL);
    //IDDefNumber(&TrackAccuracyNP, NULL);
    IDDefNumber(&ActualisationNP1, NULL);
    IDDefNumber(&ActualisationNP2, NULL);

}

/**************************************************************************************
** En cas de changement de texte dans la boîte de dialogue
***************************************************************************************/
void BAO::ISNewText (const char *dev, const char *name, char *texts[], char *names[], int n)
{
    // Ignore if not ours
    if (strcmp (dev, mydev))
        return;

    if (is_connected() == false)
    {
        IDMessage(mydev, "Error ! Please connect before issuing any commands.");
        reset_all_properties();
        return;
    }

    // ===================================
    // Object Name
    // ===================================
    if (!strcmp (name, ObjectTP.name))
    {
        if (IUUpdateText(&ObjectTP, texts, names, n) < 0)
            return;

        ObjectTP.s = IPS_OK;
        IDSetText(&ObjectTP, NULL);
        return;
    }
}

/**************************************************************************************
** En cas de changement d'une valeur numérique dans la boîte de dialogue Indi
***************************************************************************************/
void BAO::ISNewNumber (const char *dev, const char *name, double values[], char *names[], int n)
{

    // Ignore if not ours
    if (strcmp (dev, mydev))
        return;

    if (is_connected() == false)
    {
        IDMessage(mydev, "Error ! BAO is offline. Please connect before issuing any commands.");
        reset_all_properties();
        return;
    }


    // ===================================
    // Geographic  Coords
    // ===================================
    if (!strcmp (name, GeographicCoordsWNP.name))
    {
        int i=0, nset=0, error_code=0;

        Latitude=0.0;
        Longitude=0.0;

        for (nset = i = 0; i < n; i++)
        {
            INumber *eqp = IUFindNumber (&GeographicCoordsWNP, names[i]);
            if (eqp == &GeographicCoordsWN[0])
            {
                Latitude = values[i];
                nset += Latitude >= -90.0 && Latitude <= 90.0;

                Latitude*=Pidiv180;
            }
            else if (eqp == &GeographicCoordsWN[1])
            {
                Longitude = values[i];
                nset += Longitude >= 0.0 && Longitude <= 360.0;

                Longitude*=-Pidiv180;
            }
        }

        if (nset == 2)
        {
            //Vérification
            //IDLog("Geographic : RA %5.2f - DEC %5.2f\n", Latitude, Longitude);

            GeographicCoordsWNP.s = IPS_OK;
            IDSetNumber(&GeographicCoordsWNP, NULL);
        }
        else
        {
            GeographicCoordsWNP.s = IPS_ALERT;
            IDSetNumber(&GeographicCoordsWNP, "Latitude or Longitude missing or invalid");

            Latitude=0.0;
            Longitude=0.0;
        }

        return;
    }


    // ===================================
    // Equatorial Coords
    // ===================================
    if (!strcmp (name, EquatorialCoordsWNP.name))
    {
        int i=0, nset=0, error_code=0;
        double newRA =0, newDEC =0;

        for (nset = i = 0; i < n; i++)
        {
            INumber *eqp = IUFindNumber (&EquatorialCoordsWNP, names[i]);
            if (eqp == &EquatorialCoordsWN[0])
            {
                newRA = values[i];
                nset += newRA >= 0 && newRA <= 24.0;
            }
            else if (eqp == &EquatorialCoordsWN[1])
            {
                newDEC = values[i];
                nset += newDEC >= -90.0 && newDEC <= 90.0;
            }
        }

        targetRA  = newRA;
        targetDEC = newDEC;

        if (nset == 2)
        {
            char RAStr[32], DecStr[32];
            double targetAZ, targetAlt;

            fs_sexa(RAStr, newRA, 2, 3600);
            fs_sexa(DecStr, newDEC, 2, 3600);

            IDLog("We received JNow RA %s - DEC %s\n", RAStr, DecStr);

            // on convertit les coordonnées équatoriales de la zone du ciel observée
            // en unités de codeurs des moteurs

            ADDEC2Motor(newRA, newDEC);

            if (process_coords() == false)
            {
                EquatorialCoordsWNP.s = IPS_ALERT;
                IDSetNumber(&EquatorialCoordsWNP, NULL);
            }
        }
        else
        {
            EquatorialCoordsWNP.s = IPS_ALERT;
            IDSetNumber(&EquatorialCoordsWNP, "Error ! RA or Dec missing or invalid");
        }

        return;
    }

    // ===================================
    // Actualisation
    // ===================================
    if (!strcmp (name, ActualisationNP1.name))
    {
        int i=0, nset=0, error_code=0;
        double newAct1 =0;

        for (nset = i = 0; i < n; i++)
        {
            INumber *eqp = IUFindNumber (&ActualisationNP1, names[i]);
            if (eqp == &ActualisationN1[0])
            {
                newAct1 = values[i];

                if (newAct1 >= 0.0 && newAct1 <= 3600.0)
                {
                    ActualisationTM1=newAct1;

                    ActualisationNP1.s = IPS_OK;
                    IDSetNumber(&ActualisationNP1, NULL);
                }
                else
                {
                    ActualisationNP1.s = IPS_ALERT;
                    IDSetNumber(&ActualisationNP1, "Error ! Delay invalid");
                }
            }
        }
    }

    if (!strcmp (name, ActualisationNP2.name))
    {
        int i=0, nset=0, error_code=0;
        double newAct2 =0;

        for (nset = i = 0; i < n; i++)
        {
            INumber *eqp = IUFindNumber (&ActualisationNP2, names[i]);
            if (eqp == &ActualisationN2[0])
            {
                newAct2 = values[i];

                if (newAct2 >= 0.0 && newAct2 <= 3600.0)
                {
                    ActualisationTM2=newAct2;

                    ActualisationNP2.s = IPS_OK;
                    IDSetNumber(&ActualisationNP2, NULL);
                }
                else
                {
                    ActualisationNP2.s = IPS_ALERT;
                    IDSetNumber(&ActualisationNP2, "Error ! Delay invalid");
                }
            }
        }

    } /* end EquatorialCoordsWNP */
}

/**************************************************************************************
** L'utilisateur clique sur l'un des boutons de la boîte Indi
***************************************************************************************/
void BAO::ISNewSwitch (const char *dev, const char *name, ISState *states, char *names[], int n)
{
    // ignore if not ours //
    if (strcmp (mydev, dev))
        return;

    // ===================================
    // Connect Switch
    // ===================================
    if (!strcmp (name, ConnectSP.name))
    {
        if (IUUpdateSwitch(&ConnectSP, states, names, n) < 0)
            return;

        connect_telescope();
        return;
    }

    if (is_connected() == false)
    {
        IDMessage(mydev, "Error ! BAORadio is offline. Please connect before issuing any commands.");
        reset_all_properties();
        return;
    }

    // ===================================
    // Coordinate Set
    // ===================================
    if (!strcmp(name, OnCoordSetSP.name))
    {
        if (IUUpdateSwitch(&OnCoordSetSP, states, names, n) < 0)
            return;

        currentSet = get_switch_index(&OnCoordSetSP);
        OnCoordSetSP.s = IPS_OK;
        IDSetSwitch(&OnCoordSetSP, NULL);
    }

    // ===================================
    // Abort slew
    // ===================================
    if (!strcmp (name, AbortSlewSP.name))
    {
        Abort=true;

        IUResetSwitch(&AbortSlewSP);

        /*
        if (EquatorialCoordsWNP.s == IPS_BUSY)
        {

        AbortSlewSP.s = IPS_OK;
        EquatorialCoordsWNP.s       = IPS_IDLE;
        EquatorialCoordsRNP.s       = IPS_IDLE;
        HorizontalCoordsWNP.s       = IPS_IDLE;
        IDSetSwitch(&AbortSlewSP, "Slew aborted.");
        IDSetNumber(&EquatorialCoordsWNP, NULL);
        IDSetNumber(&EquatorialCoordsRNP, NULL);
        IDSetNumber(&HorizontalCoordsWNP, NULL);
        }
        */
        return;
    }


    // ===================================
    // Park
    // ===================================
    if (!strcmp (name, ParkSP.name))
    {
        Park=true;

        IUResetSwitch(&ParkSP);

        /*
        if (EquatorialCoordsWNP.s == IPS_BUSY)
        {

        AbortSlewSP.s = IPS_OK;
        EquatorialCoordsWNP.s       = IPS_IDLE;
        EquatorialCoordsRNP.s       = IPS_IDLE;
        HorizontalCoordsWNP.s       = IPS_IDLE;
        IDSetSwitch(&AbortSlewSP, "Slew aborted.");
        IDSetNumber(&EquatorialCoordsWNP, NULL);
        IDSetNumber(&EquatorialCoordsRNP, NULL);
        IDSetNumber(&HorizontalCoordsWNP, NULL);
        }
        */
        return;
    }
}

/**************************************************************************************
** fct peut-être inutile
***************************************************************************************/
void BAO::handle_error(INumberVectorProperty *nvp, int err, const char *msg)
{
    nvp->s = IPS_ALERT;

    /* If the error is a time out, then the device doesn't support this property */
    if (err == -2)
    {
        nvp->s = IPS_ALERT;
        IDSetNumber(nvp, "Device timed out. Current device may be busy or does not support %s. Will retry again.", msg);
    }
    else
        /* Changing property failed, user should retry. */
        IDSetNumber( nvp , "%s failed.", msg);

    fault = true;
}

/**************************************************************************************
** Initialisation des vecteurs INDI
***************************************************************************************/
void BAO::reset_all_properties()
{
    ConnectSP.s			= IPS_IDLE;
    OnCoordSetSP.s		= IPS_IDLE;
    AbortSlewSP.s		= IPS_IDLE;
    ParkSP.s			= IPS_IDLE;
    ObjectTP.s			= IPS_IDLE;
    EquatorialCoordsWNP.s	= IPS_IDLE;
    //EquatorialCoordsRNP.s	= IPS_IDLE;
    GeographicCoordsWNP.s	= IPS_IDLE;
    //SlewAccuracyNP.s		= IPS_IDLE;
    //TrackAccuracyNP.s		= IPS_IDLE;
    ActualisationNP1.s		= IPS_IDLE;
    ActualisationNP2.s		= IPS_IDLE;

    IUResetSwitch(&OnCoordSetSP);
    IUResetSwitch(&AbortSlewSP);
    IUResetSwitch(&ParkSP);

    OnCoordSetS[0].s = ISS_ON;
    ConnectS[0].s = ISS_OFF;
    ConnectS[1].s = ISS_ON;

    IDSetSwitch(&ConnectSP, NULL);
    IDSetSwitch(&OnCoordSetSP, NULL);
    IDSetSwitch(&AbortSlewSP, NULL);
    IDSetSwitch(&ParkSP, NULL);
    IDSetText(&ObjectTP, NULL);
    IDSetNumber(&EquatorialCoordsWNP, NULL);
    //IDSetNumber(&EquatorialCoordsRNP, NULL);
    IDSetNumber(&GeographicCoordsWNP, NULL);
    //IDSetNumber(&SlewAccuracyNP, NULL);
    //IDSetNumber(&TrackAccuracyNP, NULL);
    IDSetNumber(&ActualisationNP1, NULL);
    IDSetNumber(&ActualisationNP2, NULL);

}

/**************************************************************************************
**
***************************************************************************************/
void BAO::correct_fault()
{
    fault = false;
    IDMessage(mydev, "Telescope is online.");
}

/**************************************************************************************
**
***************************************************************************************/
bool BAO::is_connected()
{
    if (simulation) return true;

    // return (ConnectSP.sp[0].s == ISS_ON);
    return (ConnectSP.s == IPS_OK);
}

/**************************************************************************************
**
***************************************************************************************/
static void retry_connection(void * p)
{

}


/**************************************************************************************
** Extraction de la position de l'antenne
** dans le retour de la commande POS
** POS/Valeur az/Valeur alt
***************************************************************************************/

Position BAO::ExtractPosition(std::string str)
{
    Position result;

    std::string str2;

    result.x = -1;
    result.y = -1;

    int pos = str.find("/");

    if (pos != string::npos)
    {
        str2 = str.substr(pos+1);

        pos = str2.find("/");

        if (pos != string::npos)
        {
            result.x = atoi(str2.substr(0, pos).c_str());

            result.y = atoi(str2.substr(pos+1).c_str());
        }
    }

    return result;
}



/************************************************************************************
* cette procédure convertit les coordonnées equatoriales de l'objet visé
* en unités de codeurs des paraboles (nb de tours des deux axes moteurs depuis la position PARK)
************************************************************************************/
void BAO::ADDEC2Motor(double newRA, double newDEC)
{
    double targetAz;
    double targetAlt;
    char AzStr[32];
    char AltStr[32];

    // Calcule la hauteur et l'azimut de la zone du ciel pointée (en fonction de la date et du lieu)

    Azimut(tsl, Latitude, newRA * 15.0 * Pidiv180, newDEC * Pidiv180, &targetAz, &targetAlt);

    // En degrés

    targetAlt *= N180divPi;
    targetAz *= N180divPi;

    // Affichage dans les logs

    fs_sexa(AzStr, targetAz, 2, 3600);
    fs_sexa(AltStr, targetAlt, 2, 3600);

    IDLog("Horizontal coords : az %s - Alt %s\n", AzStr, AltStr);

    // Le calcul est ici très sommaire et arbitraire :
    // Je considère qu'il y a 100 positions possibles sur les deux axes
    // De plus, je considère qu'il n'est pas possible de viser un objet à
    // moins de 30° de hauteur au-dessus de l'horizon

    TargetPosition.x=(int)(targetAz*100.0/360.0);

    targetAlt=((90.0-targetAlt)/60.0);

    if (targetAlt>=1.0)
    {
        targetAlt=1.0; //on ne peut pas viser un objet situé à moins de 30°
        //au-dessus de l'horizon

        IDSetSwitch(&OnCoordSetSP, "Erreur ! L objet suivi est situe a moins de 30° au-dessus de l horizon. Goto annule.");

        Goto=false;

        LecturePosition=false;

        InitAntennes();
    }

    TargetPosition.y=(int)(100.0*targetAlt);
}


/************************************************************************************
* Retourne simplement le nombre d'antennes connectées
* et capables de communiquer
************************************************************************************/

int BAO::AntennesConnectees()
{
    int num=0;

    for (int i=1; i<SocketsNumber; i++) if (Sockets[i].Connected) num++;

    return num;
}


/************************************************************************************
* Initialisation des paramètres des antennes
*
************************************************************************************/
void BAO::InitAntennes()
{
    for (int i=0; i < MAXHOSTNAME ; i++)
    {
        Sockets[i].status=0;
        Sockets[i].sendalertes=0;
        Sockets[i].AttenteExecution=0;
        Sockets[i].AnomaliesExecution=0;
        Sockets[i].etape=0;

        Sockets[i].ack_status=false;
        Sockets[i].ack_pos=false;
        Sockets[i].ack_park=false;
        Sockets[i].ack_abort=false;
        Sockets[i].ack_goto=false;

        Sockets[i].PosValides=false;
        Sockets[i].GotoOk=false;
    }
}


/**************************************************************************************
** Procédure principale
** Elle est appelée toutes les POLLMS ms (ici 1 ms)
***************************************************************************************/
void BAO::ISPoll()
{
    static bool ISPOLLRunning=false;
    static int memSocketsNumber=-1;
    static unsigned int compt=100;

    int pos;

    struct tm date;
    time_t t;
    struct timeval tv;
    struct timezone tz;

    if (is_connected() == false) return;

    if (ISPOLLRunning) return;

    ISPOLLRunning=true;

    compt++;


    //toutes les 100 millisec

    if ( compt%100 == 0)
    {
        //Récupération de la date et de l'heure

        time(&t);
        date=*gmtime(&t);
        gettimeofday(&tv, &tz);

        Annee=(double)(date.tm_year+1900);
        Mois=(double)(date.tm_mon+1);
        Jour=(double)date.tm_mday;
        Heu=(double)date.tm_hour;
        Min=(double)date.tm_min;
        Sec=(double)date.tm_sec+tv.tv_usec/1.0E6;
        UTCP=0.0;//(double)date.tm_isdst;


        //Calcul du temps sidéral local

        CalculTSL();


        //Y a-t-il de nouvelles tentatives de connexion sur le serveur ?

        InitThread();
    }




    if (InitThreadOK)  // Il faut qu'il y ait eu au moins une connexion détectée par le thread pour continuer
    {

        // Nouvelle connexion sur le socket !

        if (SocketsNumber>memSocketsNumber)
        {
            memSocketsNumber=SocketsNumber;

            IDSetSwitch(&ConnectSP, "Connexion de l antenne %s. (Antennes connectees : %i)",
                        Sockets[SocketsNumber-1].IP.c_str(), AntennesConnectees());
        }



        // Début des échanges avec les microcontrôleurs

        // Analyse des réponses des microcontrôleurs

        for (int i=1; i<SocketsNumber; i++) /*if (Sockets[i].Connected)*/
        {
            try
            {
                std::string reponse, memreponse;
                // on récupère la réponse du microcontrôleur

                Sockets[i].new_sock >> reponse;

                //IDSetSwitch(&OnCoordSetSP, "Réponse ISPOLL : %s\n", reponse.c_str());        // pour vérif

                //Dans le cas où plusieurs trames seraient arrivées entre deux appels de POLLMS
                //les traiter successivement

                pos=reponse.find("\n");   // d'où l'intérêt de mettre un '\n' à la fin des trames
                //pour différencier une trame de la précédente

                while ((pos!=string::npos) && (reponse.length()>1))
                {
                    memreponse=reponse.substr(pos+1);

                    reponse=reponse.substr(0, pos);


                    // On traite ici les acknowledges

                    if ((reponse.find("ACK")!=string::npos) && (reponse.find("NACK")==string::npos))
                    {
                        if (reponse.find("POSITION")!=string::npos)
                        {
                            Sockets[i].ack_pos=true;
                        }
                        else if (reponse.find("GOTO")!=string::npos)
                        {
                            Sockets[i].ack_goto=true;
                        }
                        else if (reponse.find("PARK")!=string::npos)
                        {
                            Sockets[i].ack_park=true;
                        }
                        else if (reponse.find("ABORT")!=string::npos)
                        {
                            Sockets[i].ack_abort=true;
                        }
                    }
                    else
                    {

                        //réponse à la requête POSITION
                        if (reponse.find("POSITION")!=string::npos)
                        {
                            if (reponse.find("NACK")!=string::npos)
                            {
                                OnCoordSetSP.s = IPS_ALERT;
                                IDSetSwitch(&OnCoordSetSP, "ALERTE antenne %s : position de l antenne inconnue !\n",
                                            Sockets[i].IP.c_str());
                                Sockets[i].PosValides=false;
                                // Si la position de l'antenne est inconnue, on déconnecte l'antenne
                                Sockets[i].Connected=false;
                            }
                            else if (Sockets[i].ack_pos)
                            {
                                OnCoordSetSP.s = IPS_OK;
                                Sockets[i].Pos = ExtractPosition(reponse);
                                Sockets[i].PosValides = true;
                                IDSetSwitch(&ParkSP, "Antenne %s : POSITION OK  (x=%i, y=%i)\n",
                                            Sockets[i].IP.c_str(), Sockets[i].Pos.x, Sockets[i].Pos.y);
                            }

                        }//réponse à la requête PARK
                        else if (reponse.find("PARK")!=string::npos)
                        {
                            if (reponse.find("NACK")!=string::npos)
                            {
                                ParkSP.s = IPS_ALERT;
                                IDSetSwitch(&ParkSP, "ALERTE antenne %s : erreur PARK !\n", Sockets[i].IP.c_str());
                            }
                            else if (reponse.find("OK")!=string::npos)
                            {
                                ParkSP.s = IPS_OK;
                                IDSetSwitch(&ParkSP, "Antenne %s : PARK OK\n",  Sockets[i].IP.c_str());
                            }

                        }//réponse à la requête ABORT
                        else if (reponse.find("ABORT")!=string::npos)
                        {
                            if (reponse.find("NACK")!=string::npos)
                            {
                                AbortSlewSP.s = IPS_ALERT;
                                IDSetSwitch(&AbortSlewSP, "ALERTE antenne %s : erreur ABORT !\n",  Sockets[i].IP.c_str());
                            }

                            if (reponse.find("OK")!=string::npos)
                            {
                                AbortSlewSP.s = IPS_OK;
                                IDSetSwitch(&AbortSlewSP, "Antenne %s : ABORT OK\n",  Sockets[i].IP.c_str());
                            }

                        } //réponse à la requête GOTO
                        else if (reponse.find("GOTO")!=string::npos)
                        {
                            if (reponse.find("NACK")!=string::npos)
                            {
                                OnCoordSetSP.s = IPS_ALERT;
                                IDSetSwitch(&OnCoordSetSP, "ALERTE antenne %s : Erreur GOTO !\n",  Sockets[i].IP.c_str());
                                Sockets[i].Connected=false;
                            }
                            else if (Sockets[i].ack_goto)
                            {
                                if (reponse.find("OK")!=string::npos)
                                {
                                    OnCoordSetSP.s = IPS_OK;

                                    Sockets[i].GotoOk=true;

                                    IDSetSwitch(&ParkSP, "Antenne %s : GOTO OK.\n",  Sockets[i].IP.c_str());

                                    lastRA  = targetRA;
                                    lastDEC = targetDEC;

                                    //IDLog("We received JNow RA %s - DEC %s\n", RAStr, DecStr);*/

                                    EquatorialCoordsWNP.s = IPS_OK;
                                    IDSetNumber (&EquatorialCoordsWNP, NULL);

                                    // Fin du Goto pour toutes les antennes ?

                                    int num=0;

                                    for (int j=1; j<SocketsNumber; j++) if (Sockets[j].Connected)
                                        {
                                            if (Sockets[j].GotoOk) num++;
                                        }

                                    if ((num == AntennesConnectees()) && (num>0))
                                    {
                                        LecturePosition=false;

                                        InitAntennes();

                                        LastGotoOK=true;

                                        IDSetSwitch(&OnCoordSetSP, "GOTO OK !");
                                    }
                                }
                            }
                        }
                    }

                    // On passe éventuellement à la trame suivante

                    reponse=memreponse;
                    pos=reponse.find("\n");

                }
            }
            catch (SocketException& e) //Aïe
            {
                Sockets[i].new_sock.shutdown();
                Sockets[i].new_sock.create();
                Sockets[i].Connected = Sockets[i].new_sock.connect((std::string)Sockets[i].IP);

                if (Sockets[i].Connected)
                {
                    Sockets[i].AttenteExecution=0;
                    Sockets[i].AnomaliesExecution=0;
                }

                std::string oss;
                oss="SocketException IsPoll : " + e.description() + "\n";
                size_t size = oss.size() + 1;
                char* buffer = new char[size];
                strncpy(buffer, oss.c_str(), size);
                IDLog(buffer);
                delete [] buffer;
            }
        }



        if (Abort)
        {
            IDSetSwitch(&ConnectSP, "Envoi de la commande Abort\n");

            Goto=false;

            for (int i=1; i<SocketsNumber; i++) if (Sockets[i].Connected)
                {
                    if (!ABORT(i)) Sockets[i].sendalertes++;
                }

            LecturePosition=false;

            InitAntennes();

            Abort=false;
        }


        if (Park)
        {
            IDSetSwitch(&ConnectSP, "Envoi de la commande Park\n");

            Goto=false;

            for (int i=1; i<SocketsNumber; i++) if (Sockets[i].Connected)
                {
                    if (!PARK(i)) Sockets[i].sendalertes++;
                }

            LecturePosition=false;

            InitAntennes();

            Park=false;
        }


        // Gestion du suivi

        if ((Goto) && (LastGotoOK))
        {
            // Durée entre deux actualisations

            double delai=ActualisationTM1/3600.0/24.0;   // Actualisation toutes les 15 minutes en mode transit

            if (TrackingMode==2) delai=ActualisationTM2/3600.0/24.0;   //et 5 secs en mode tracking


            // On actualise la position

            if (JJ-JJAnc > delai)
            {

                LastGotoOK=false;

                LecturePosition=true;

                ADDEC2Motor(targetRA, targetDEC);

                InitAntennes();

                JJAnc=JJ;
            }

            //Plus d'antenne !

            if (AntennesConnectees() == 0)
            {
                // LecturePosition=false;

                // Goto=false;

                // InitAntennes();

                if ( compt % 1000 == 0)  IDSetSwitch(&OnCoordSetSP, "Erreur ! Plus d antennes connectees !");
            }
        }



        // Exécution de la procédure complète de lecture de la position de l'antenne
        // puis envoi d'une commande Goto

        if (LecturePosition)
        {
            for (int i=1; i<SocketsNumber; i++) /*if (Sockets[i].Connected)*/
            {
                switch (Sockets[i].etape)
                {

                    //Envoi de la commande POS
                case 0 :
                {
                    Sockets[i].ack_pos=false;
                    Sockets[i].PosValides=false;

                    if (!POSITION(i)) Sockets[i].sendalertes++;

                    Sockets[i].etape++;
                }
                break;

                //ack POS
                case 1 :
                {
                    if (Sockets[i].ack_pos)
                    {
                        Sockets[i].AttenteExecution=0;
                        Sockets[i].AnomaliesExecution=0;
                        Sockets[i].etape++;
                        i--;
                    }
                    else
                    {
                        // on réitère l'ordre précédent si rien ne se passe

                        Sockets[i].AttenteExecution++;

                        if (Sockets[i].AttenteExecution>MAXATTENTE)
                        {
                            Sockets[i].etape=0;
                            Sockets[i].AttenteExecution=0;
                            Sockets[i].AnomaliesExecution++;
                        }

                        if (Sockets[i].AnomaliesExecution>MAXANOMALIES)
                        {
                            Sockets[i].etape=3;

                            // if ( compt % 1000 == 0)
                            {
                                IDSetSwitch(&OnCoordSetSP, "Erreur sur l antenne %s : pas d acknowledge recu apres l ordre POSITION. \
                                Deconnexion de l antenne.", Sockets[i].IP.c_str());
                                Sockets[i].Connected=false;
                            }
                        }
                    }
                }
                break;

                //Valeurs pos valides ?
                case 2 :
                {
                    if (Sockets[i].PosValides)
                    {
                        Sockets[i].AttenteExecution=0;
                        Sockets[i].etape++;
                    }
                    else
                    {
                        // on réitère l'ordre précédent si rien ne se passe

                        Sockets[i].AttenteExecution++;

                        if (Sockets[i].AttenteExecution>MAXATTENTE)
                        {
                            Sockets[i].etape=2;
                            Sockets[i].AttenteExecution=0;
                            Sockets[i].AnomaliesExecution++;
                        }

                        if (Sockets[i].AnomaliesExecution>MAXANOMALIES)
                        {
                            Sockets[i].etape=3;

                            // if ( compt % 1000 == 0)
                            {
                                IDSetSwitch(&OnCoordSetSP, "Erreur sur l antenne %s : la position retournee n est pas valide. \
                                Deconnexion de l antenne.", Sockets[i].IP.c_str());
                                Sockets[i].Connected=false;
                            }
                        }
                    }
                }
                break;

                //ack goto  ?
                case 4 :
                {
                    if (Sockets[i].ack_goto)
                    {
                        Sockets[i].AttenteExecution=0;
                        Sockets[i].AnomaliesExecution=0;
                        Sockets[i].etape++;
                    }
                    else
                    {
                        // on réitère l'ordre précédent si rien ne se passe

                        Sockets[i].AttenteExecution++;

                        if (Sockets[i].AttenteExecution>MAXATTENTE)
                        {
                            Sockets[i].etape=4;
                            Sockets[i].AttenteExecution=0;
                            Sockets[i].AnomaliesExecution++;
                        }

                        if (Sockets[i].AnomaliesExecution>MAXANOMALIES)
                        {
                            Sockets[i].etape=6;

                            // if ( compt % 1000 == 0)
                            {
                                IDSetSwitch(&OnCoordSetSP, "Erreur sur l antenne %s : pas d acknowledge recu apres l ordre GOTO. \
                                Deconnexion de l antenne.", Sockets[i].IP.c_str());
                                Sockets[i].Connected=false;
                            }
                        }
                    }
                }
                break;

                //goto ok  ?
                case 5 :
                {
                    if (Sockets[i].GotoOk)
                    {
                        Sockets[i].AttenteExecution=0;
                        Sockets[i].AnomaliesExecution=0;
                        Sockets[i].etape++;
                    }
                    else
                    {
                        // on réitère l'ordre précédent si rien ne se passe

                        Sockets[i].AttenteExecution++;

                        if (Sockets[i].AttenteExecution>MAXATTENTE)
                        {
                            Sockets[i].etape=5;
                            Sockets[i].AttenteExecution=0;
                            Sockets[i].AnomaliesExecution++;
                        }

                        if (Sockets[i].AnomaliesExecution>MAXANOMALIESGOTO)
                        {
                            Sockets[i].etape=6;

                            // if ( compt % 1000 == 0)
                            {
                                IDSetSwitch(&OnCoordSetSP, "Erreur sur l antenne %s : l antenne n a pas renvoye GOTO/OK. \
                                Deconnexion de l antenne.", Sockets[i].IP.c_str());
                                Sockets[i].Connected=false;


                            }
                        }
                    }
                }
                break;

                }
            }
        }






        // Détection d'anomalies concernant l'envoi de trames sur la socket. déconnexion du micro-cont ?

        for (int i=1; i<SocketsNumber; i++) if (Sockets[i].Connected)
            {
                if (Sockets[i].sendalertes > 0)
                {
                    Sockets[i].etape=5;

                    //  if ( compt % 1000 == 0)
                    {
                        IDSetSwitch(&OnCoordSetSP, "Erreur sur l antenne %s : deconnexion de l antenne.", Sockets[i].IP.c_str());

                        Sockets[i].Connected=false;
                    }
                }
            }



        //On attend que toutes les antennes soient prêtes pour lancer l'ordre Goto -> meilleure synchronisation

        int num=0;

        for (int i=1; i<SocketsNumber; i++) if (Sockets[i].Connected)
            {
                if (Sockets[i].etape == 3) num++; //fin de la procédure LecturePosition. Les antennes sont prêtes
                //à recevoir l'ordre GOto
            }

        if ((num == AntennesConnectees()) && (num>0))
        {
            for (int i=1; i<SocketsNumber; i++ ) if (Sockets[i].Connected)
                {
                    Sockets[i].ack_goto=false;
                    Sockets[i].AttenteExecution=0;
                    Sockets[i].AnomaliesExecution=0;

                    if (!GOTO(i, Sockets[i].Pos.x - TargetPosition.x, Sockets[i].Pos.y - TargetPosition.y )) Sockets[i].sendalertes++;

                    Sockets[i].etape++;
                }
        }
    }

    ISPOLLRunning=false;
}



/**************************************************************************************
**
***************************************************************************************/
bool BAO::process_coords()
{
    switch (currentSet)
    {
        // Transit
    case BAO_TRANSIT:

        EquatorialCoordsWNP.s = IPS_BUSY;

        IDSetNumber (&EquatorialCoordsWNP, NULL);

        InitAntennes();

        JJAnc=JJ;

        TrackingMode = 1;

        Goto=true;

        LastGotoOK=false;

        LecturePosition=true;

        ADDEC2Motor(targetRA, targetDEC);

        break;

        // Tracking
    case BAO_TRACKING:

        EquatorialCoordsWNP.s = IPS_BUSY;

        IDSetNumber (&EquatorialCoordsWNP, NULL);

        InitAntennes();

        JJAnc=JJ;

        TrackingMode = 2;

        Goto=true;

        LastGotoOK=false;

        LecturePosition=true;

        ADDEC2Motor(targetRA, targetDEC);

        break;
    }

    return true;
}

/**************************************************************************************
**
***************************************************************************************/
int BAO::get_switch_index(ISwitchVectorProperty *sp)
{
    for (int i=0; i < sp->nsp ; i++)
        if (sp->sp[i].s == ISS_ON)
            return i;

    return -1;
}

/**************************************************************************************
** Activation de l'interface
***************************************************************************************/
void BAO::connect_telescope()
{
    switch (ConnectSP.sp[0].s)
    {
    case ISS_ON:
        ConnectS[0].s = ISS_ON;
        ConnectS[1].s = ISS_OFF;
        IDLog("\nHello BAORadio !\n");

        ConnectSP.s = IPS_OK;
        IDSetSwitch (&ConnectSP, "BAORadio is online. Retrieving basic data...");


        break;

    case ISS_OFF:
        ConnectS[0].s = ISS_OFF;
        ConnectS[1].s = ISS_ON;
        ConnectSP.s = IPS_IDLE;

        SocketsNumber=1;
        InitThreadOK=false;

        for (int i=0; i<MAXHOSTNAME; i++)
        {
            Sockets[i].Connected=false;
            Sockets[i].IP="";
            Sockets[i].new_sock.shutdown();
        }

        InitAntennes();

        IDSetSwitch (&ConnectSP, "BAORadio is offline.");
        IDLog("Telescope is offline.");

        break;
    }
}

/**************************************************************************************
**
***************************************************************************************/
void BAO::get_initial_data()
{
    //  IDSetNumber (&EquatorialCoordsRNP, NULL);


}

/**************************************************************************************
**
***************************************************************************************/
void BAO::slew_error(int slewCode)
{
    OnCoordSetSP.s = IPS_IDLE;

    if (slewCode == 1)
        IDSetSwitch (&OnCoordSetSP, "Object below horizon.");
    else if (slewCode == 2)
        IDSetSwitch (&OnCoordSetSP, "Object below the minimum elevation limit.");
    else
        IDSetSwitch (&OnCoordSetSP, "Slew failed.");
}

/**************************************************************************************
**
***************************************************************************************/
void BAO::enable_simulation(bool enable)
{
    simulation = enable;

    if (simulation)
        IDLog("Warning: Simulation is activated.\n");
    else
        IDLog("Simulation is disabled.\n");
}

/**************************************************************************************
**
***************************************************************************************/
void BAO::connection_lost()
{
    ConnectSP.s = IPS_IDLE;
    IDSetSwitch(&ConnectSP, "The connection to the telescope is lost.");
    return;
}

/**************************************************************************************
**
***************************************************************************************/
void BAO::connection_resumed()
{
    ConnectS[0].s = ISS_ON;
    ConnectS[1].s = ISS_OFF;
    ConnectSP.s = IPS_OK;

    IDSetSwitch(&ConnectSP, "The connection to the telescope has been resumed.");
}


/**************************************************************************************
**  Envoi d'une commande sur le socket puis attente de l'acknowledge
***************************************************************************************/

bool BAO::COMMANDE(int numsocket, char* Commande, char* Params)
{
    char chaine[MAXCARACTERES];

    try
    {
        sprintf(chaine, "%s%s\n", Commande, Params);

        Sockets[numsocket].new_sock << chaine;
    }
    catch (SocketException& e)
    {
        Sockets[numsocket].new_sock.shutdown();
        Sockets[numsocket].new_sock.create();
        Sockets[numsocket].Connected = Sockets[numsocket].new_sock.connect((std::string)Sockets[numsocket].IP);

        if (Sockets[numsocket].Connected)
        {
            Sockets[numsocket].AttenteExecution=0;
            Sockets[numsocket].AnomaliesExecution=0;
        }

        //  if (AntennesConnectees() == 0) { InitAntennes(); InitThreadOK=false;}

        std::string oss;
        oss="COMMANDE exception : " + e.description() + "\n";
        size_t size = oss.size() + 1;
        char* buffer = new char[size];
        strncpy(buffer, oss.c_str(), size);

        IDLog(buffer);

        delete [] buffer;

        return false;
    }

    return true;
}


/**************************************************************************************
** Commande POSITION
***************************************************************************************/

bool BAO::POSITION(int numsocket)
{
    return COMMANDE(numsocket, (char*)"P", (char*)"");
}

/**************************************************************************************
** Commande PARK
***************************************************************************************/

bool BAO::PARK(int numsocket)
{
    return COMMANDE(numsocket, (char*)"Z", (char*)"");
}

/**************************************************************************************
** Commande ABORT
***************************************************************************************/

bool BAO::ABORT(int numsocket)
{
    return COMMANDE(numsocket, (char*)"A", (char*)"");
}


/**************************************************************************************
** Commande GOTO
***************************************************************************************/

bool BAO::GOTO(int numsocket, int deltaAz, int deltaAlt)
{
    char Params[MAXCARACTERES];
    char sensAz;
    char sensAlt;

    sensAlt='b';
    sensAz='b';

    if (deltaAz<0)
    {
        deltaAz=-deltaAz;
        sensAz='f';
    }

    if (deltaAlt<0)
    {
        deltaAlt=-deltaAlt;
        sensAlt='f';
    }

    sprintf(Params, "%c%04i%c%04i", sensAz, deltaAz, sensAlt, deltaAlt);

    return COMMANDE(numsocket, (char*)"G", Params);
}