#pragma once

extern const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL;
const uint16_t FLAG_VOLKSWAGEN_LONG_CONTROL = 1;
extern const uint16_t FLAG_VOLKSWAGEN_ALT_CRC_VARIANT_1;
const uint16_t FLAG_VOLKSWAGEN_ALT_CRC_VARIANT_1 = 2;
extern const uint16_t FLAG_VOLKSWAGEN_NO_GAS_OFFSET;
const uint16_t FLAG_VOLKSWAGEN_NO_GAS_OFFSET = 4;
extern const uint16_t FLAG_VOLKSWAGEN_ALLOW_LONG_ACCEL_WITH_GAS_PRESSED;
const uint16_t FLAG_VOLKSWAGEN_ALLOW_LONG_ACCEL_WITH_GAS_PRESSED = 8;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_ALC_MODULE;
const uint16_t FLAG_VOLKSWAGEN_PQ_ALC_MODULE = 32;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_LOWLINE;
const uint16_t FLAG_VOLKSWAGEN_PQ_LOWLINE = 64;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_NO_CAM_BUS;
const uint16_t FLAG_VOLKSWAGEN_PQ_NO_CAM_BUS = 128;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_ACC_FTS_EPB;
const uint16_t FLAG_VOLKSWAGEN_PQ_ACC_FTS_EPB = 256;
extern const uint16_t FLAG_VOLKSWAGEN_PQ_SNG_ECD;
const uint16_t FLAG_VOLKSWAGEN_PQ_SNG_ECD = 512;

static uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR

extern bool volkswagen_longitudinal;
bool volkswagen_longitudinal = false;

extern bool volkswagen_alt_crc_variant_1;
bool volkswagen_alt_crc_variant_1 = false;

extern bool volkswagen_no_gas_offset;
bool volkswagen_no_gas_offset = false;

extern bool volkswagen_allow_long_accel_with_gas_pressed;
bool volkswagen_allow_long_accel_with_gas_pressed = false;

extern bool volkswagen_set_button_prev;
bool volkswagen_set_button_prev = false;

extern bool volkswagen_resume_button_prev;
bool volkswagen_resume_button_prev = false;

extern bool volkswagen_brake_pedal_switch;
extern bool volkswagen_brake_pressure_detected;
bool volkswagen_brake_pedal_switch = false;
bool volkswagen_brake_pressure_detected = false;

#define VW_IQ_MAX_LAT_ACCEL   3.0f
#define VW_IQ_MAX_LONG_ACCEL  2000
#define VW_IQ_MIN_LONG_ACCEL  -3500
#define VW_IQ_INACTIVE_LONG_ACCEL 3010
#define VW_IQ_DEG_TO_RAD      0.017453292f

extern float vw_iq_apd_steer_ratio;
extern float vw_iq_apd_wheelbase;
extern bool vw_iq_apd_params_valid;
float vw_iq_apd_steer_ratio = 0.0f;
float vw_iq_apd_wheelbase = 0.0f;
bool vw_iq_apd_params_valid = false;

extern float vw_iq_measured_angle_deg;
float vw_iq_measured_angle_deg = 0.0f;

extern bool vw_iq_aol_active;
bool vw_iq_aol_active = false;

extern bool vw_iq_no_cam;
bool vw_iq_no_cam = false;

extern float vw_iq_angle_offset_deg;
float vw_iq_angle_offset_deg = 0.0f;

extern float vw_iq_alc_desired_angle_deg;
float vw_iq_alc_desired_angle_deg = 0.0f;

extern bool vw_iq_alc_active;
bool vw_iq_alc_active = false;

extern float vw_iq_debug_lat_accel;
float vw_iq_debug_lat_accel = 0.0f;

void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook);
void can_set_checksum(CANPacket_t *packet);

