/*******************************************************************************
  Copyright(c) 2023 Chrysikos Efstathios. All rights reserved.

  Pegasus FocusCube

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

  This program is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  more details.

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

  The full GNU General Public License is included in this distribution in the
  file called LICENSE.
*******************************************************************************/
#include "pegasus_focuscube3.h"

#include "indicom.h"
#include "connectionplugins/connectionserial.h"

#include <cmath>
#include <cstring>
#include <memory>
#include <regex>
#include <termios.h>
#include <unistd.h>

#define DMFC_TIMEOUT 3
#define FOCUS_SETTINGS_TAB "Settings"
#define TEMPERATURE_THRESHOLD 0.1

static std::unique_ptr<PegasusFocusCube3> focusCube(new PegasusFocusCube3());


PegasusFocusCube3::PegasusFocusCube3()
{
    setVersion(1, 0);
    // Can move in Absolute & Relative motions, can AbortFocuser motion.
    FI::SetCapability(FOCUSER_CAN_ABS_MOVE |
                      FOCUSER_CAN_REL_MOVE |
                      FOCUSER_CAN_ABORT |
                      FOCUSER_CAN_REVERSE |
                      FOCUSER_CAN_SYNC |
                      FOCUSER_HAS_BACKLASH);
}


bool PegasusFocusCube3::Handshake()
{
    int tty_rc = 0, nbytes_written = 0, nbytes_read = 0;
    char command[PEGASUS_LEN] = {0}, response[PEGASUS_LEN] = {0};
    PortFD = serialConnection->getPortFD();
    LOG_DEBUG("CMD <##>");




    tcflush(PortFD, TCIOFLUSH);
    strncpy(command, "##\r\n", PEGASUS_LEN);
    if ( (tty_rc = tty_write_string(PortFD, command, &nbytes_written)) != TTY_OK)
    {
        char errorMessage[MAXRBUF];
        tty_error_msg(tty_rc, errorMessage, MAXRBUF);
        LOGF_ERROR("Serial write error: %s", errorMessage);
        return false;
    }

    // Try first with stopChar as the stop character
    if ( (tty_rc = tty_nread_section(PortFD, response, PEGASUS_LEN, stopChar, 1, &nbytes_read)) != TTY_OK)
    {
        // Try 0xA as the stop character
        if (tty_rc == TTY_OVERFLOW || tty_rc == TTY_TIME_OUT)
        {
            tcflush(PortFD, TCIOFLUSH);
            tty_write_string(PortFD, command, &nbytes_written);
            stopChar = 0xA;
            tty_rc = tty_nread_section(PortFD, response, PEGASUS_LEN, stopChar, 1, &nbytes_read);
        }

        if (tty_rc != TTY_OK)
        {
            char errorMessage[MAXRBUF];
            tty_error_msg(tty_rc, errorMessage, MAXRBUF);
            LOGF_ERROR("Serial read error: %s", errorMessage);
            return false;
        }
    }

    tcflush(PortFD, TCIOFLUSH);
    response[nbytes_read - 1] = '\0';
    LOGF_DEBUG("RES <%s>", response);

    setupComplete = false;

    return (strstr(response, "FC3") != nullptr);
}

const char *PegasusFocusCube3::getDefaultName()
{
    return "Pegasus FocusCube3";
}

