/*
    TeenAstro Focuser
    Copyright (C) 2021 Markus Noga

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

*/

#include "teenastro.h"
#include "indicom.h"
#include "indicontroller.h"

#include <unistd.h> // for sleep()
#include <termios.h> // for tcflush

// Default, minimal and maximal values for focuser configuration properties
// In absolute units (not device units, where e.g. current is /10 and microsteps are log_2)
//

#define TAF_curr_default  500
#define TAF_curr_min 100
#define TAF_curr_max 1600
#define TAF_micro_default  (1<<4)
#define TAF_micro_min (1<<2)
#define TAF_micro_max (1<<7)
#define TAF_steprot_default  200
#define TAF_steprot_min 10
#define TAF_steprot_max 800
#define TAF_pos_default 0
#define TAF_pos_min 0
#define TAF_pos_max 2000000000UL
#define TAF_speed_default 20
#define TAF_speed_min 1
#define TAF_speed_max 999
#define TAF_acc_default 30
#define TAF_acc_min 1
#define TAF_acc_max 99
#define TAF_res_default 16
#define TAF_res_min 1
#define TAF_res_max 512

#define TAF_UI_STEPS    100.0
#define TAF_STEP(min,max)  (((max)-(min))/(TAF_UI_STEPS))


#define TAF_FOCUSER_TIMEOUT 2 
#define TAF_FOCUSER_BUFSIZE 128

// Focuser singleton
std::unique_ptr<TeenAstroFocuser> teenAstroFocuser(new TeenAstroFocuser());

// Public methods
//

TeenAstroFocuser::TeenAstroFocuser()
{
    FI::SetCapability(FOCUSER_CAN_ABS_MOVE | FOCUSER_CAN_REL_MOVE | FOCUSER_CAN_ABORT | 
    	              FOCUSER_CAN_REVERSE  | FOCUSER_CAN_SYNC     | FOCUSER_HAS_VARIABLE_SPEED);
}

TeenAstroFocuser::~TeenAstroFocuser()
{
}

const char * TeenAstroFocuser::getDefaultName()
{
    return "TeenAstroFocuser";
}

bool TeenAstroFocuser::Handshake()
{
    char resp[TAF_FOCUSER_BUFSIZE];

    // Issue handshake challenge for TeenAstro focuser
    if(!sendAndReceive(":FV#", resp, TAF_FOCUSER_BUFSIZE))
        return false;
    if (strncmp(resp, "$ TeenAstro Focuser ",20))
    {
        DEBUGF(INDI::Logger::DBG_ERROR, "Handshake response: %s", resp);
        return false;
    }

    // TeenAstro mounts internally forward focuser commands via a 
    // serial connection. When mount & focuser are both on USB,
    // and scanning for devices, a race condition can occur. The
    // focuser device driver may probe the USB connection of the 
    // mount first, and receive a correct response to the focuser
    // handshake challenge. Then the focuser driver would block 
    // the USB port of the mount. And the scan performed by 
    // mount driver would fail. To avoid this race condition,
    // we issue the handshake challenge for a LX200 mount and 
    // abort if it is answered. 
    if(!sendAndExpectTimeout(":GR#", resp, TAF_FOCUSER_BUFSIZE))
    {
        DEBUGF(INDI::Logger::DBG_DEBUG, "Device responded to focuser and mount handshake (%s), skipping.", resp);
        return false;
    }

    DEBUGF(INDI::Logger::DBG_DEBUG, "TeenAstroFocuser %s found", &resp[20]);
    return true;
}

