// Copyright 2026 MBARI.                                                    //
// Monterey Bay Aquarium Research Institute Proprietary Information.        //
// All rights reserved.                                                     //
//////////////////////////////////////////////////////////////////////////////
#include <cstdio>
#include <chrono>
#include <cstring>
#include <sstream>
#include <iostream>

// socket headers
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

// LARS data structure
#include "lars_data.hpp"

// LARS utils
#include "lars_utils.hpp"

// LCM header
#include <lcm/lcm-cpp.hpp>

// LCM messages
#include "lars/data_msg_t.hpp"

//////////////////////////////////////////////////////////////////////////////
// constants

static const std::string appName = "lars_lcm_pub";

static const int buffsize = 1024;

// LCM publish channel name(s)
static const std::string larsPubName = "LARS_DATA";

//////////////////////////////////////////////////////////////////////////////
//  globals

//////////////////////////////////////////////////////////////////////////////
// objects

//////////////////////////////////////////////////////////////////////////////
// function prototypes

//////////////////////////////////////////////////////////////////////////////
// debug macro

#define DEBUG_LINE std::cout << "DEBUG_LINE: in file " << __FILE__ \
                             << " at line " << __LINE__ << std::endl;

int main(int argc, char *argv[])
{
    int rc;
    int sockfd;
    char buffer[buffsize];
    struct sockaddr_in servaddr, cliaddr;

    // LCM object
    lcm::LCM lcm;

    // LARS data
    //lars::data lars_data;

    // LARS LCM message
    lars::data_msg_t lars_data_msg;

    //////////////////////////////////////////////////////////////////////////
    // startup banner

    //std::cout << appName << " built on ";
    //std::cout << __DATE__ << " at " __TIME__ << std::endl;

    //////////////////////////////////////////////////////////////////////////
    // get the start up arg(s)

    if ( argc < 2 )
    {
        std::cout << "ERR: missing arguments" << std::endl;
        std::cout << std::endl;
        std::cout << "usage: ./" << appName << " [port]";
        std::cout << std::endl;
        std::cout << std::endl;
        std::cout << "port - LARS data listening port" << std::endl;
        std::cout << std::endl;

        return -1;
    }

    int server_port = std::atoi(argv[1]);

    //////////////////////////////////////////////////////////////////////////
    // check LCM instance

    if( lcm.good() == false )
    {
        lars::err_msg("bad LCM instance");
        return -1;
    }

    //////////////////////////////////////////////////////////////////////////
    // creating socket file descriptor

    sockfd = socket(AF_INET, SOCK_DGRAM, 0);

    if ( sockfd < 0 )
    {
        lars::err_msg("socket creation failed");
        lars::err_msg(std::strerror(sockfd));

        return -1;
    }

    //////////////////////////////////////////////////////////////////////////
    // initialize the client and server socket address structs

    std::memset(&servaddr, 0, sizeof(servaddr));
    std::memset(&cliaddr, 0, sizeof(cliaddr));

    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(server_port);

    //////////////////////////////////////////////////////////////////////////
    // set timeout on socket

    struct timeval tv;
    tv.tv_sec = 3;
    tv.tv_usec = 0;

    rc = setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv,sizeof(tv));

    if ( rc < 0 )
    {
        lars::err_msg("failed to set timeout on socket");
        lars::err_msg(std::strerror(rc));
        return -1;
    }

    //////////////////////////////////////////////////////////////////////////
    // bind socket to the address

    rc = bind(sockfd, (const struct sockaddr*)&servaddr, sizeof(servaddr));

    if ( rc < 0 )
    {
        lars::err_msg("bind failed");
        lars::err_msg(std::strerror(rc));
        return -1;
    }

    //////////////////////////////////////////////////////////////////////////
    // wait to receive data from the sender

    for(;;)
    {
        socklen_t len;

        // get client address size
        len = sizeof(cliaddr);

        rc = recvfrom(sockfd, (char*)buffer, buffsize,
                      MSG_WAITALL, (struct sockaddr*)&cliaddr, &len);

        if ( rc < 0)
        {
            lars::info_msg("timed out waiting for message");
        }
        else if ( rc > buffsize )
        {
            std::stringstream ss;

            ss << "WARN: received datagram of " << rc << " bytes, ";
            ss << "but expected less than " << buffsize << " bytes.";

            lars::warn_msg(ss.str());
        }
        else
        {
            // copy LARS data to structure
            //std::memcpy(&lars_data, buffer, sizeof(lars_data));
            // parse LARS data to structure
            sscanf(buffer,"%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d,%f",
			    &(lars_data_msg.TargetTension),
			    &(lars_data_msg.LoadcellTension),
			    &(lars_data_msg.TractionWinchCommandedTorque),
			    &(lars_data_msg.TractionWinchCommandedRPM),
			    &(lars_data_msg.TractionWinchActualTorque),
			    &(lars_data_msg.TractionWinchActualRPM),
                            &(lars_data_msg.SDRactualTorque),
                            &(lars_data_msg.SDRactualRPM),
                            &(lars_data_msg.SWCposition),
                            &(lars_data_msg.PID_FF),
			    &(lars_data_msg.PID_p),
			    &(lars_data_msg.PID_i),
			    &(lars_data_msg.PID_d),
			    &(lars_data_msg.PID_Tf),
			    &(lars_data_msg.cmd_min),
			    &(lars_data_msg.cmd_max),
			    &(lars_data_msg.SDRCommandRPM),
			    &(lars_data_msg.SWC_OilPressure),
			    &(lars_data_msg.SWC_OilPressureAlarmLimit),
			    &(lars_data_msg.TorqueMode_VFD_RPMLimit),
			    &(lars_data_msg.TorqueMode_PID_RPMLimitHaulIn),
			    &(lars_data_msg.CraneIndorMode),
			    &(lars_data_msg.OutHaulerActive),
			    &(lars_data_msg.ControlWord),
			    &(lars_data_msg.StatusWord),
			    &(lars_data_msg.TorqueMode_PID_RPMLimitPayOut));


            // update LCM message
            lars_data_msg.timestamp = lars::get_timestamp();

            // publish LCM message
            lcm.publish(larsPubName, &lars_data_msg);

            printf("%f,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d,%f\n",
                            (lars_data_msg.timestamp),
			    (lars_data_msg.TargetTension),
			    (lars_data_msg.LoadcellTension),
			    (lars_data_msg.TractionWinchCommandedTorque),
			    (lars_data_msg.TractionWinchActualTorque),
			    (lars_data_msg.TractionWinchCommandedRPM),
			    (lars_data_msg.TractionWinchActualRPM),
                            (lars_data_msg.SDRactualTorque),
                            (lars_data_msg.SDRactualRPM),
                            (lars_data_msg.SWCposition),
                            (lars_data_msg.PID_FF),
			    (lars_data_msg.PID_p),
			    (lars_data_msg.PID_i),
			    (lars_data_msg.PID_d),
			    (lars_data_msg.PID_Tf),
			    (lars_data_msg.cmd_min),
			    (lars_data_msg.cmd_max),
			    (lars_data_msg.SDRCommandRPM),
			    (lars_data_msg.SWC_OilPressure),
			    (lars_data_msg.SWC_OilPressureAlarmLimit),
			    (lars_data_msg.TorqueMode_VFD_RPMLimit),
			    (lars_data_msg.TorqueMode_PID_RPMLimitHaulIn),
			    (lars_data_msg.CraneIndorMode),
			    (lars_data_msg.OutHaulerActive),
			    (lars_data_msg.ControlWord),
			    (lars_data_msg.StatusWord),
			    (lars_data_msg.TorqueMode_PID_RPMLimitPayOut));
        }
    }

    return 0;
}