bool PegasusFocusCube3::initProperties()
{
    INDI::Focuser::initProperties();

    TemperatureNP[0].fill("TEMP", "Level", "%.0f", -40, 40, 1, 0);
    TemperatureNP.fill(getDeviceName(), "TEMP", "Temperature", FOCUS_SETTINGS_TAB, IP_RW, 60, IPS_IDLE);

    FirmwareVersionTP[0].fill("Version", "Version", "");
    FirmwareVersionTP.fill(getDeviceName(), "Firmware", "Firmware", MAIN_CONTROL_TAB, IP_RO, 0, IPS_IDLE);

    SpeedNP[0].fill("Speed", "Value", "%6.2f", 100, 1000., 100., 400.);
    SpeedNP.fill(getDeviceName(), "MaxSpeed", "", FOCUS_SETTINGS_TAB, IP_RW, 0, IPS_IDLE);

    // Relative and absolute movement
    FocusRelPosN[0].min   = 0.;
    FocusRelPosN[0].max   = 50000.;
    FocusRelPosN[0].value = 0;
    FocusRelPosN[0].step  = 1000;

    FocusAbsPosN[0].min   = 0.;
    FocusAbsPosN[0].value = 0;
    FocusAbsPosN[0].step  = 1000;

    // Backlash compensation
    FocusBacklashN[0].min   = 1; // 0 is off.
    FocusBacklashN[0].max   = 1000;
    FocusBacklashN[0].value = 1;
    FocusBacklashN[0].step  = 1;

    FocusMaxPosN[0].max = 1317500;
    FocusMaxPosN[0].value = 1317500;
    FocusAbsPosN[0].max = 1317500;

    addDebugControl();
    setDefaultPollingPeriod(200);
    serialConnection->setDefaultBaudRate(Connection::Serial::B_115200);

    return true;
}

bool PegasusFocusCube3::updateProperties()
{
    INDI::Focuser::updateProperties();

    if (isConnected())
    {
        defineProperty(TemperatureNP);

        defineProperty(FirmwareVersionTP);
        std::string firmware = getFirmwareVersion();
        FirmwareVersionTP[0].setText(firmware);
        FirmwareVersionTP.setState(IPS_OK);
        FirmwareVersionTP.apply();
        defineProperty(SpeedNP);


    }
    else
    {
        deleteProperty(TemperatureNP);
        deleteProperty(FirmwareVersionTP);
        deleteProperty(SpeedNP);
    }

    return true;
}


bool PegasusFocusCube3::ISNewNumber(const char * dev, const char * name, double values[], char * names[], int n)
{
    if (dev != nullptr && strcmp(dev, getDeviceName()))
    {

        if(SpeedNP.isNameMatch(name))
        {
            SpeedNP.update(values, names, n);
            IPState result = IPS_OK;
            if (isConnected())
            {
                if(!setSpeed(values[0]))
                {
                    result = IPS_ALERT;
                }
            }
            SpeedNP.setState(result);
            SpeedNP.apply();
        }

    }

    return INDI::Focuser::ISNewNumber(dev, name, values, names, n);
}

IPState PegasusFocusCube3::MoveAbsFocuser(uint32_t targetTicks)
{
    targetPosition = targetTicks;

    bool rc = move(targetPosition);

    if (!rc)
        return IPS_ALERT;

    FocusAbsPosNP.s = IPS_BUSY;

    return IPS_BUSY;
}

IPState PegasusFocusCube3::MoveRelFocuser(FocusDirection dir, uint32_t ticks)
{
    double newPosition = 0;
    bool rc = false;

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

    rc = move(newPosition);

    if (!rc)
        return IPS_ALERT;

    FocusRelPosN[0].value = ticks;
    FocusRelPosNP.s       = IPS_BUSY;

    return IPS_BUSY;
}

bool PegasusFocusCube3::move(uint32_t newPosition)
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0}, expectedResponse[PEGASUS_LEN] = {0};
    snprintf(expectedResponse, PEGASUS_LEN, "%u", newPosition);
    snprintf(cmd, PEGASUS_LEN, "FM:%d", newPosition);
    if(sendCommand(cmd, res))
    {
        if(!strstr(expectedResponse, res))
        {
            LOGF_ERROR("Error on move [Cmd=%s Res=%s]", cmd, res);
            return false;
        }
    }
    else
    {
        LOGF_ERROR("Error on move [Position=%d]", newPosition);
        return false;
    }
    return true;
}


