#include "overlay.h"
#include "gauges.h"

const F32 TRAIL_TACH_MAX = 0.14f;
const F32 TRAIL_BOOST_MAX = 0.16f;

OVERLAY_DATA* overlay_inst() {
  static OVERLAY_DATA d;
  return &d;
}

void overlay_handle_teleport( OVERLAY_DATA* d ) {
  TELEMETRY *t = &d->telem;

  U8  iszero = fabsf( t->pos_x ) < 0.01f && fabsf( t->pos_y ) < 0.01f && fabsf( t->pos_z ) < 0.01f;
  U8  iszero_last = fabsf( d->last_pos_x ) < 0.01f && fabsf( d->last_pos_y ) < 0.01f && fabsf( d->last_pos_z ) < 0.01f;
  F32 dist = vec_dist( t->pos_x, t->pos_y, t->pos_z, d->last_pos_x, d->last_pos_y, d->last_pos_z );
  U8  unteleported = 0;

  if( d->teleported && !iszero ) {
    F32 tdist = vec_dist( t->pos_x, t->pos_y, t->pos_z, d->tele_pos_x, d->tele_pos_y, d->tele_pos_z );
    if( tdist > 1.f ) {
      d->teleported = 0;
      unteleported = 1;
    }
  }

  U8 iszero_vel = fabsf( t->vel_z ) < 3.f;
  if( !iszero_vel ) {
    if( d->teleported )
      unteleported = 1;
    d->teleported = 0;
  }

  // teleported
  if( !unteleported && !d->teleported && !iszero && !iszero_last && dist > 100.f ) {
    d->teleported = 1;
    d->tele_pos_x = t->pos_x;
    d->tele_pos_y = t->pos_y;
    d->tele_pos_z = t->pos_z;
  }

  d->last_brake = t->brake;
  d->last_hb = t->handbrake;
  d->last_pos_x = t->pos_x;
  d->last_pos_y = t->pos_y;
  d->last_pos_z = t->pos_z;

  if( !d->was_race_on && t->race_on ) {
    d->teleported = 0;
  }
}

void overlay_handle_redline( OVERLAY_DATA* d, F64 dt, F64 now ) {
  TELEMETRY* t = &d->telem;
  if( !t->car_ordinal || !t->race_on )
    return;

  if( !d->cfg.autoscale_rpm ) {
    d->valid_redline = 0.86f;
    return;
  }

  if( d->candidate_redline > 0.1f && now - d->redlinebang_time > .175f ) {
    d->redlinechange_time = now;
    d->target_redline = d->candidate_redline;
    d->candidate_redline = 0.f;
  }

  if( d->valid_redline != d->target_redline ) {
    F64 diff = d->target_redline - d->valid_redline;
    if( fabs( diff ) > 0.001f ) {
      F64 max = diff > 0 ? 0.1 : 0.0015 * dt;
      if( diff > 0 && diff > max ) diff = max;
      if( diff < 0 && diff < -max ) diff = -max;
      d->valid_redline += diff;
    }
  }

  if( t->gear != d->last_gear ) {
    d->candidate_redline = 0.f;
    d->last_shifttime = now;

    if( t->gear != 0 && t->gear != 11 && t->gear > d->last_valid_gear )
      d->valid_shift = 1;
    else
      d->valid_shift = 0;
  }

  F64 rpmr = (F64)(t->rpm) / t->rpm_max;
  F64 rpmo = (F64)(t->rpm - d->cfg.rpm_off) / t->rpm_max;
  if( d->valid_shift ) {
    if( rpmr >= 0.98f ) {
      d->valid_redline = 0.86f;
      d->target_redline = 0.86f;
    }
    if( rpmo > d->target_redline ) {
      d->target_redline = rpmo;
    }
  }

  if( d->candidate_redline > 0.f && rpmo >= d->candidate_redline )
    d->candidate_redline = rpmo;
  else if( !d->candidate_redline ) {
    F32 geartime = now - d->last_shifttime;
    if( geartime > 0.9f && rpmo < d->valid_redline && t->power * 0.001341022 <= 1.f && t->accel >= 230 && t->gear != 11 ) {
      d->candidate_redline = (F32)rpmo;
      if( d->candidate_redline < d->valid_redline - 0.01f )
        d->candidate_redline = d->valid_redline - 0.01f;

      d->redlinebang_time = now;
    }
  }

  d->last_gear = t->gear;
  if( t->gear != 11 )
    d->last_valid_gear = t->gear;

  if( d->valid_redline < 0.65f )
    d->valid_redline = 0.65f;
  if( d->target_redline < 0.65f )
    d->target_redline = 0.65f;
}