#define MSG_LH_EPS_03        0x09FU   // RX from EPS, for driver steering torque
#define MSG_ESP_19           0x0B2U   // RX from ABS, for wheel speeds
#define MSG_ESP_05           0x106U   // RX from ABS, for brake switch state
#define MSG_TSK_06           0x120U   // RX from ECU, for ACC status from drivetrain coordinator
#define MSG_MOTOR_20         0x121U   // RX from ECU, for driver throttle input
#define MSG_ACC_06           0x122U   // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_HCA_01           0x126U   // TX by OP, Heading Control Assist steering torque
#define MSG_GRA_ACC_01       0x12BU   // TX by OP, ACC control buttons for cancel/resume
#define MSG_ACC_07           0x12EU   // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_ACC_02           0x30CU   // TX by OP, ACC HUD data to the instrument cluster
#define MSG_LDW_02           0x397U   // TX by OP, Lane line recognition and text alerts
#define MSG_MOTOR_14         0x3BEU   // RX from ECU, for brake switch status

// MLB only messages
#define MSG_ESP_03      0x103U   // RX from ABS, for wheel speeds
#define MSG_LS_01       0x10BU   // TX by OP, ACC control buttons for cancel/resume
#define MSG_MOTOR_03    0x105U   // RX from ECU, for driver throttle input and brake switch status
#define MSG_TSK_02      0x10CU   // RX from ECU, for ACC status from drivetrain coordinator
#define MSG_ACC_05      0x10DU   // RX from radar, for ACC status
#define MSG_ACC_01      0x109U   // RX from radar, for ACC status (Audi B8)

static void volkswagen_common_init(void) {
  volkswagen_set_button_prev = false;
  volkswagen_resume_button_prev = false;
  volkswagen_brake_pedal_switch = false;
  volkswagen_brake_pressure_detected = false;
  volkswagen_alt_crc_variant_1 = false;
  volkswagen_no_gas_offset = false;
  volkswagen_allow_long_accel_with_gas_pressed = false;
  vw_iq_apd_steer_ratio = 0.0f;
  vw_iq_apd_wheelbase = 0.0f;
  vw_iq_apd_params_valid = false;
  vw_iq_aol_active = false;
  vw_iq_no_cam = false;
  vw_iq_angle_offset_deg = 0.0f;
  vw_iq_alc_desired_angle_deg = 0.0f;
  vw_iq_alc_active = false;
  vw_iq_measured_angle_deg = 0.0f;
  gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
  return;
}

bool volkswagen_longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) {
  bool accel_valid = controls_allowed &&
                     (volkswagen_allow_long_accel_with_gas_pressed || !gas_pressed_prev) &&
                     !safety_max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
  bool accel_inactive = desired_accel == limits.inactive_accel;
  return !(accel_valid || accel_inactive);
}

static void volkswagen_iq_decode_apd(const CANPacket_t *msg) {
  uint8_t version = (msg->data[1] >> 4) & 0x0FU;
  uint8_t flags = msg->data[2] & 0x0FU;
  if (version == 1U) {
    vw_iq_aol_active = (flags & 0x08U) != 0U;
    uint16_t angle_offset_raw = ((msg->data[5] >> 2) & 0x3FU) | (((uint16_t)msg->data[6] & 0x1FU) << 6);
    vw_iq_angle_offset_deg = (float)angle_offset_raw * 0.01f - 10.0f;
  }
  if ((version == 1U) && (flags & 0x01U)) {
    uint16_t sr_raw = ((msg->data[2] >> 4) & 0x0FU) | (((uint16_t)msg->data[3] & 0x7FU) << 4);
    uint16_t wb_raw = ((msg->data[3] >> 7) & 0x01U) | (((uint16_t)msg->data[4]) << 1) | (((uint16_t)msg->data[5] & 0x03U) << 9);
    vw_iq_apd_steer_ratio = (float)sr_raw * 0.01f + 8.0f;
    vw_iq_apd_wheelbase = ((float)wb_raw + 2000.0f) * 0.001f;
    vw_iq_apd_params_valid = (vw_iq_apd_steer_ratio > 1.0f) && (vw_iq_apd_wheelbase > 1.0f);
  }
}