bool PegasusFocusCube3::sendCommand(const char * cmd, char * res)
{
    int nbytes_read = 0, nbytes_written = 0, tty_rc = 0;
    char command[PEGASUS_LEN] = {0};
    LOGF_DEBUG("CMD <%s>", cmd);

    for (int i = 0; i < 2; i++)
    {
        tcflush(PortFD, TCIOFLUSH);
        snprintf(command, PEGASUS_LEN, "%s\n", cmd);
        if ( (tty_rc = tty_write_string(PortFD, command, &nbytes_written)) != TTY_OK)
            continue;

        if (!res)
        {
            tcflush(PortFD, TCIOFLUSH);
            return true;
        }

        if ( (tty_rc = tty_nread_section(PortFD, res, PEGASUS_LEN, stopChar, PEGASUS_TIMEOUT, &nbytes_read)) != TTY_OK
                || nbytes_read == 1)
            continue;

        tcflush(PortFD, TCIOFLUSH);
        res[nbytes_read - 1] = '\0';
        LOGF_DEBUG("RES <%s>", res);
        return true;
    }

    if (tty_rc != TTY_OK)
    {
        char errorMessage[MAXRBUF];
        tty_error_msg(tty_rc, errorMessage, MAXRBUF);
        LOGF_ERROR("Serial error: %s", errorMessage);
    }

    return false;
}

bool PegasusFocusCube3::setSpeed(uint16_t speed)
{

    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0}, expectedResponse[PEGASUS_LEN] = {0};
    snprintf(expectedResponse, PEGASUS_LEN, "%u", speed);
    snprintf(cmd, PEGASUS_LEN, "SP:%d", speed);
    if(sendCommand(expectedResponse, res))
    {
        if(!strstr(cmd, res))
        {
            LOGF_ERROR("Error on set speed [Cmd=%s Res=%s]", cmd, res);
            return false;
        }
    }
    else
    {
        LOGF_ERROR("Error on set speed [Speed=%d]", speed);
        return false;
    }
    return true;
}

std::string  PegasusFocusCube3::getFirmwareVersion()
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0};
    snprintf(cmd, PEGASUS_LEN, "FV");
    if(sendCommand(cmd, res))
    {

    }
    else
    {
        LOG_ERROR("Error on get Firmware");
    }
    return res;
}

std::vector<std::string> PegasusFocusCube3::split(const std::string &input, const std::string &regex)
{
    // passing -1 as the submatch index parameter performs splitting
    std::regex re(regex);
    std::sregex_token_iterator
    first{input.begin(), input.end(), re, -1},
          last;
    return {first, last};
}

bool PegasusFocusCube3::AbortFocuser()
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0};
    snprintf(cmd, PEGASUS_LEN, "FH");
    if(sendCommand(cmd, res))
    {

    }
    else
    {
        LOG_ERROR("Error on abort");
        return false;
    }


    FocusAbsPosNP.s = IPS_IDLE;
    FocusRelPosNP.s = IPS_IDLE;
    IDSetNumber(&FocusAbsPosNP, nullptr);
    IDSetNumber(&FocusRelPosNP, nullptr);
    return true;
}

bool PegasusFocusCube3::SyncFocuser(uint32_t ticks)
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0}, expectedResponse[PEGASUS_LEN] = {0};
    snprintf(expectedResponse, PEGASUS_LEN, "%u", ticks);
    snprintf(cmd, PEGASUS_LEN, "FN:%ud", ticks);
    if(sendCommand(cmd, res))
    {
        if(!strstr(expectedResponse, res))
        {
            LOGF_ERROR("Error on sync  [Cmd=%s Res=%s]", cmd, res);
            return false;
        }
    }
    else
    {
        LOG_ERROR("Error on sync");
        return false;
    }

    return true;
}

bool PegasusFocusCube3::ReverseFocuser(bool enabled)
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0};
    snprintf(cmd, PEGASUS_LEN, "FD:%d", enabled ? 1 : 0);
    if(sendCommand(cmd, res))
    {

    }
    else
    {
        LOG_ERROR("Error on sync");
        return false;
    }

    return true;
}

bool PegasusFocusCube3::SetFocuserBacklash(int32_t steps)
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0}, expectedResponse[PEGASUS_LEN] = {0};
    snprintf(expectedResponse, PEGASUS_LEN, "%u", steps);
    snprintf(cmd, PEGASUS_LEN, "BL:%d", steps);
    if(sendCommand(cmd, res))
    {
        if(!strstr(expectedResponse, res))
        {
            LOGF_ERROR("Error on sync  [Cmd=%s Res=%s]", cmd, res);
            return false;
        }
    }
    else
    {
        LOG_ERROR("Error on sync");
        return false;
    }

    return true;
}