bool TeenAstroFocuser::initProperties()
{
    INDI::Focuser::initProperties();
 
	strcpy(FocusSyncN[0].label,  "Ticks"); // Rename unit of measure to ticks, to avoid confusion with motor steps,
	strcpy(FocusMaxPosN[0].label,"Ticks"); // motor microsteps and the effect of the resolution setting.
	strcpy(FocusRelPosN[0].label,"Ticks"); // Ticks are resolution units.
	strcpy(FocusAbsPosN[0].label,"Ticks");

	strcpy(FocusReverseSP.group, FOCUS_TAB);

	strcpy(FocusSpeedNP.label, "Go-to speed");
	strcpy(FocusSpeedNP.group, FOCUS_TAB);
	strcpy(FocusSpeedN[0].label,"Steps/s");
	FocusSpeedN[0].min=TAF_speed_min;	
	FocusSpeedN[0].max=TAF_speed_max;
	FocusSpeedN[0].step=TAF_STEP(TAF_speed_min, TAF_speed_max);

    // Main control tab
    //

    IUFillNumber(&CfgParkPosN[0], "VAL", "Ticks", "%.f", 0, 100000, 1000, 0);
    IUFillNumberVector(&CfgParkPosNP, CfgParkPosN, 1, getDeviceName(), "PARK_POS", "Park position", MAIN_CONTROL_TAB, IP_RW, 60, IPS_IDLE );

    IUFillSwitch(&GoToParkS[0], "VAL", "Park", ISS_OFF);
    IUFillSwitchVector(&GoToParkSP, GoToParkS, 1, getDeviceName(), "GOTO_PARK", "Go-to park", MAIN_CONTROL_TAB, IP_RW, ISR_ATMOST1, 60, IPS_IDLE );

    IUFillNumber(&CurSpeedN[0], "VAL", "Steps/s", "%.f", TAF_speed_min, TAF_speed_max, TAF_STEP(TAF_speed_min, TAF_speed_max), TAF_speed_default);
    IUFillNumberVector(&CurSpeedNP, CurSpeedN, 1, getDeviceName(), "CUR_SPEED", "Current Speed", MAIN_CONTROL_TAB, IP_RO, 0, IPS_IDLE );

    IUFillNumber(&TempN[0], "VAL", "Celsius", "%+.1f°", -50., 50., 0, 0);
    IUFillNumberVector(&TempNP, TempN, 1, getDeviceName(), "TEMP", "Temperature", MAIN_CONTROL_TAB, IP_RO, 0, IPS_IDLE );

    // Focuser tab
    //

    IUFillText(&DeviceVersionT[0], "VAL", "Version", "unknown");
    IUFillTextVector(&DeviceVersionTP, DeviceVersionT, 1, getDeviceName(), "DEVICE_VERSION", "Device version", FOCUS_TAB, IP_RO, 60, IPS_IDLE);

    // Focuser tab: Motor configuration
    //

    IUFillNumber(&CfgMotorStepsPerRevolutionN[0], "VAL", "Steps", "%.f", TAF_steprot_min, TAF_steprot_max, TAF_STEP(TAF_steprot_min, TAF_steprot_max), TAF_steprot_default);
    IUFillNumberVector(&CfgMotorStepsPerRevolutionNP, CfgMotorStepsPerRevolutionN, 1, getDeviceName(), "MOT_STEPS_REV", "Steps/rev.", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    IUFillSwitch(&CfgMotorMicrostepsS[TAF_MICROS_4],   "4",   "4",   ISS_OFF);
    IUFillSwitch(&CfgMotorMicrostepsS[TAF_MICROS_8],   "8",   "8",   ISS_OFF);
    IUFillSwitch(&CfgMotorMicrostepsS[TAF_MICROS_16],  "16",  "16",  ISS_OFF);
    IUFillSwitch(&CfgMotorMicrostepsS[TAF_MICROS_32],  "32",  "32",  ISS_OFF);
    IUFillSwitch(&CfgMotorMicrostepsS[TAF_MICROS_64],  "64",  "64",  ISS_OFF);
    IUFillSwitch(&CfgMotorMicrostepsS[TAF_MICROS_128], "128", "128", ISS_OFF);
    IUFillSwitchVector(&CfgMotorMicrostepsSP, CfgMotorMicrostepsS, TAF_MICROS_N, getDeviceName(), "MOT_USTEPS", "Microsteps", FOCUS_TAB, IP_RW, ISR_ATMOST1, 60, IPS_IDLE );

    IUFillNumber(&CfgMotorResolutionN[0], "VAL", "Microsteps/tick", "%.f", TAF_res_min, TAF_res_max, TAF_STEP(TAF_res_min, TAF_res_max), TAF_res_default);
    IUFillNumberVector(&CfgMotorResolutionNP, CfgMotorResolutionN, 1, getDeviceName(), "MOT_RES", "Resolution", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    IUFillNumber(&CfgMotorCurrentN[0], "VAL", "mA", "%.f", TAF_curr_min, TAF_curr_max, TAF_STEP(TAF_curr_min, TAF_curr_max), TAF_curr_default);
    IUFillNumberVector(&CfgMotorCurrentNP, CfgMotorCurrentN, 1, getDeviceName(), "MOT_CUR", "Motor current", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    // Focuser tab: Motion configuration
    //

    IUFillNumber(&CfgGoToAccN[0], "VAL", "Steps/s^2", "%.f", TAF_acc_min, TAF_acc_max, TAF_STEP(TAF_acc_min, TAF_acc_max), TAF_acc_default);
    IUFillNumberVector(&CfgGoToAccNP, CfgGoToAccN, 1, getDeviceName(), "GOTO_ACCEL", "Go-to accel.", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    IUFillNumber(&CfgManualSpeedN[0], "VAL", "Steps/s", "%.f", TAF_speed_min, TAF_speed_max, TAF_STEP(TAF_speed_min, TAF_speed_max), TAF_speed_default);
    IUFillNumberVector(&CfgManualSpeedNP, CfgManualSpeedN, 1, getDeviceName(), "MAN_SPEED", "Manual speed", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    IUFillNumber(&CfgManualAccN[0], "VAL", "Steps/s^2", "%.f", TAF_acc_min, TAF_acc_max, TAF_STEP(TAF_acc_min, TAF_acc_max), TAF_acc_default);
    IUFillNumberVector(&CfgManualAccNP, CfgManualAccN, 1, getDeviceName(), "MAN_ACCEL", "Manual accel.", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    IUFillNumber(&CfgManualDecN[0], "VAL", "Steps/s^2", "%.f", TAF_acc_min, TAF_acc_max, TAF_STEP(TAF_acc_min, TAF_acc_max), TAF_acc_default);
    IUFillNumberVector(&CfgManualDecNP, CfgManualDecN, 1, getDeviceName(), "MAN_DECEL", "Manual decel.", FOCUS_TAB, IP_RW, 60, IPS_IDLE );

    // Focuser tab: Device actions
    //

    IUFillSwitch(&RebootDeviceS[0], "VAL", "Reboot", ISS_OFF);
    IUFillSwitchVector(&RebootDeviceSP, RebootDeviceS, 1, getDeviceName(), "REBOOT_DEVICE", "Reboot device", FOCUS_TAB, IP_RW, ISR_ATMOST1, 60, IPS_IDLE );

    IUFillSwitch(&EraseEEPROMS[0], "VAL", "Erase", ISS_OFF);
    IUFillSwitchVector(&EraseEEPROMSP, EraseEEPROMS, 1, getDeviceName(), "ERASE_EEPROM", "Erase EEPROM", FOCUS_TAB, IP_RW, ISR_ATMOST1, 60, IPS_IDLE );

    addDebugControl();

    return true;
}

void TeenAstroFocuser::initPositionPropertiesRanges(uint32_t maxPos)
{
    FocusAbsPosN[0].max  = FocusRelPosN[0].max  = FocusSyncN[0].max  = CfgParkPosN[0].max  = maxPos;
    FocusAbsPosN[0].step = FocusRelPosN[0].step = FocusSyncN[0].step = CfgParkPosN[0].step = maxPos / TAF_UI_STEPS;
}

bool TeenAstroFocuser::updateProperties()
{
    if(!INDI::Focuser::updateProperties())
        return false;

    if (isConnected())
    {
        // Update values from device before defining UI controls. 
        // This minimizes flicker as changes to FocuserMaxPosNP
        // must redraw all positional controls to update their range.
        if(!(updateDeviceVersion() && updateMotorConfig() && updateMotionConfig() && updateState()))
            return false;

    	defineMainControlProperties();
    	defineOtherProperties();

        DEBUG(INDI::Logger::DBG_DEBUG, "TeenAstroFocuser ready for use.");
    }
    else
    {
    	deleteMainControlProperties();
    	deleteOtherProperties();
    }

    return true;
}

void TeenAstroFocuser::defineMainControlProperties() 
{
	deleteProperty(FocusSyncNP.name);   // remove superclass controls to improve ordering of UI elements
	deleteProperty(FocusMaxPosNP.name);
	deleteProperty(FocusAbortSP.name);
	deleteProperty(FocusRelPosNP.name);
	deleteProperty(FocusAbsPosNP.name);

	defineProperty(&FocusRelPosNP);
	defineProperty(&FocusAbsPosNP);
	defineProperty(&FocusAbortSP);
    defineProperty(&GoToParkSP);
    defineProperty(&CfgParkPosNP);
	defineProperty(&FocusSyncNP); 
	defineProperty(&FocusMaxPosNP);
    defineProperty(&CurSpeedNP);
    defineProperty(&TempNP);
}

void TeenAstroFocuser::deleteMainControlProperties() 
{
    deleteProperty(GoToParkSP.name);
    deleteProperty(CfgParkPosNP.name);
    deleteProperty(CurSpeedNP.name);
    deleteProperty(TempNP.name);
}

void TeenAstroFocuser::defineOtherProperties() 
{
	deleteProperty(FocusReverseSP.name); // Place this on the focuser tab to avoid clutter
    deleteProperty(FocusSpeedNP.name);

    defineProperty(&DeviceVersionTP);

    defineProperty(&FocusReverseSP);
    defineProperty(&CfgMotorStepsPerRevolutionNP);
    defineProperty(&CfgMotorMicrostepsSP);
    defineProperty(&CfgMotorResolutionNP);
    defineProperty(&CfgMotorCurrentNP);

    defineProperty(&FocusSpeedNP);
    defineProperty(&CfgGoToAccNP);
    defineProperty(&CfgManualSpeedNP);
    defineProperty(&CfgManualAccNP);
    defineProperty(&CfgManualDecNP);

    defineProperty(&RebootDeviceSP);
    defineProperty(&EraseEEPROMSP);
}

void TeenAstroFocuser::deleteOtherProperties() 
{
    deleteProperty(DeviceVersionTP.name);

    deleteProperty(CfgMotorStepsPerRevolutionNP.name);
    deleteProperty(CfgMotorMicrostepsSP.name);
    deleteProperty(CfgMotorResolutionNP.name);
    deleteProperty(CfgMotorCurrentNP.name);

    deleteProperty(CfgGoToAccNP.name);
    deleteProperty(CfgManualSpeedNP.name);
    deleteProperty(CfgManualAccNP.name);
    deleteProperty(CfgManualDecNP.name);

    deleteProperty(EraseEEPROMSP.name);
    deleteProperty(EraseEEPROMSP.name);
}

bool TeenAstroFocuser::ISNewSwitch (const char *dev, const char *name, ISState *states, char *names[], int n)
{
    if(dev==NULL || strcmp(dev,getDeviceName()))
	    return INDI::Focuser::ISNewSwitch(dev, name, states, names, n);

	if(!strcmp(name, GoToParkSP.name)) {
		GoToParkSP.s=IPS_BUSY;
		IDSetSwitch(&GoToParkSP, NULL);
		bool res=goToPark();
		GoToParkS[0].s=ISS_OFF;
		GoToParkSP.s= res ? IPS_OK : IPS_ALERT;
		IDSetSwitch(&GoToParkSP, NULL);
		return res;
	}
    else if(!strcmp(name, CfgMotorMicrostepsSP.name))
    {
    	// parse selected number of usteps from states and labels. Labels are "Vxxx" where xxx is a number
        uint32_t usteps=0;
        for(int i=0; i<n; i++)
        	if(states[i]==ISS_ON)
        		usteps=atoi(names[i]);

        bool res=setMotorMicrosteps(usteps);
        if(res)
            IUUpdateSwitch(&CfgMotorMicrostepsSP, states, names, n);
		CfgMotorMicrostepsSP.s= res ? IPS_OK : IPS_ALERT;
		IDSetSwitch(&CfgMotorMicrostepsSP, NULL);
		return res;
    }
	else if(!strcmp(name, RebootDeviceSP.name)) 
	{
		RebootDeviceSP.s=IPS_BUSY;
		IDSetSwitch(&RebootDeviceSP, NULL);
		bool res=rebootDevice();
		RebootDeviceS[0].s=ISS_OFF;
		RebootDeviceSP.s= res ? IPS_OK : IPS_ALERT;
		IDSetSwitch(&RebootDeviceSP, NULL);
		return res;
	} 
	else if(!strcmp(name, EraseEEPROMSP.name)) 
	{
		EraseEEPROMSP.s=IPS_BUSY;
		IDSetSwitch(&EraseEEPROMSP, NULL);
		bool res=eraseDeviceEEPROM();
		EraseEEPROMS[0].s=ISS_OFF;
		EraseEEPROMSP.s= res ? IPS_OK : IPS_ALERT;
		IDSetSwitch(&EraseEEPROMSP, NULL);
		return res;
	} else 
	    return INDI::Focuser::ISNewSwitch(dev, name, states, names, n);
}

bool TeenAstroFocuser::ISNewNumberHelper(INumberVectorProperty *NP, double values[], char *names[], int n, bool res) 
{
	if(res)
		IUUpdateNumber(NP, values, names, n);               
	NP->s=res ? IPS_OK : IPS_ALERT;
	IDSetNumber(NP, NULL);
	return res;
}

bool TeenAstroFocuser::ISNewNumber (const char *dev, const char *name, double values[], char *names[], int n)
{
	if(dev==NULL || strcmp(dev,getDeviceName()))
		return INDI::Focuser::ISNewNumber(dev, name, values, names, n);

	if(!strcmp(name, FocusSyncNP.name)) 
    {
        bool res=SyncFocuser((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&FocusSyncNP, values, names, n, res);
    }
	else if(!strcmp(name, CfgParkPosNP.name)) 
    {
        bool res=setParkPos((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgParkPosNP, values, names, n, res);
    }
 	else if(!strcmp(name, FocusMaxPosNP.name))
    {
    	uint32_t val=(uint32_t) rint(values[0]);
        bool res=SetFocuserMaxPosition(val);
        if(res && FocusMaxPosN[0].value!=val) {
    	    initPositionPropertiesRanges(val);
			deleteMainControlProperties();      // Force redraw of UI controls as IUUpdateMinMax
			defineMainControlProperties();    	// does not reliably update the step size.
        }
        return ISNewNumberHelper(&FocusMaxPosNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgManualSpeedNP.name))
    {
        bool res=setManualSpeed((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgManualSpeedNP, values, names, n, res);
    }
    else if(!strcmp(name, FocusSpeedNP.name))
    {
        bool res=SetFocuserSpeed(rint(values[0]));
        return ISNewNumberHelper(&FocusSpeedNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgGoToAccNP.name))
    {
        bool res=setGoToAcc((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgGoToAccNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgManualAccNP.name))
    {
        bool res=setManualAcc((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgManualAccNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgManualDecNP.name))
    {
        bool res=setManualDec((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgManualDecNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgMotorStepsPerRevolutionNP.name))
    {
        bool res=setMotorStepsPerRevolution((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgMotorStepsPerRevolutionNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgMotorResolutionNP.name))
    {
        bool res=setMotorResolution((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgMotorResolutionNP, values, names, n, res);
    }
    else if(!strcmp(name, CfgMotorCurrentNP.name))
    {
        bool res=setMotorCurrent((uint32_t) rint(values[0]));
        return ISNewNumberHelper(&CfgMotorCurrentNP, values, names, n, res);
    }
    else
        return INDI::Focuser::ISNewNumber(dev, name, values, names, n);
}


IPState TeenAstroFocuser::MoveAbsFocuser(uint32_t pos)
{
    FocusAbsPosNP.s = IPS_BUSY;
    IDSetNumber(&FocusAbsPosNP, NULL);
    FocusRelPosNP.s = IPS_BUSY;
    IDSetNumber(&FocusRelPosNP, NULL);

    if(!goTo(pos)) {
	    FocusAbsPosNP.s = IPS_ALERT;
	    IDSetNumber(&FocusAbsPosNP, NULL);
	    FocusRelPosNP.s = IPS_ALERT;
	    IDSetNumber(&FocusRelPosNP, NULL);

        return IPS_ALERT;
    }
    return IPS_BUSY;
}

IPState TeenAstroFocuser::MoveRelFocuser(FocusDirection dir, uint32_t ticks)
{
    FocusRelPosN[0].value = ticks;

    uint32_t pos=uint32_t(FocusAbsPosN[0].value);
    if (dir == FOCUS_INWARD)
        pos -= ticks;
    else
        pos += ticks;

    return MoveAbsFocuser(pos);
}

void TeenAstroFocuser::TimerHit()
{
	if(!isConnected())
		return;

	updateState();
    SetTimer(getCurrentPollingPeriod());
}

bool TeenAstroFocuser::AbortFocuser()
{
	FocusAbortSP.s=IPS_BUSY;
	IDSetSwitch(&FocusAbortSP, NULL);

    if(!stop()) {
		FocusAbortSP.s=IPS_ALERT;
		IDSetSwitch(&FocusAbortSP, NULL);
        return false;
    }
	FocusAbortSP.s=IPS_OK;
	IDSetSwitch(&FocusAbortSP, NULL);

    return updateState();
}


// Protected methods
//

bool TeenAstroFocuser::send(const char *const msg) {
    DEBUGF(INDI::Logger::DBG_DEBUG, "send(\"%s\")", msg);

    tcflush(PortFD, TCIOFLUSH);

    int nbytes_written=0, rc=-1;
    if ( (rc = tty_write(PortFD, msg, strlen(msg), &nbytes_written)) != TTY_OK)
    {
        char errstr[MAXRBUF];
        tty_error_msg(rc, errstr, MAXRBUF);
        DEBUGF(INDI::Logger::DBG_ERROR, "send(\"%s\"): %s.", msg, errstr);
        return false;
    }
    return true;
}


bool TeenAstroFocuser::sendAndReceive(const char *const msg, char *resp, int bufsize) {
    if(!send(msg))
        return false;

    int nbytes_read=0;
    int rc = tty_nread_section(PortFD, resp, bufsize, '#', TAF_FOCUSER_TIMEOUT, &nbytes_read);
    resp[nbytes_read]='\0';
    if(rc!=TTY_OK || nbytes_read==0 || resp[nbytes_read-1]!='#')
    {
        char errstr[MAXRBUF];
        tty_error_msg(rc, errstr, MAXRBUF);
        DEBUGF(INDI::Logger::DBG_ERROR, "sendAndReceive(\"%s\") received \"%s\": %s.", msg, resp, errstr);
        return false;
    }
    DEBUGF(INDI::Logger::DBG_DEBUG, "sendAndReceive(\"%s\") received \"%s\".", msg, resp);
    return true;
}

bool TeenAstroFocuser::sendAndReceiveBool(const char *const msg) {
    if(!send(msg))
        return false;

    char resp[2];
    int nbytes_read=0;
    int rc = tty_read(PortFD, resp, 1, TAF_FOCUSER_TIMEOUT, &nbytes_read);
    resp[nbytes_read]='\0';
    if(rc!=TTY_OK || resp[0]!='1')
    {
        char errstr[MAXRBUF];
        tty_error_msg(rc, errstr, MAXRBUF);
        DEBUGF(INDI::Logger::DBG_ERROR, "sendAndReceiveBool(\"%s\") received \"%s\": %s.", msg, resp, errstr);
        return false;
    }
    DEBUGF(INDI::Logger::DBG_DEBUG, "sendAndReceiveBool(\"%s\") received \"%s\".", msg, resp);
    return resp[0]=='1';
}


bool TeenAstroFocuser::sendAndExpectTimeout(const char *const msg, char *resp, int bufsize) {
    if(!send(msg))
        return false;

    int nbytes_read=0;
    int rc = tty_nread_section(PortFD, resp, bufsize, '#', TAF_FOCUSER_TIMEOUT, &nbytes_read);
    resp[nbytes_read]='\0';
    if(rc!=TTY_TIME_OUT || nbytes_read!=0)
    {
        char errstr[MAXRBUF];
        tty_error_msg(rc, errstr, MAXRBUF);
        DEBUGF(INDI::Logger::DBG_ERROR, "sendAndExpectTimeout(\"%s\") received \"%s\".", msg, resp);
        return false;
    }
    DEBUGF(INDI::Logger::DBG_DEBUG, "sendAndExpectTimeout(\"%s\") got timeout.", msg, resp);
    return true;
}


bool TeenAstroFocuser::updateDeviceVersion()
{
    char resp[TAF_FOCUSER_BUFSIZE];
    if(!sendAndReceive(":FV#", resp, TAF_FOCUSER_BUFSIZE))
        return false;
    int len=strlen(resp);
    resp[len-1]=0;
    IUSaveText(&DeviceVersionT[0], resp+2);
    DeviceVersionTP.s = IPS_OK;
    IDSetText(&DeviceVersionTP, NULL);
    return true;
}

bool TeenAstroFocuser::updateState()
{
    char resp[TAF_FOCUSER_BUFSIZE];
    int pos=-1, speed=-1;
    float temp=-1;

    if(!sendAndReceive(":F?#", resp, TAF_FOCUSER_BUFSIZE))
        return false;

    if(sscanf(resp, "?%d %d %f#", &pos, &speed, &temp)<=0)
    {
        DEBUGF(INDI::Logger::DBG_ERROR, "Invalid format: focuser state (%s)", resp);
        return false;
    }

    if(FocusAbsPosNP.s==IPS_BUSY && speed==0)
        DEBUG(INDI::Logger::DBG_SESSION, "Focuser reached target position.");

    FocusAbsPosN[0].value = pos;
    FocusAbsPosNP.s = speed>0 ? IPS_BUSY : IPS_OK;
    IDSetNumber(&FocusAbsPosNP, NULL);

	FocusRelPosNP.s = speed>0 ? IPS_BUSY : IPS_OK;
    IDSetNumber(&FocusRelPosNP, NULL);

    CurSpeedN[0].value = speed;
    CurSpeedNP.s = speed>0 ? IPS_BUSY : IPS_OK;
    IDSetNumber(&CurSpeedNP, NULL);

    TempN[0].value = temp;
    TempNP.s = (temp==-99) ? IPS_ALERT : IPS_OK;
    IDSetNumber(&TempNP, NULL);

    return true;
}

bool TeenAstroFocuser::isMoving()
{
    if(!updateState())
        return false;
    return CurSpeedN[0].value > 0;
}


bool TeenAstroFocuser::updateMotionConfig() 
{
    char resp[TAF_FOCUSER_BUFSIZE];
    int parkPos=-1, maxPos=-1, manualSpeed=-1, goToSpeed=-1, gotoAcc=-1, manAcc=-1, manDec=-1;

    if(!sendAndReceive(":F~#", resp, TAF_FOCUSER_BUFSIZE))
        return false;

    if(sscanf(resp, "~%d %d %d %d %d %d %d#", &parkPos, &maxPos, &manualSpeed, &goToSpeed, &gotoAcc, &manAcc, &manDec)<=0)
    {
        DEBUGF(INDI::Logger::DBG_ERROR, "Invalid format: focuser motion config (%s)", resp);
        return false;
    }

    if(maxPos!=FocusMaxPosN[0].value)
	    initPositionPropertiesRanges(maxPos);

    CfgParkPosN[0].value = parkPos;
    CfgParkPosNP.s = IPS_OK;
    IDSetNumber(&CfgParkPosNP, NULL);
    FocusMaxPosN[0].value = maxPos;
    FocusMaxPosNP.s = IPS_OK;
    IDSetNumber(&FocusMaxPosNP, NULL);
    CfgManualSpeedN[0].value = manualSpeed;
    CfgManualSpeedNP.s = IPS_OK;
    IDSetNumber(&CfgManualSpeedNP, NULL);
    FocusSpeedN[0].value = goToSpeed;
    FocusSpeedNP.s = IPS_OK;
    IDSetNumber(&FocusSpeedNP, NULL);
    CfgGoToAccN[0].value = gotoAcc;
    CfgGoToAccNP.s = IPS_OK;
    IDSetNumber(&CfgGoToAccNP, NULL);
    CfgManualAccN[0].value = manAcc;
    CfgManualAccNP.s = IPS_OK;
    IDSetNumber(&CfgManualAccNP, NULL);
    CfgManualDecN[0].value = manDec;
    CfgManualDecNP.s = IPS_OK;
    IDSetNumber(&CfgManualDecNP, NULL);

    return true;
}

bool TeenAstroFocuser::setConfigItem(char item, uint32_t deviceValue)
{
    char cmd[64];
    snprintf(cmd, 64, ":F%c,%d#", item, deviceValue);
    return sendAndReceiveBool(cmd);
}

bool TeenAstroFocuser::setParkPos(uint32_t value)
{
    return setConfigItem('0', value);
}

bool TeenAstroFocuser::SetFocuserMaxPosition(uint32_t value)
{
    return setConfigItem('1', value);
}

bool TeenAstroFocuser::setManualSpeed(uint32_t value)
{
    return setConfigItem('2', value);
}

bool TeenAstroFocuser::SetFocuserSpeed(int value)
{
    return setConfigItem('3', (uint32_t) value);
}

bool TeenAstroFocuser::setGoToAcc(uint32_t value)
{
    return setConfigItem('4', value);
}

bool TeenAstroFocuser::setManualAcc(uint32_t value)
{
    return setConfigItem('5', value);
}

bool TeenAstroFocuser::setManualDec(uint32_t value)
{
    return setConfigItem('6', value);
}


bool TeenAstroFocuser::updateMotorConfig() 
{
    char resp[TAF_FOCUSER_BUFSIZE];
    int reverse=-1, log2_micro=-1, resolution=-1, curr_10ma=-1, steprot=-1;

    if(!sendAndReceive(":FM#", resp, TAF_FOCUSER_BUFSIZE))
        return false;

    if(sscanf(resp, "M%d %d %d %d %d#", &reverse, &log2_micro, &resolution, &curr_10ma, &steprot)<=0)
    {
        DEBUGF(INDI::Logger::DBG_ERROR, "Invalid format: focuser motor config (%s)", resp);
        return false;
    }

    FocusReverseS[0].s=(reverse==0) ? ISS_OFF : ISS_ON;
    FocusReverseS[1].s=(reverse==0) ? ISS_ON  : ISS_OFF;
    FocusReverseSP.s  = IPS_OK;
    IDSetSwitch(&FocusReverseSP, NULL);

    uint32_t micro=(1<<log2_micro); // TeenAstro device returns and expects log_2(microsteps)
    bool microFound=false;
    for(int i=0; i<TAF_MICROS_N; i++)
    {
    	uint32_t thisMicro=(uint32_t) atoi(CfgMotorMicrostepsS[i].name);
    	CfgMotorMicrostepsS[i].s= (micro==thisMicro) ? ISS_ON : ISS_OFF;
    	if(micro==thisMicro)
    		microFound=true;
    }
    CfgMotorMicrostepsSP.s = microFound ? IPS_OK : IPS_ALERT;
    IDSetSwitch(&CfgMotorMicrostepsSP, NULL);

    CfgMotorResolutionN[0].value = resolution;
    CfgMotorResolutionNP.s = IPS_OK;
    IDSetNumber(&CfgMotorResolutionNP, NULL);
    CfgMotorCurrentN[0].value = curr_10ma * 10;    // TeenAstro device returns and expects units of 10 mA
    CfgMotorCurrentNP.s = IPS_OK;
    IDSetNumber(&CfgMotorCurrentNP, NULL);
    CfgMotorStepsPerRevolutionN[0].value = steprot;
    CfgMotorStepsPerRevolutionNP.s = IPS_OK;
    IDSetNumber(&CfgMotorStepsPerRevolutionNP, NULL);

    return true;
}

bool TeenAstroFocuser::ReverseFocuser(bool enable)
{
    return setConfigItem('7', enable ? 1 : 0);
}

bool TeenAstroFocuser::setMotorMicrosteps(uint32_t value)
{
    uint32_t bitPos=0;
    value>>=1;
    for(; value!=0; bitPos++)
        value>>=1;
    return setConfigItem('m', bitPos);      // TeenAstro device returns and expects log_2(microsteps)
}

bool TeenAstroFocuser::setMotorResolution(uint32_t value)
{
    return setConfigItem('8', value);
}

bool TeenAstroFocuser::setMotorCurrent(uint32_t value)
{
    return setConfigItem('c', value / 10);  // TeenAstro device returns and expects units of 10 mA
}

bool TeenAstroFocuser::setMotorStepsPerRevolution(uint32_t value)
{
    return setConfigItem('r', value);
}

bool TeenAstroFocuser::SyncFocuser(uint32_t value)
{
    char cmd[64];
    snprintf(cmd, 64, ":FS,%d#", value);
    return send(cmd); // no confirmation via "0" or "1"
}


bool TeenAstroFocuser::goTo(uint32_t position)
{
    char cmd[64];
    snprintf(cmd, 64, ":FG,%d#", position);
    return send(cmd);
}

bool TeenAstroFocuser::goToPark()
{
    return send(":FP#");  
}

bool TeenAstroFocuser::stop()
{
    return send(":FQ#");
}

bool TeenAstroFocuser::rebootDevice()
{
    if(!send(":F!#"))
    	return false;
    sleep(3);
    return updateDeviceVersion() && updateMotorConfig() && updateMotionConfig() && updateState();
}

bool TeenAstroFocuser::eraseDeviceEEPROM()
{
    if(!send(":F$#"))
    	return false;
    sleep(1);
    return updateDeviceVersion() && updateMotorConfig() && updateMotionConfig() && updateState();
}

bool TeenAstroFocuser::saveConfigItems(FILE *fp)
{
    // This is a verbatim copy of Focuser::saveConfigItems except one override
    //

    DefaultDevice::saveConfigItems(fp);

    // Do not save focuser configuration items on INDI host, as they are stored on the focuser device.
    // FI::saveConfigItems(fp);

    IUSaveConfigNumber(fp, &PresetNP);
    this->controller->saveConfigItems(fp);

    return true;
}