static bool volkswagen_iq_lat_accel_torque_check(int desired_torque) {
  if (!controls_allowed && !vw_iq_aol_active) {
    vw_iq_debug_lat_accel = 0.0f;
    return desired_torque != 0;
  }

  if (!vw_iq_apd_params_valid) {
    vw_iq_debug_lat_accel = 0.0f;
    return false;
  }

  float speed_ms = (float)(vehicle_speed.min) / VEHICLE_SPEED_FACTOR;
  if (speed_ms < 1.0f) {
    vw_iq_debug_lat_accel = 0.0f;
    return false;
  }

  float abs_angle = vw_iq_measured_angle_deg >= 0.0f ? vw_iq_measured_angle_deg : -vw_iq_measured_angle_deg;
  float angle_rad = abs_angle * VW_IQ_DEG_TO_RAD;
  float curvature = angle_rad / (vw_iq_apd_steer_ratio * vw_iq_apd_wheelbase);
  float lat_accel = curvature * speed_ms * speed_ms;
  vw_iq_debug_lat_accel = lat_accel;

  if (lat_accel > VW_IQ_MAX_LAT_ACCEL) {
    bool torque_positive = desired_torque > 0;
    bool angle_positive = vw_iq_measured_angle_deg > 0.0f;
    if (torque_positive == angle_positive) {
      return true;
    }
  }

  return false;
}

static float volkswagen_iq_angle_to_lat_accel(float angle_deg) {
  float abs_angle = angle_deg >= 0.0f ? angle_deg : -angle_deg;
  float angle_rad = abs_angle * VW_IQ_DEG_TO_RAD;
  float curvature = angle_rad / (vw_iq_apd_steer_ratio * vw_iq_apd_wheelbase);
  float speed_ms = (float)(vehicle_speed.min) / VEHICLE_SPEED_FACTOR;
  return curvature * speed_ms * speed_ms;
}

static bool volkswagen_iq_alc_angle_accel_check(bool require_activation_gate) {
  if (require_activation_gate && !controls_allowed && !vw_iq_aol_active) {
    return true;
  }
  if (!vw_iq_apd_params_valid) {
    return false;
  }
  float speed_ms = (float)(vehicle_speed.min) / VEHICLE_SPEED_FACTOR;
  if (speed_ms < 1.0f) {
    return false;
  }

  const float desired_effective_angle = vw_iq_alc_desired_angle_deg - vw_iq_angle_offset_deg;
  const float actual_effective_angle = vw_iq_measured_angle_deg - vw_iq_angle_offset_deg;
  const float delta_angle = desired_effective_angle - actual_effective_angle;
  const float delta_lat_accel = volkswagen_iq_angle_to_lat_accel(delta_angle);

  vw_iq_debug_lat_accel = delta_lat_accel;

  if (delta_lat_accel <= VW_IQ_MAX_LAT_ACCEL) {
    return false;
  }

  return true;
}

static void volkswagen_iq_send_debug_la(uint32_t debug_addr, uint8_t bus) {
  CANPacket_t msg = {0};
  msg.addr = debug_addr;
  msg.bus = bus;
  msg.data_len_code = 8U;

  uint16_t la_raw = (uint16_t)(vw_iq_debug_lat_accel * 1000.0f);
  float speed_kmh = ((float)(vehicle_speed.min) / VEHICLE_SPEED_FACTOR) * 3.6f;
  uint16_t spd_raw = (uint16_t)(speed_kmh * 100.0f);
  int16_t ang_raw = (int16_t)(vw_iq_measured_angle_deg * 100.0f);
  uint8_t flags = (vw_iq_apd_params_valid ? 0x01U : 0x00U) | (vw_iq_aol_active ? 0x02U : 0x00U) | (vw_iq_no_cam ? 0x04U : 0x00U);

  msg.data[0] = (uint8_t)(la_raw & 0xFFU);
  msg.data[1] = (uint8_t)((la_raw >> 8) & 0xFFU);
  msg.data[2] = (uint8_t)(spd_raw & 0xFFU);
  msg.data[3] = (uint8_t)((spd_raw >> 8) & 0xFFU);
  msg.data[4] = (uint8_t)((uint16_t)ang_raw & 0xFFU);
  msg.data[5] = (uint8_t)(((uint16_t)ang_raw >> 8) & 0xFFU);
  msg.data[6] = flags;
  msg.data[7] = 0U;

  can_set_checksum(&msg);
  can_send(&msg, bus, true);
}

