/** \file
 *
 *  Contains the MultiRay class implementation.
 *
 *  Copyright (c) 2022, MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */

#include "MultiRay.h"
#include "MultiRayIF.h"

#include <cstdlib>
#include <string>       // std::string
#include <iostream>     // std::cout
#include <sstream>      // std::stringstream

#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "sensorModule/OnboardIF.h"

const char ACK[4] = {'\r', '\n', 0, 0};
const char NAK[4] = {'?', '\r', '\n', 0};

MultiRay::MultiRay( const Module* module )
    : SyncSensorComponent( MultiRayIF::NAME, module ),
      debug_( false ),
      uart_( MultiRayIF::UART, MultiRayIF::BAUD, 0.010, logger_, MULTIRAY_UART_BUFFSIZE ),
      uart2_( MultiRayIF::UART2, MultiRayIF::BAUD, 0.010, logger_, MULTIRAY_UART_BUFFSIZE ),
      loadControl_( MultiRayIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      loadControl2_( MultiRayIF::LOAD_CONTROL2, !simulateHardware(), logger_, this ),
      startTime_( Timestamp::NOT_SET_TIME ),
      powerOnTimeout_( 1.0 ),
      timeout_( 45.0 ),
      lightMode_( LIGHT_OFF ),
      changeRequested_( false ),
      readNewLightMode_( false ),
      waitBeforeAck_( false ),
      changeModeState_( 0 )
{
    setRunState( START );

    // readers
    brightnessWhiteCfgReader_ = newConfigReader( MultiRayIF::BRIGHTNESS_WHITE );
    brightnessRedCfgReader_ = newConfigReader( MultiRayIF::BRIGHTNESS_RED );
    lightModeReader_ = newDataReader( MultiRayIF::LIGHT_MODE_COMMAND );
    lightModeWriter_ = newDataWriter( MultiRayIF::LIGHT_MODE_LOG );
}

MultiRay::~MultiRay()
{
}

bool MultiRay::isDataRequested()
{
    return lightModeWriter_->isDataRequested();
}

bool MultiRay::readConfig()
{
    bool ok = true;
    ok &= brightnessWhiteCfgReader_->read( Units::NONE_INT, brightnessWhite_ );
    ok &= brightnessRedCfgReader_->read( Units::NONE_INT, brightnessRed_ );
    return ok;
}

int MultiRay::getAcknowledge( UartStream &uart )
{
    char read_buf[4];
    memset( &read_buf, '\0', sizeof( read_buf ) );
    uart.read( read_buf, uart.dataAvailable() );
    if( debug_ ) logger_.syslog( "From uart, got: ", read_buf, Syslog::IMPORTANT );
    int got_ack = !strcmp( read_buf, ACK );
    int got_nak = !strcmp( read_buf, NAK );
    if( got_ack )
    {
        if( debug_ ) logger_.syslog( "Recieved MultiRay acknowledge", Syslog::IMPORTANT );
    }
    else if( got_nak )
    {
        if( debug_ ) logger_.syslog( "Recieved MultiRay NOT acknowledge", Syslog::IMPORTANT );
    }
    else
    {
        if( debug_ ) logger_.syslog( "Recieved MultiRay unrecognized response", Syslog::IMPORTANT );
        return 0;
    }
    return got_ack;
}

void MultiRay::sendRedLightOn( UartStream &uart )
{
    uart.flush();
    uart << "!001:lout=" << brightnessRed_ << "\r\n";
}

void MultiRay::sendChangeLightsRed( UartStream &uart )
{
    uart.flush();
    uart << "!001:chsw=002\r\n";
}

void MultiRay::sendChangeLightsWhite( UartStream &uart )
{
    uart.flush();
    uart << "!001:chsw=001\r\n";
}

void MultiRay::sendWhiteLightOn( UartStream &uart )
{
    uart.flush();
    uart << "!001:lout=" << brightnessWhite_ << "\r\n";
}

void MultiRay::sendLightOff( UartStream &uart )
{
    uart.flush();
    uart << "!001:lout=0\r\n";
}

// Enter this function until the desired change in lights is fulfilled
Component::RunState MultiRay::changeLightMode()
{
    int mode;
    if( readNewLightMode_ && lightModeReader_->read( Units::COUNT, mode )
            && lightModeReader_->isActive() )
    {
        lightMode_ = ( LightMode ) mode;
        if( debug_ ) logger_.syslog( "New light mode loaded", Syslog::IMPORTANT );
    }
    else if( debug_ ) logger_.syslog( "Did not read new lightMode!", Syslog::IMPORTANT );

    switch( lightMode_ )
    {
    case LIGHT_ON_RED:
    {
        if( readNewLightMode_ )
            // Just got new light mode. Need to step through commanding LED array to Red, then commanding lights on.
            // Each command requires one cycle delay between sending the command and looking for an acknowledge.
        {
            if( debug_ ) logger_.syslog( "Turning on red lights", Syslog::IMPORTANT );
            readNewLightMode_ = false;
            waitBeforeAck_ = true;
            changeModeState_ = CHANGE_LED_ARRAY;
        }
        switch( changeModeState_ )
        {
        case CHANGE_LED_ARRAY:
        {
            if( debug_ ) logger_.syslog( "Changing LED array to red", Syslog::IMPORTANT );
            if( !waitBeforeAck_ )
            {
                if( debug_ ) logger_.syslog( "Look for acknowledge", Syslog::IMPORTANT );
                bool confirm = ( getAcknowledge( uart_ ) && getAcknowledge( uart2_ ) );
                if( debug_ ) logger_.syslog( "Got confirm value: ", ( int ) confirm, Syslog::IMPORTANT );
                if( confirm )
                {
                    if( debug_ ) logger_.syslog( "Change LED array RED command successful", Syslog::IMPORTANT );
                    changeModeState_ = CHANGE_LED_BRIGHTNESS;
                    waitBeforeAck_ = true;
                }
                else
                {
                    if( debug_ ) logger_.syslog( "Change LED array RED command failed! Try again next cycle", Syslog::IMPORTANT );
                    waitBeforeAck_ = true;
                }
            }
            else
            {
                if( debug_ ) logger_.syslog( "Send command, don't look for acknowledge yet", Syslog::IMPORTANT );
                sendChangeLightsRed( uart_ );
                sendChangeLightsRed( uart2_ );
                waitBeforeAck_ = false;
            }
            break;
        }
        case CHANGE_LED_BRIGHTNESS:
        {
            if( debug_ ) logger_.syslog( "Turning on red LED array", Syslog::IMPORTANT );
            if( !waitBeforeAck_ )
            {
                if( debug_ ) logger_.syslog( "Look for acknowledge", Syslog::IMPORTANT );
                bool confirm = ( getAcknowledge( uart_ ) && getAcknowledge( uart2_ ) );
                if( debug_ ) logger_.syslog( "Got confirm value: ", ( int ) confirm, Syslog::IMPORTANT );
                if( confirm )
                {
                    logger_.syslog( "Multiray red lights ON", Syslog::IMPORTANT );
                    changeRequested_ = false;
                }
                else
                {
                    if( debug_ ) logger_.syslog( "Change LED brightness RED command failed! Try again next cycle", Syslog::IMPORTANT );
                    waitBeforeAck_ = true;
                }
            }
            else
            {
                if( debug_ ) logger_.syslog( "Send command, don't look for acknowledge yet", Syslog::IMPORTANT );
                sendRedLightOn( uart_ );
                sendRedLightOn( uart2_ );
                waitBeforeAck_ = false;
            }
            break;
        }
        }
        break;
    }
    case LIGHT_ON_WHITE:
    {
        if( readNewLightMode_ )
        {
            if( debug_ ) logger_.syslog( "Turning on white lights", Syslog::IMPORTANT );
            readNewLightMode_ = false;
            waitBeforeAck_ = true;
            changeModeState_ = CHANGE_LED_ARRAY;
            if( debug_ ) logger_.syslog( "set change mode state to change array", Syslog::IMPORTANT );
        }

        switch( changeModeState_ )
        {
        case CHANGE_LED_ARRAY:
        {
            if( debug_ ) logger_.syslog( "Changing LED array to white", Syslog::IMPORTANT );
            if( !waitBeforeAck_ )
            {
                if( debug_ ) logger_.syslog( "Look for acknowledge", Syslog::IMPORTANT );
                bool confirm = ( getAcknowledge( uart_ ) && getAcknowledge( uart2_ ) );
                if( debug_ ) logger_.syslog( "Got confirm value: ", ( int ) confirm, Syslog::IMPORTANT );
                if( confirm )
                {
                    if( debug_ ) logger_.syslog( "Change LED array WHITE command successful", Syslog::IMPORTANT );
                    changeModeState_ = CHANGE_LED_BRIGHTNESS;
                    waitBeforeAck_ = true;
                }
                else
                {
                    if( debug_ ) logger_.syslog( "Change LED array WHITE command failed! Try again next cycle",
                                                     Syslog::IMPORTANT );
                    waitBeforeAck_ = true;
                }
            }
            else
            {
                if( debug_ ) logger_.syslog( "Send command, don't look for acknowledge yet", Syslog::IMPORTANT );
                sendChangeLightsWhite( uart_ );
                sendChangeLightsWhite( uart2_ );
                waitBeforeAck_ = false;
            }
            break;
        }

        case CHANGE_LED_BRIGHTNESS:
        {
            if( debug_ ) logger_.syslog( "Turning on white LED array", Syslog::IMPORTANT );
            if( !waitBeforeAck_ )
            {
                if( debug_ ) logger_.syslog( "Look for acknowledge", Syslog::IMPORTANT );
                bool confirm = ( getAcknowledge( uart_ ) && getAcknowledge( uart2_ ) );
                if( debug_ ) logger_.syslog( "Got confirm value: ", ( int ) confirm, Syslog::IMPORTANT );
                if( confirm )
                {
                    logger_.syslog( "MultiRay white lights ON", Syslog::IMPORTANT );
                    changeRequested_ = false;
                }
                else
                {
                    if( debug_ ) logger_.syslog( "Change LED brightness WHITE command failed! Try again next cycle",
                                                     Syslog::IMPORTANT );
                    waitBeforeAck_ = true;
                }
            }
            else
            {
                if( debug_ ) logger_.syslog( "Send command, don't look for acknowledge yet", Syslog::IMPORTANT );
                sendWhiteLightOn( uart2_ );
                sendWhiteLightOn( uart_ );
                waitBeforeAck_ = false;
            }
            break;
        }
        }
        break;
    }
    case LIGHT_OFF:
    {
        if( debug_ ) logger_.syslog( "Turning off all lights", Syslog::IMPORTANT );
        if( readNewLightMode_ )
        {
            readNewLightMode_ = false;
            waitBeforeAck_ = true;
        }

        if( !waitBeforeAck_ )
        {
            if( debug_ ) logger_.syslog( "Look for acknowledge", Syslog::IMPORTANT );
            bool confirm = ( getAcknowledge( uart_ ) && getAcknowledge( uart2_ ) );
            if( debug_ ) logger_.syslog( "Got confirm of ", ( int ) confirm, Syslog::IMPORTANT );
            if( confirm )
            {
                logger_.syslog( "MultiRay all lights OFF", Syslog::IMPORTANT );
                changeRequested_ = false;
                return PAUSE;
            }
            else
            {
                logger_.syslog( "All lights off failed! Try again next cycle", Syslog::INFO );
                waitBeforeAck_ = true;
            }
        }
        else
        {
            if( debug_ ) logger_.syslog( "Send command, don't look for acknowledge yet", Syslog::IMPORTANT );
            waitBeforeAck_ = false;
            sendLightOff( uart_ );
            sendLightOff( uart2_ );
        }
        break;
    }
    }
    return RUNNABLE;
}

Component::RunState MultiRay::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    logger_.syslog( "Powering up MultiRay Lights", Syslog::INFO );

    if( simulateHardware() )
    {
        return STARTING;
    }

    this->setAllowableFailures( 3 );
    this->setRetryTimeout( 180 );

    if( !loadControl_.powerUp() )
    {
        logger_.syslog( Str( "MultiRay load controller 1 failed to power up." ), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        return START;
    }

    if( !loadControl2_.powerUp() )
    {
        logger_.syslog( Str( "MultiRay load controller 2 failed to power up." ), Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        return START;
    }

    // Open the uart ports
    uart_.enableUART();
    uart_.open();
    if( uart_.hasError() )
    {
        logger_.syslog( "Error opening uart: ", uart_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    uart_.flush();

    uart2_.enableUART();
    uart2_.open();
    if( uart2_.hasError() )
    {
        logger_.syslog( "Error opening uart2: ", uart2_.errorString(), Syslog::ERROR );
        this->setFailure( FailureMode::COMMUNICATIONS );
        return STOP;
    }
    uart2_.flush();

    // Start the clock for PO timeout in starting()
    startTime_ = Timestamp::Now();
    return STARTING;
}

Component::RunState MultiRay::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    if( !simulateHardware() )
    {
        if( readConfig() )
        {
            startTime_ = Timestamp::Now();
            if( debug_ ) logger_.syslog( "Finished reading config", Syslog::INFO );
            sendLightOff( uart_ );
            sendLightOff( uart2_ );     // Turn off lights at startup
            return RUNNABLE;
        }
        if( startTime_.elapsed() > timeout_ )
        {
            logger_.syslog( "Failed to initialize MultiRay lights within timeout.", Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
    }
    return RUNNABLE;
}

Component::RunState MultiRay::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    if( !simulateHardware() )
    {
        loadControl_.requestVoltageAndCurrent();
        loadControl2_.requestVoltageAndCurrent();
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down MultiRay LCB1", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        uart_.close();

        if( !loadControl2_.powerDown() )
        {
            logger_.syslog( "Failed to power down MultiRay LCB2", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        uart2_.close();
    }
    return PAUSED;
}

Component::RunState MultiRay::paused()
{
    if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isDataRequested() )
    {
        if( lightModeReader_->wasTouchedSinceLastRun( this ) )
        {
            changeRequested_ = true;
            readNewLightMode_ = true;
            return RESUME;
        }
    }
    return PAUSED;
}

Component::RunState MultiRay::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );

    if( !simulateHardware() )
    {
        // Start LCBs
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( Str( "MultiRay load controller 1 failed to resume." ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }

        if( !loadControl2_.powerUp() )
        {
            logger_.syslog( Str( "MultiRay load controller 2 failed to resume." ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }

        // Open uart ports
        uart_.enableUART();
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error resuming uart: ", uart_.errorString(), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        uart_.flush();

        uart2_.enableUART();
        uart2_.open();
        if( uart2_.hasError() )
        {
            logger_.syslog( "Error resuming uart2: ", uart2_.errorString(), Syslog::ERROR );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        uart2_.flush();
    }
    if( debug_ ) logger_.syslog( "Finish turning things back on in Resume", Syslog::INFO );

    if( !isDataRequested() ) return PAUSE;
    return RUNNABLE;
}

Component::RunState MultiRay::runnable()
{
    if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );
    if( !isDataRequested() ) return PAUSE;
    loadControl_.requestVoltageAndCurrent();
    loadControl2_.requestVoltageAndCurrent();

    if( lightModeReader_->wasTouchedSinceLastRun( this ) )
    {
        changeRequested_ = true;
        readNewLightMode_ = true;
    }
    if( changeRequested_ ) return changeLightMode();
    else return RUNNABLE;
}

Component::RunState MultiRay::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );

    uninitialize(); // First power down then query for faults next cycle
    return STOPPING;
}

Component::RunState MultiRay::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );

    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            // Put anything that isn't an actual fault first
            if( loadControl_.errorString().find( "Software" ) )
            {
                if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR );
            }
            // And things that set failures second
            else
            {
                logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }

        loadControl2_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl2_.hasError() )
        {
            // Put anything that isn't an actual fault first
            if( loadControl2_.errorString().find( "Software" ) )
            {
                if( debug_ )logger_.syslog( "LCB error:" + loadControl2_.errorString(), Syslog::ERROR );
            }
            // And things that set failures second
            else
            {
                logger_.syslog( "LCB fault: " + loadControl2_.errorString(), Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
            }
        }

    }
    return STOPPED;
}

Component::RunState MultiRay::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );

    readConfig();

    if( !simulateHardware() )
    {
        if( ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            return stop();
        }
        if( ( loadControl2_.getPowerState() != LoadControl::OFF ) && ( loadControl2_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            return stop();
        }

        // Close if the uart if it is open
        if( uart_.isReadable() )
        {
            uart_.close();
        }
        if( uart2_.isReadable() )
        {
            uart2_.close();
        }
    }
    return STOPPED;
}

void MultiRay::uninitialize()
{
    if( !simulateHardware() )
    {
        uart_.close();
        uart_.disableUART();
        uart2_.close();
        uart2_.disableUART();
    }

    logger_.syslog( "Powering down.", Syslog::INFO );
    if( !simulateHardware() && !loadControl_.powerDown() && !loadControl2_.powerDown() )
    {
        logger_.syslog( "Failed to power down", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
    }
}

ConfigURI MultiRay::getConfigURI( ConfigOption configOption ) const
{
    return ConfigURI::NO_CONFIG_URI;
}

// unused component functions
void MultiRay::run()
{
    if( debug_ ) logger_.syslog( "Run", Syslog::IMPORTANT );
}

Component::RunState MultiRay::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::IMPORTANT );
    return RESUMING;
}