// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/DeviceImpl.cpp 1167 2019-01-25 11:51:32Z depa $
// #include "basda/service/bcu/worker/DeviceImpl.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/DeviceImpl.h"
// #include "basda/service/bcu/worker/Const.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/Const.h"
#include "tiptilt_ctrl/bcu/util/Assertions.h"



#undef LOG_NAME
#define LOG_NAME name_

namespace Sharknir { namespace Bcu {

typedef DeviceImpl::DWord DWord;

const double DeviceImpl::TEMPERATURE_CONVERSION_FACTOR= 0.0078125;


DeviceImpl::DeviceImpl(const Nice::JointPtr & config) :
    config_(config)
{
    SHARKNIR_PRECONDITION(config->exist(PropertyName::name()),
            "Name property is missing");
    SHARKNIR_PRECONDITION(0 != config, "Config must be valid");
    name_= config->getAtNode<std::string>(PropertyName::name());
}

void DeviceImpl::initialize()
{
    SHARKNIR_PRECONDITION(0 == wrappedDevice_,
            "Wrapped device is already initialized");
    wrappedDevice_= new Basda::BcuDevice(config_);
    wrappedDevice_->init();
}


double DeviceImpl::readBcuFpgaTemperatureInC()
{
    checkInvariant();

    D_LOG("Reading BCU FPGA temperature");
    DWord temperatures= readBcuTemperatureDword();
    const double fpgaTemperatureInC= (temperatures & 0xffff) *
                                            TEMPERATURE_CONVERSION_FACTOR;
    D_LOG(fpgaTemperatureInC);
    return fpgaTemperatureInC;
}

DWord DeviceImpl::readBcuTemperatureDword()
{
    // TODO 2012-02-28
    // Put into constant pool because python module uses same value!
    const int32_t BCU_SYSTEM_DIAGNOSTIC_BEGIN= 0x3801c;
    Basda::Microgate::MemoryBufferPtr b= wrappedDevice_->read(
            Basda::Microgate::MGP_OP_RDSEQ_SRAM,
            Const::BCU_DSP, Const::BCU_DSP, BCU_SYSTEM_DIAGNOSTIC_BEGIN, 1);

    return b->getBeginOfBuffer()[0];
}

double DeviceImpl::readBcuPcbTemperatureInC()
{
    checkInvariant();
    D_LOG("Reading BCU PCB temperature")

    DWord temperatureDword= readBcuTemperatureDword();
    const double pcbTempInC= ((temperatureDword >> 16) & 0xffff) *
                                            TEMPERATURE_CONVERSION_FACTOR;

    return pcbTempInC;
}

// double DeviceImpl::readDsp16do0FpgaTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading DSP16do0 FPGA temperature")

//     return readDsp16doFpgaTemperatureInC(Const::DSP16DO_0);
// }

// double DeviceImpl::readDsp16doFpgaTemperatureInC(uint8_t dsp)
// {
//     DWord temperatureWord= readDsp16doTemperatureWord(dsp);
//     double tempInC= (temperatureWord & 0xffff) *
//                                         TEMPERATURE_CONVERSION_FACTOR;
//     return tempInC;
// }

// DWord DeviceImpl::readDsp16doTemperatureWord(uint8_t dsp)
// {
//     const DWord DSP_SYSTEM_DIAGNOSTIC_BEGIN= 0x181f9;
//     Basda::Microgate::MemoryBufferPtr b= wrappedDevice_->read(
//             Basda::Microgate::MGP_OP_RDSEQ_SRAM, dsp, dsp,
//             DSP_SYSTEM_DIAGNOSTIC_BEGIN, 1);
//     DWord temperatureWord= b->getBeginOfBuffer()[0];
//     return temperatureWord;
// }

// double DeviceImpl::readDsp16do0PcbTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading DSP16do0 PCB temperature");

//     return readDsp16DoPcbTemperatureInC(Const::DSP16DO_0);
// }

// double DeviceImpl::readDsp16DoPcbTemperatureInC(uint8_t dsp)
// {
//     DWord temperatureWord= readDsp16doTemperatureWord(dsp);
//     double tempInC= ((temperatureWord >> 16) & 0xffff) *
//                                             TEMPERATURE_CONVERSION_FACTOR;
//     return tempInC;
// }

// double DeviceImpl::readDsp16do1FpgaTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading DSP16do1 FPGA temperature");
//     return readDsp16doFpgaTemperatureInC(Const::DSP16DO_1);
// }

// double DeviceImpl::readDsp16do1PcbTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading DSP16do1 PCB temperature");

//     return readDsp16DoPcbTemperatureInC(Const::DSP16DO_1);
// }

// double DeviceImpl::readHvc0FpgaTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading HVC0 FPGA temperature");

//     return readHvcFpgaTemperatureInC(Const::HVC_0_DSP);
// }

// double DeviceImpl::readHvcFpgaTemperatureInC(uint8_t dsp)
// {
//     DWord temperatureDWord= readHvcTemperatureDword(dsp);
//     double tempInC= (temperatureDWord & 0xffff) *
//                                         TEMPERATURE_CONVERSION_FACTOR;
//     return tempInC;
// }

// DWord DeviceImpl::readHvcTemperatureDword(uint8_t dsp)
// {
//     const DWord HVC_SYSTEM_DIAGNOSTIC_BEGIN= 0x181a8;
//     Basda::Microgate::MemoryBufferPtr b= wrappedDevice_->read(
//             Basda::Microgate::MGP_OP_RDSEQ_SRAM,
//             dsp, dsp,
//             HVC_SYSTEM_DIAGNOSTIC_BEGIN, 1);
//     DWord temperatureDWord= b->getBeginOfBuffer()[0];
//     return temperatureDWord;
// }

// double DeviceImpl::readHvc0PcbTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading HVC0 PCB temperature");

//     return readHvcPcbTemperatureInC(Const::HVC_0_DSP);
// }

// double DeviceImpl::readHvcPcbTemperatureInC(uint8_t dsp)
// {
//     DWord tempDWord= readHvcTemperatureDword(dsp);
//     double tempInC= (tempDWord >> 16) * TEMPERATURE_CONVERSION_FACTOR;

//     return tempInC;
// }

// double DeviceImpl::readHvc1FpgaTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading HVC1 FPGA temperature");

//     return readHvcFpgaTemperatureInC(Const::HVC_1_DSP);
// }

// double DeviceImpl::readHvc1PcbTemperatureInC()
// {
//     checkInvariant();
//     D_LOG("Reading HVC1 PCB temperature");
//     return readHvcPcbTemperatureInC(Const::HVC_1_DSP);
// }

Basda::Microgate::MemoryBufferPtr DeviceImpl::read(
        Basda::Microgate::OpCode opcode,
        uint8_t firstDsp, uint8_t lastDsp,
        int32_t address, unsigned lengthInDword)
{
    D_LOG("Reading with opcode " << opcode <<
            " from DSP range [" << firstDsp << ":" <<
            lastDsp << "] " << " " << lengthInDword <<
            " DWords from address" <<
            std::hex << std::showbase << address
            << std::dec << std::noshowbase);

    return wrappedDevice_->read(opcode, firstDsp, lastDsp,
            address, lengthInDword);
}

void DeviceImpl::write(Basda::Microgate::OpCode opcode,
                   uint8_t firstDsp,
                   uint8_t lastDsp,
                   int32_t address,
                   const Basda::Microgate::MemoryBufferPtr& buffer )
{
    D_LOG("Writing with opcode " << opcode << "from DSP range [" <<
            firstDsp << ":" << lastDsp << "] to address " << std::hex
            << std::showbase << address << std::dec << std::noshowbase <<
            " " << buffer->getBufferLengthInDword() << " DWords")

    wrappedDevice_->write(opcode, firstDsp, lastDsp, address, buffer);
}

void DeviceImpl::uploadDspCode(Nice::U8 firstDsp, Nice::U8 lastDsp,
                       const Nice::File& dspProgram)
{
    D_LOG("Uploading DSP code " << dspProgram.filename <<
            " to DSP range [" << firstDsp << ":" << lastDsp << "]")

    wrappedDevice_->uploadDspCode(firstDsp, lastDsp, dspProgram);
}

void DeviceImpl::enableMasterDiagnostics( Nice::U16 decimationFactor,
        const std::string& remoteIpv4Address,
        const std::string& remoteMacAddress, Nice::U16 remoteUdpPortNr)
{
    D_LOG("Enabling master diagnostics with decimation factor " <<
            decimationFactor << ", IPv4 address " << remoteIpv4Address <<
            ", UDP port " << remoteUdpPortNr << " and MAC address " <<
            remoteMacAddress)

    wrappedDevice_->enableMasterDiagnostic(decimationFactor,
            remoteIpv4Address, remoteMacAddress, remoteUdpPortNr);
}
DeviceImpl::~DeviceImpl()
{
}

void DeviceImpl::checkInvariant()
{
    SHARKNIR_PRECONDITION(0 != wrappedDevice_, "Device is uninitialized");
}

}
}