static bool volkswagen_iq_long_accel_check(int desired_accel) {
  if (desired_accel == VW_IQ_INACTIVE_LONG_ACCEL) {
    return false;
  }
  if (!controls_allowed) {
    return true;
  }
  if (gas_pressed_prev && !volkswagen_allow_long_accel_with_gas_pressed) {
    return true;
  }
  return (desired_accel > VW_IQ_MAX_LONG_ACCEL) || (desired_accel < VW_IQ_MIN_LONG_ACCEL);
}

static uint32_t volkswagen_mqb_meb_get_checksum(const CANPacket_t *msg) {
  return (uint8_t)msg->data[0];
}

static uint8_t volkswagen_mqb_meb_get_counter(const CANPacket_t *msg) {
  // MQB/MEB message counters are consistently found at LSB 8.
  return (uint8_t)msg->data[1] & 0xFU;
}

static uint32_t volkswagen_mqb_meb_compute_crc(const CANPacket_t *msg) {
  int len = GET_LEN(msg);

  // This is CRC-8H2F/AUTOSAR with a twist. See the iqdbc/car/volkswagen/ implementation
  // of this algorithm for a version with explanatory comments.

  uint8_t crc = 0xFFU;
  for (int i = 1; i < len; i++) {
    crc ^= (uint8_t)msg->data[i];
    crc = volkswagen_crc8_lut_8h2f[crc];
  }

  uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
  if (msg->addr == MSG_LH_EPS_03) {
    crc ^= (uint8_t[]){0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}[counter];
  } else if (msg->addr == MSG_ESP_05) {
    crc ^= (uint8_t[]){0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07}[counter];
  } else if (msg->addr == MSG_TSK_06) {
    crc ^= (uint8_t[]){0xC4, 0xE2, 0x4F, 0xE4, 0xF8, 0x2F, 0x56, 0x81, 0x9F, 0xE5, 0x83, 0x44, 0x05, 0x3F, 0x97, 0xDF}[counter];
  } else if (msg->addr == MSG_MOTOR_20) {
    crc ^= (uint8_t[]){0xE9, 0x65, 0xAE, 0x6B, 0x7B, 0x35, 0xE5, 0x5F, 0x4E, 0xC7, 0x86, 0xA2, 0xBB, 0xDD, 0xEB, 0xB4}[counter];
  } else if (msg->addr == MSG_GRA_ACC_01) {
    crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter];
  } else {
    // Undefined CAN message, CRC check expected to fail
  }
  crc = volkswagen_crc8_lut_8h2f[crc];

  return (uint8_t)(crc ^ 0xFFU);
}

static int volkswagen_mlb_mqb_driver_input_torque(const CANPacket_t *msg) {
  // Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
  // Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
  int torque_driver_new = msg->data[5] | ((msg->data[6] & 0x1FU) << 8);
  bool sign = GET_BIT(msg, 55U);
  if (sign) {
    torque_driver_new *= -1;
  }
  return torque_driver_new;
}

static int volkswagen_mlb_mqb_steering_control_torque(const CANPacket_t *msg) {
  // Signal: HCA_01.HCA_01_LM_Offset (absolute torque)
  // Signal: HCA_01.HCA_01_LM_OffSign (direction)
  int desired_torque = msg->data[2] | ((msg->data[3] & 0x1U) << 8);
  bool sign = GET_BIT(msg, 31U);
  if (sign) {
    desired_torque *= -1;
  }
  return desired_torque;
}