void overlay_handle_car_changed( OVERLAY_DATA* d ) {
  TELEMETRY* t = &d->telem;
  if( !t->extended )
    return;

  if( !t->car_ordinal )
    return;

  U8 car_changed = ( t->car_ordinal != d->car_ordinal || fabsf( t->rpm_max - d->car_rpm_max ) > 1.0f );
  if( car_changed ) {
    d->car_ordinal    = t->car_ordinal;
    d->car_rpm_max    = t->rpm_max;
    d->peak_boost_bar = 0.0;
    d->boost_since    = 0.0;
    d->valid_redline  = 0.86;
    d->target_redline = 0.86;
    d->candidate_redline = 0.f;
  }
}

void overlay_update_rpm_boost( OVERLAY_DATA* d, U8 live, F64 dt, F64 now ) {
  TELEMETRY* t = &d->telem;

  F64 k = dt * 20.0;
  if( k > 1.0 )
    k = 1.0;
  d->disp_rpm += ( (F64)t->rpm - d->disp_rpm ) * k;
  if( d->disp_rpm > d->peak_rpm )
    d->peak_rpm = d->disp_rpm;
  else
    d->peak_rpm -= 2500.0 * dt;
  if( d->peak_rpm < 0.0 )
    d->peak_rpm = 0.0;

  F64 bar = (F64)t->boost_psi * PSI_TO_BAR;
  F64 kb  = dt * 25.0;
  if( kb > 1.0 )
    kb = 1.0;
  d->disp_boost_bar += ( bar - d->disp_boost_bar ) * kb;
  if( live && bar > d->peak_boost_bar )
    d->peak_boost_bar = bar;

  d->was_race_on = t->race_on;
  F32 kmh = t->speed_ms * 3.6f;
  d->lc_armed = 0;
  if( t->handbrake > 30 && t->accel > 30 && kmh < 1.0f )
    d->lc_armed = 1;

  F64 idt = dt > 1e-4 ? 1.0 / dt : 0.0;
  F64 sm  = dt * 15.0;
  if( sm > 1.0 )
    sm = 1.0;
  d->rpm_vel += ( ( d->disp_rpm - d->prev_disp_rpm ) * idt - d->rpm_vel ) * sm;
  d->bar_vel += ( ( d->disp_boost_bar - d->prev_disp_bar ) * idt - d->bar_vel ) * sm;
  d->prev_disp_rpm = d->disp_rpm;
  d->prev_disp_bar = d->disp_boost_bar;

  if( d->peak_boost_bar > 0.10 && d->boost_since <= 0.0 )
    d->boost_since = now;
}

void overlay_handle_assists( OVERLAY_DATA* d, F64 now ) {
  TELEMETRY* t = &d->telem;

  I32 w0 = 0, w1 = 3;                               // AWD
  if(      t->drivetrain == 0 ) { w0 = 0; w1 = 1; } // FWD
  else if( t->drivetrain == 1 ) { w0 = 2; w1 = 3; } // RWD

  if( t->gear == 11 )
    d->tcs_until = 0;
  else if( t->accel > 30 && t->rpm < t->rpm_max - t->rpm_max * 0.1f && t->rpm > d->last_rpm ) {
    for( I32 i = w0; i <= w1; i++ ) {
      if( t->combined_slip[i] > 1.35f && t->power * 1.02f < d->last_power && t->accel >= d->last_acc )
        d->tcs_until = now + 0.05;
    }
  }

  if( t->brake > 30 ) {
    for( I32 i = 0; i < 4; i++ ) {
      if( t->slip_ratio[i] < -1.f )
        d->abs_until = now + 0.02;
    }
  }
}

void overlay_update( OVERLAY_DATA* d, F64 now, F64 dt ) {
  TELEMETRY* t = &d->telem;

  overlay_handle_teleport( d );

  U8 live = overlay_live( d, now );
  if( live && !d->was_live )
    d->live_since = now;
  d->was_live = live;

  overlay_handle_car_changed( d );
  overlay_handle_redline( d, dt, now );
  overlay_update_rpm_boost( d, live, dt, now );

  if( !t->extended )
    return;

  overlay_handle_assists( d, now );

  d->prev_race_on = t->race_on;
  d->last_power = t->power;
  d->last_acc = t->accel;
  d->last_rpm = t->rpm;
}

