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

// socket headers
#include <netdb.h>
#include <unistd.h>
#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"

//////////////////////////////////////////////////////////////////////////////
// constants
static const int buffsize = 1024;
static const std::string appName = "lars_lcm_pub";

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

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

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

int main(int argc, char *argv[])
{
    int sockfd;

    struct hostent* hp;
    struct sockaddr_in destaddr;

    // LARS data
    lars::data lars_data;

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

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

    //////////////////////////////////////////////////////////////////////////
    // get the startup args

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

        return -1;
    }

    // resolve the destination name
    hp = gethostbyname(argv[1]);

    if ( hp == NULL )
    {
        std::cout << "ERR: failed to resolve destination address";
        std::cout << std::endl;
        return -1;
    }

    char* ip = inet_ntoa(*((struct in_addr*)hp->h_addr_list[0]));
    std::string dest_address = std::string(ip);

    int dest_port = std::atoi(argv[2]);

    //////////////////////////////////////////////////////////////////////////
    // create the socket file descriptor

    sockfd = socket(AF_INET, SOCK_DGRAM, 0);

    if ( sockfd < 0 )
    {
        std::perror("socket creation failed");
        return -1;
    }

    //////////////////////////////////////////////////////////////////////////
    // initialize the destination socket address struct

    std::memset(&destaddr, 0, sizeof(destaddr));

    // set up the server sockaddr structure
    destaddr.sin_family = AF_INET;
    destaddr.sin_addr.s_addr = inet_addr(dest_address.c_str());
    destaddr.sin_port = htons(dest_port);

    //////////////////////////////////////////////////////////////////////////
    // send the message

    std::cout << "INFO: sending datagrams to " << dest_address << " on port ";
    std::cout << dest_port << std::endl;

    int i = 0;

    
    for (;;)
    {
        lars::sleep_ms(250);

        lars_data.TargetTension = ++i;
        lars_data.LoadcellTension = i + 1;

        lars_data.TractionWinchCommandedTorque = i + 2;
        lars_data.TractionWinchCommandedRPM = i + 3;
        lars_data.TractionWinchActualTorque = i + 4;
        lars_data.TractionWinchActualRPM = i + 5;
        
	lars_data.PID_p = i + 6;
	lars_data.PID_i = i + 7;
	lars_data.PID_d = i + 8;
	lars_data.PID_error = i + 9;

        // reset at 1K
        if ( i > 999 ) i = 0;

        char lars_str[buffsize];

	int n_strlen = sprintf(lars_str,"%d,%d,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
			                  lars_data.TargetTension,
			                  lars_data.LoadcellTension,
			                  lars_data.TractionWinchCommandedTorque,
			                  lars_data.TractionWinchCommandedRPM,
			                  lars_data.TractionWinchActualTorque,
			                  lars_data.TractionWinchActualRPM,
					  lars_data.PID_p,
					  lars_data.PID_i,
					  lars_data.PID_d,
					  lars_data.PID_error);


        std::cout << "INFO: sending LARS datagram (" << i << ") ";
        std::cout << lars_str << std::endl;

        sendto(sockfd, (const void*)lars_str, n_strlen, 0,
               (const struct sockaddr*)&destaddr, sizeof(destaddr));
    }

    close(sockfd);

    return 0;
}