void PegasusFocusCube3::TimerHit()
{
    if (!isConnected())
    {
        SetTimer(getCurrentPollingPeriod());
        return;
    }

    bool rc = updateFocusParams();

    if (rc)
    {
        if (FocusAbsPosNP.s == IPS_BUSY || FocusRelPosNP.s == IPS_BUSY)
        {
            if (isMoving == false)
            {
                FocusAbsPosNP.s = IPS_OK;
                FocusRelPosNP.s = IPS_OK;
                IDSetNumber(&FocusAbsPosNP, nullptr);
                IDSetNumber(&FocusRelPosNP, nullptr);
                LOG_INFO("Focuser reached requested position.");
            }
        }
    }

    SetTimer(getCurrentPollingPeriod());
}

bool PegasusFocusCube3::updateFocusParams()
{
    char cmd[PEGASUS_LEN] = {0}, res[PEGASUS_LEN] = {0};
    snprintf(cmd, PEGASUS_LEN, "FA");
    if(sendCommand(cmd, res))
    {

    }
    else
    {
        LOGF_ERROR("Error on [Cmd=%s Res=%s]", cmd, res);
        return false;
    }

    std::vector<std::string> result = split(res, ":");

    // Status
    if(result[0] != "FC3")
        return false;


    // #1 Position
    currentPosition = atoi(result[1].c_str());
    if (currentPosition != FocusAbsPosN[0].value)
    {
        FocusAbsPosN[0].value = currentPosition;
        IDSetNumber(&FocusAbsPosNP, nullptr);
    }

    // #2 Moving Status
    isMoving = (result[2] == "1");


    // #3 Temperature
    TemperatureNP[0].setValue(atof(result[3].c_str()));
    TemperatureNP.setState(IPS_OK);
    TemperatureNP.apply();


    // #4 Reverse Status
    int reverseStatus = atoi(result[4].c_str());
    if (reverseStatus >= 0 && reverseStatus <= 1)
    {
        IUResetSwitch(&FocusReverseSP);
        FocusReverseS[INDI_ENABLED].s = (reverseStatus == 1) ? ISS_ON : ISS_OFF;
        FocusReverseS[INDI_DISABLED].s = (reverseStatus == 0) ? ISS_ON : ISS_OFF;
        FocusReverseSP.s = IPS_OK;
        IDSetSwitch(&FocusReverseSP, nullptr);
    }


    // #5 Backlash

    int backlash = atoi(result[5].c_str());
    // If backlash is zero then compensation is disabled
    if (backlash == 0 && FocusBacklashS[INDI_ENABLED].s == ISS_ON)
    {
        LOG_WARN("Backlash value is zero, disabling backlash switch...");

        FocusBacklashS[INDI_ENABLED].s = ISS_OFF;
        FocusBacklashS[INDI_DISABLED].s = ISS_ON;
        FocusBacklashSP.s = IPS_IDLE;
        IDSetSwitch(&FocusBacklashSP, nullptr);
    }
    else if (backlash > 0 && (FocusBacklashS[INDI_DISABLED].s == ISS_ON || backlash != FocusBacklashN[0].value))
    {
        if (backlash != FocusBacklashN[0].value)
        {
            FocusBacklashN[0].value = backlash;
            FocusBacklashNP.s = IPS_OK;
            IDSetNumber(&FocusBacklashNP, nullptr);
        }

        if (FocusBacklashS[INDI_DISABLED].s == ISS_ON)
        {
            FocusBacklashS[INDI_ENABLED].s = ISS_OFF;
            FocusBacklashS[INDI_DISABLED].s = ISS_ON;
            FocusBacklashSP.s = IPS_IDLE;
            IDSetSwitch(&FocusBacklashSP, nullptr);
        }
    }

    return true;
}