U8 overlay_visible( OVERLAY_DATA* d, F64 now ) {
  return overlay_live( d, now ) && fade_alpha( d->live_since, now ) > 0.0f;
}

U8 overlay_live( OVERLAY_DATA* d, F64 now ) {
  U8 live = d->telem.race_on && !d->teleported;
  return now - d->telem.stamp < 3.0 && ( live || d->cfg.keep );
}

void overlay_build_cluster( OVERLAY_DATA* d, GAUGE_RENDERER* gauge, F32 w, F32 h, F64 now ) {
  TELEMETRY* t = &d->telem;
  CONFIG*    c = &d->cfg;

  F32 sc = (F32)c->scale;
  if( sc < 0.2f || sc > 4.0f )
    sc = 1.0f;
  F32 a_main = fade_alpha( d->live_since, now );

  gauge->clear();
  gauge->set_fade( a_main );

  F32 blur = (F32)c->blur;
  if( blur < 0.0f || blur > 0.5f )
    blur = 0.0f;
  F32 rpm_max_safe = t->rpm_max < 100.0f ? 8000.0f : t->rpm_max;
  F32 trail_tach = blur > 0.0f ? (F32)( d->rpm_vel * blur ) / rpm_max_safe : 0.0f;
  if( trail_tach >  TRAIL_TACH_MAX ) trail_tach =  TRAIL_TACH_MAX;
  if( trail_tach < -TRAIL_TACH_MAX ) trail_tach = -TRAIL_TACH_MAX;

  F32 speed = t->speed_ms * ( c->units ? 2.23694f : 3.6f );
  F32 kmh = t->speed_ms * 3.6f;
  F32 rpmoff = c->autoscale_rpm ? (F32)(c->rpm_off - 100.f) / rpm_max_safe : 0.f;
  gauge->build_tach(
    (F32)( c->tach_x * w ), (F32)( c->tach_y * h ), (F32)( c->tach_r * h ) * sc, trail_tach,
    (F32)d->disp_rpm, t->rpm_max, d->valid_redline, rpmoff, (F32)d->peak_rpm,
    t->gear, speed, t->extended, c->flash_needle
  );

  F32 a_boost = a_main * fade_alpha( d->boost_since, now );
  if( c->show_boost && t->extended && a_boost > 0.0f ) {
    F32 need  = (F32)d->peak_boost_bar + 0.25f;
    F32 range = ceilf( need / 0.25f ) * 0.25f;
    if( range < 0.5f ) range = 0.5f;

    F32 shown = (F32)d->disp_boost_bar;
    if( shown < -1.0f ) shown = -1.0f;
    if( shown > range ) shown = range;

    F32 span = range + 1.0f;
    F32 trail_boost = blur > 0.0f ? (F32)( d->bar_vel * blur ) / span : 0.0f;
    if( trail_boost >  TRAIL_BOOST_MAX ) trail_boost =  TRAIL_BOOST_MAX;
    if( trail_boost < -TRAIL_BOOST_MAX ) trail_boost = -TRAIL_BOOST_MAX;
    gauge->set_fade( a_boost );
    gauge->build_boost(
      (F32)( c->boost_x * w ), (F32)( c->boost_y * h ), (F32)( c->boost_r * h ) * sc,
      shown, range, trail_boost
    );
    gauge->set_fade( a_main );
  }

  if( t->extended && ( c->show_abs || c->show_tcs || c->show_lc ) ) {
    U8 abs_show = c->show_abs;
    U8 tcs_show = c->show_tcs;
    U8 lc_show = c->show_lc;
    U8 abs_lit  = abs_show && now < d->abs_until;
    U8 tcs_lit  = ( tcs_show && now < d->tcs_until );
    U8 lc_lit = (now < d->tcs_until && kmh < 50);
    U8 lc_flash = c->show_lc && ( ( d->lc_armed && fmod( now, 0.30 ) < 0.20 ) || lc_lit );
    gauge->build_lamps(
      (F32)( c->tach_x * w ), (F32)( c->tach_y * h ),
      (F32)( c->tach_r * h * sc ), (F32)( c->tach_r * h * sc * 0.1f ),
      t->handbrake > 0,
      abs_show, tcs_show || lc_show, abs_lit, tcs_lit || lc_flash
    );
  }

  if( t->extended && c->show_tires ) {
    gauge->build_tires( (F32)( c->tire_x * w ), (F32)( c->tire_y * h ), (F32)( c->tire_sc * h ),
        d->telem.tire_temp[0], d->telem.tire_temp[1], d->telem.tire_temp[2], d->telem.tire_temp[3] );
  }
}