#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>

#include "udp.h"

UDP_LISTENER::UDP_LISTENER() {
  fd = -1;
}

UDP_LISTENER::~UDP_LISTENER() {
  if( fd >= 0 )
    close( fd );
}

STAT udp_open( UDP_LISTENER* udp, I32 port ) {
  udp->fd = socket( AF_INET, SOCK_DGRAM, 0 );
  if( udp->fd < 0 )
    return STAT_ERR;

  I32 one = 1;
  setsockopt( udp->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one) );

  struct sockaddr_in a;
  memset( &a, 0, sizeof( a ) );
  a.sin_family      = AF_INET;
  a.sin_addr.s_addr = htonl( INADDR_ANY );
  a.sin_port        = htons( (U16)port );

  if( bind( udp->fd, (struct sockaddr*)&a, sizeof(a) ) < 0 ) {
    close( udp->fd );
    udp->fd = -1;
    return STAT_ERR;
  }

  fcntl( udp->fd, F_SETFL, O_NONBLOCK );
  return STAT_OK;
}

void udp_parse( UDP_LISTENER* udp, TELEMETRY* t ) {
  if( udp->fd < 0 )
    return;

  U8 buf[2048];
  for( ;; ) {
    ssize_t n = recv( udp->fd, buf, sizeof( buf ), 0 );
    if( n < 0 )
      break;     // EAGAIN
    if( n < 20 )
      continue;  // not a forza packet

    I32 race;
    F32 fmax, fidle, fcur;
    memcpy( &race,  buf +  0, 4 );
    memcpy( &fmax,  buf +  8, 4 );
    memcpy( &fidle, buf + 12, 4 );
    memcpy( &fcur,  buf + 16, 4 );

    if( !( fmax >= 0.0f && fmax < 60000.0f ) )
      continue;
    if( !( fcur >= 0.0f && fcur < 60000.0f ) )
      continue;

    memcpy( &t->vel_x, buf + 28, 4 );
    memcpy( &t->vel_y, buf + 32, 4 );
    memcpy( &t->vel_z, buf + 40, 4 );

    t->race_on  = race;
    t->rpm_max  = fmax;
    t->rpm_idle = fidle;
    t->rpm      = fcur;
    t->extended = n >= 324;

    if( t->extended ) {
      I32 dt;
      memcpy( &t->pos_x,     buf + 244, 4 );
      memcpy( &t->pos_y,     buf + 248, 4 );
      memcpy( &t->pos_z,     buf + 252, 4 );
      memcpy( &t->power,     buf + 260, 4 );
      memcpy( &t->speed_ms,  buf + 256, 4 );
      memcpy( &t->boost_psi, buf + 284, 4 );
      memcpy( &dt,           buf + 224, 4 );
      memcpy( &t->laptime,   buf + 304, 4 );
      memcpy( &t->racetime,  buf + 308, 4 );
      t->lapnum     = *(U16*)( &buf[312] );
      t->racepos    = buf[314];
      t->drivetrain = dt;
      t->accel      = buf[315];
      t->brake      = buf[316];
      t->clutch     = buf[317];
      t->handbrake  = buf[318];
      t->gear       = buf[319];
      t->norm_driving_line = buf[321];
      t->norm_ai_brake_diff = buf[322];
      memcpy( &t->car_ordinal, buf + 212, 4 );
      for( I32 i = 0; i < 4; i++ ) {
        memcpy( &t->slip_ratio[i],    buf +  84 + 4*  i, 4 );
        memcpy( &t->combined_slip[i], buf + 180 + 4*  i, 4 );
        memcpy( &t->tire_temp[i],      buf + 268 + 4*  i, 4 );
      }
    }
    t->stamp = t_nows();
  }
}