car.cpp

#include "car.h"
#include "config.h"

#include <pthread.h>
#include <chrono>
#include <iostream>
#include "../teensy/CarMessages.h"
#include "async-buf.h"
#include "json.hpp"
#include "logger.h"
#include "lookup-table.h"
#include "socket-server.h"
#include "string-utils.h"
using namespace std;
using namespace std::chrono;
using namespace std::chrono_literals;

#include "diagnostics.h"

Car::Car(bool online) : 
  usb_queue(1) // set to 1 to get the latest only
{
  log_entry_exit("Car::Car");
  log_info("reading configuration");
  read_configuration(config_path);
  front_right_wheel.meters_per_tick = this->meters_per_odometer_tick;
  front_left_wheel.meters_per_tick = this->meters_per_odometer_tick;
  motor.meters_per_tick = this->motor_meters_per_odometer_tick;
  log_info("all wheels set");
  this->online = online;
  reset_odometry();
  if (online) {
    connect_usb();
    start_web_server();
    connect_lidar();
    start_socket();
    camera.warm_up();
  }
}

Car::~Car() {
  quit = true;
  if (usb_thread.joinable()) usb_thread.join();
  if (lidar_thread.joinable()) lidar_thread.join();
}

void Car::connect_lidar() {
  lidar_thread = thread(&Car::lidar_thread_start, this);
  pthread_setname_np(lidar_thread.native_handle(), "car-lidar");
}

void Car::connect_usb() {
  usb_thread = thread(&Car::usb_thread_start, this);
  pthread_setname_np(usb_thread.native_handle(), "car-usb");
}

void Car::start_socket() {
  socket_thread = thread(&Car::socket_thread_start, this);
  pthread_setname_np(socket_thread.native_handle(), "car-socket");
}

void Car::start_web_server() {
  web_server_thread = thread(&Car::web_server_thread_start, this);
  pthread_setname_np(web_server_thread.native_handle(), "car-web-server");

}

string Car::get_mode() {
  if (rc_mode_enabled) return "automatic";

  if (command_from_socket == "record") return "recording";

  return "manual";
}

LidarScan Car::get_latest_scan() const {
  lock_guard<mutex> lock(recent_scans_mutex);
  if(recent_scans.size() > 0) {
    return recent_scans.front();
  }
  return LidarScan();
}

string Car::get_scan_json(int scan_to_skip) {
    last_scan_request_time = system_clock::now();

  // lidar.get_scan();
  if (recent_scans.size() > 0) {
    auto wait_end_time = system_clock::now() + milliseconds(200);
    while (system_clock::now() < wait_end_time) {
      int scan_number;
      {
        lock_guard<mutex> lock(recent_scans_mutex);
        scan_number = recent_scans.front().scan_number;
      }
      if (scan_number != scan_to_skip) {
        string result;
        try {
          LidarScan scan;
          {
            lock_guard<mutex> lock(recent_scans_mutex);
            scan = recent_scans.front();
          }
          result = scan.get_json().dump();
        } catch (...) {
          log_error("exception caught getting scan json");
        }

        return result;
        break;
      } else {
        this_thread::sleep_for(chrono::milliseconds(1));
      }
    }
  }

  // not found
  if(scan_to_skip==0) {
    this_thread::sleep_for(chrono::milliseconds(500));
  }
  LidarScan fake;
  return fake.get_json().dump();
}

void Car::socket_get_scan(vector<string>& params) {
  int scan_to_skip = -1;
  if (params.size() > 1) {
    try {
      scan_to_skip = atoi(params[1].c_str());
    } catch (...) {
      log_warning("invalid scan number requested");
    }
  }

  bool found = false;

  // turn on lidar if required
  if (!lidar.is_running) {
    lidar.motor_on();
  }
  last_scan_request_time = system_clock::now();

  // lidar.get_scan();
  if (recent_scans.size() > 0) {
    auto wait_end_time = system_clock::now() + milliseconds(200);
    while (!found && system_clock::now() < wait_end_time) {
      int scan_number;
      {
        lock_guard<mutex> lock(recent_scans_mutex);
        scan_number = recent_scans.front().scan_number;
      }
      if (scan_number != scan_to_skip) {
        found = true;
        string result;
        try {
          LidarScan scan;
          {
            lock_guard<mutex> lock(recent_scans_mutex);
            scan = recent_scans.front();
          }
          result = scan.get_json().dump();
          // result = "{\"angle\":[0.0,0.017453292519943295,0.03490658503988659,0.05235987755982988,0.06981317007977318,0.08726646259971647,0.10471975511965977,0.12217304763960307,0.13962634015954636,0.15707963267948966,0.17453292519943295,0.19198621771937624,0.20943951023931953,0.22689280275926285,0.24434609527920614,0.2617993877991494,0.2792526803190927,0.29670597283903605,0.3141592653589793,0.3316125578789226,0.3490658503988659,0.3665191429188092,0.3839724354387525,0.40142572795869574,0.41887902047863906,0.4363323129985824,0.4537856055185257,0.47123889803846897,0.4886921905584123,0.5061454830783556,0.5235987755982988,0.5410520681182421,0.5585053606381855,0.5759586531581288,0.5934119456780721,0.6108652381980153,0.6283185307179586,0.6457718232379019,0.6632251157578452,0.6806784082777885,0.6981317007977318,0.715584993317675,0.7330382858376184,0.7504915783575616,0.767944870877505,0.7853981633974483,0.8028514559173915,0.8203047484373349,0.8377580409572781,0.8552113334772214,0.8726646259971648,0.890117918517108,0.9075712110370514,0.9250245035569946,0.9424777960769379,0.9599310885968813,0.9773843811168246,0.9948376736367678,1.0122909661567112,1.0297442586766543,1.0471975511965976,1.064650843716541,1.0821041362364843,1.0995574287564276,1.117010721276371,1.1344640137963142,1.1519173063162575,1.1693705988362006,1.1868238913561442,1.2042771838760873,1.2217304763960306,1.239183768915974,1.2566370614359172,1.2740903539558606,1.2915436464758039,1.3089969389957472,1.3264502315156903,1.3439035240356338,1.361356816555577,1.3788101090755203,1.3962634015954636,1.413716694115407,1.43116998663535,1.4486232791552935,1.4660765716752369,1.4835298641951802,1.5009831567151233,1.5184364492350666,1.53588974175501,1.5533430342749535,1.5707963267948966,1.5882496193148399,1.605702911834783,1.6231562043547263,1.6406094968746698,1.6580627893946132,1.6755160819145563,1.6929693744344996,1.710422666954443,1.7278759594743864,1.7453292519943295,1.7627825445142729,1.780235837034216,1.7976891295541593,1.8151424220741028,1.8325957145940461,1.8500490071139892,1.8675022996339325,1.8849555921538759,1.902408884673819,1.9198621771937625,1.9373154697137058,1.9547687622336491,1.9722220547535922,1.9896753472735356,2.007128639793479,2.0245819323134224,2.0420352248333655,2.0594885173533086,2.076941809873252,2.0943951023931953,2.111848394913139,2.129301687433082,2.1467549799530254,2.1642082724729685,2.1816615649929116,2.199114857512855,2.2165681500327987,2.234021442552742,2.251474735072685,2.2689280275926285,2.286381320112572,2.303834612632515,2.321287905152458,2.3387411976724013,2.356194490192345,2.3736477827122884,2.3911010752322315,2.4085543677521746,2.426007660272118,2.443460952792061,2.4609142453120043,2.478367537831948,2.4958208303518914,2.5132741228718345,2.5307274153917776,2.548180707911721,2.5656340004316647,2.5830872929516078,2.600540585471551,2.6179938779914944,2.6354471705114375,2.6529004630313806,2.670353755551324,2.6878070480712677,2.705260340591211,2.722713633111154,2.740166925631097,2.7576202181510405,2.775073510670984,2.792526803190927,2.8099800957108703,2.827433388230814,2.844886680750757,2.8623399732707,2.8797932657906435,2.897246558310587,2.9146998508305306,2.9321531433504737,2.949606435870417,2.9670597283903604,2.9845130209103035,3.0019663134302466,3.01941960595019,3.036872898470133,3.0543261909900763,3.07177948351002,3.0892327760299634,3.106686068549907,3.12413936106985,3.141592653589793,3.159045946109736,3.1764992386296798,3.193952531149623,3.211405823669566,3.2288591161895095,3.2463124087094526,3.2637657012293966,3.2812189937493397,3.2986722862692828,3.3161255787892263,3.3335788713091694,3.3510321638291125,3.368485456349056,3.385938748868999,3.4033920413889422,3.420845333908886,3.438298626428829,3.455751918948773,3.473205211468716,3.490658503988659,3.5081117965086026,3.5255650890285457,3.543018381548489,3.560471674068432,3.5779249665883754,3.5953782591083185,3.6128315516282616,3.6302848441482056,3.6477381366681487,3.6651914291880923,3.6826447217080354,3.7000980142279785,3.717551306747922,3.735004599267865,3.752457891787808,3.7699111843077517,3.787364476827695,3.804817769347638,3.822271061867582,3.839724354387525,3.8571776469074686,3.8746309394274117,3.8920842319473548,3.9095375244672983,3.9269908169872414,3.9444441095071845,3.9618974020271276,3.979350694547071,3.9968039870670142,4.014257279586958,4.031710572106902,4.049163864626845,4.066617157146788,4.084070449666731,4.101523742186674,4.118977034706617,4.136430327226561,4.153883619746504,4.171336912266447,4.1887902047863905,4.2062434973063345,4.223696789826278,4.241150082346221,4.258603374866164,4.276056667386107,4.293509959906051,4.310963252425994,4.328416544945937,4.34586983746588,4.363323129985823,4.380776422505767,4.39822971502571,4.4156830075456535,4.4331363000655974,4.4505895925855405,4.468042885105484,4.485496177625427,4.50294947014537,4.520402762665313,4.537856055185257,4.5553093477052,4.572762640225144,4.590215932745087,4.60766922526503,4.625122517784973,4.642575810304916,4.6600291028248595,4.677482395344803,4.694935687864747,4.71238898038469,4.729842272904633,4.747295565424577,4.76474885794452,4.782202150464463,4.799655442984406,4.817108735504349,4.834562028024293,4.852015320544236,4.869468613064179,4.886921905584122,4.9043751981040655,4.921828490624009,4.939281783143953,4.956735075663896,4.97418836818384,4.991641660703783,5.009094953223726,5.026548245743669,5.044001538263612,5.061454830783555,5.078908123303498,5.096361415823442,5.113814708343385,5.131268000863329,5.1487212933832724,5.1661745859032155,5.183627878423159,5.201081170943102,5.218534463463045,5.235987755982989,5.253441048502932,5.270894341022875,5.288347633542818,5.305800926062761,5.323254218582705,5.340707511102648,5.358160803622591,5.375614096142535,5.3930673886624785,5.410520681182422,5.427973973702365,5.445427266222308,5.462880558742251,5.480333851262194,5.497787143782138,5.515240436302081,5.532693728822025,5.550147021341968,5.567600313861911,5.585053606381854,5.602506898901797,5.6199601914217405,5.6374134839416845,5.654866776461628,5.672320068981571,5.689773361501514,5.707226654021458,5.7246799465414,5.742133239061344,5.759586531581287,5.777039824101231,5.794493116621174,5.811946409141117,5.829399701661061,5.8468529941810035,5.8643062867009474,5.88175957922089,5.899212871740834,5.916666164260777,5.934119456780721,5.951572749300663,5.969026041820607,5.986479334340551,6.003932626860493,6.021385919380437,6.03883921190038,6.056292504420323,6.073745796940266,6.09119908946021,6.108652381980153,6.126105674500097,6.14355896702004,6.161012259539983,6.178465552059927,6.19591884457987,6.213372137099814,6.230825429619756,6.2482787221397,6.265732014659642],\"distance_meters\":[1.3,1.292,null,1.278,1.269,1.264,1.257,1.253,1.247,1.243,1.239,1.234,1.231,1.228,1.225,1.223,1.22,1.218,1.216,1.215,1.216,1.216,1.215,1.216,1.219,1.221,1.224,1.227,1.228,1.233,1.237,1.241,1.243,1.25,1.255,1.26,1.266,1.274,1.279,1.287,1.294,1.302,1.312,1.322,1.331,1.34,1.351,1.362,1.374,1.386,null,1.36,1.318,1.285,1.249,1.219,1.187,1.159,1.13,1.106,null,null,null,null,0.618,null,null,null,0.933,0.919,0.905,0.891,0.88,0.867,0.855,0.844,0.834,0.825,0.815,0.806,0.797,0.788,0.781,0.773,0.766,0.758,0.752,0.746,0.74,0.734,0.729,0.724,0.724,null,null,null,null,null,null,null,null,null,null,null,0.685,0.681,0.68,0.679,0.678,0.677,0.677,0.676,0.675,0.676,0.677,0.677,0.677,0.68,null,0.675,0.393,0.394,0.395,null,null,null,null,null,0.702,0.705,null,null,0.492,null,null,0.732,0.737,0.743,0.75,0.756,0.763,0.767,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,0.875,1.734,1.808,1.899,1.99,2.105,2.225,2.369,2.512,2.696,2.804,null,null,1.219,1.216,1.213,1.211,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,4.201,4.145,null,null,2.792,2.757,2.759,2.778,2.776,2.809,2.824,null,0.83,0.784,0.775,0.756,0.75,0.749,0.749,0.749,0.747,0.737,0.727,0.691,0.691,0.689,0.688,0.701,0.686,0.695,0.694,0.689,0.695,0.714,0.714,0.688,0.686,0.68,0.683,0.664,0.668,0.674,0.674,0.684,0.698,0.721,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,4.086,4.09,4.168,null,null,null,null,null,null,null,null,null,2.476,2.466,2.401,2.313,2.244,2.191,2.135,2.085,2.032,1.986,null,null,null,null,null,1.765,1.729,1.7,1.673,1.644,1.62,1.593,1.57,1.548,1.531,1.506,1.489,1.47,1.454,1.435,1.42,1.406,1.391,1.378,1.364,1.353,1.34,1.329,1.322,1.309],\"number\":"
          // +to_string(scan_number)
          // +",\"pose_theta\":[2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.7867672443389893,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087,2.786592721939087],\"pose_x\":[0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.2031012773513794,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319,0.20310856401920319],\"pose_y\":[-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.0751250684261322,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931,-0.07510543614625931],\"signal_strength\":[191.0,189.0,null,157.0,195.0,192.0,193.0,199.0,201.0,206.0,199.0,199.0,192.0,194.0,198.0,204.0,212.0,203.0,212.0,196.0,202.0,194.0,197.0,206.0,215.0,203.0,200.0,203.0,201.0,197.0,198.0,212.0,200.0,201.0,195.0,199.0,197.0,186.0,192.0,193.0,192.0,191.0,187.0,186.0,191.0,192.0,191.0,194.0,178.0,175.0,null,131.0,137.0,149.0,153.0,169.0,170.0,186.0,185.0,49.0,null,null,null,null,214.0,null,null,null,253.0,268.0,270.0,277.0,283.0,280.0,298.0,305.0,313.0,328.0,326.0,323.0,338.0,342.0,354.0,342.0,357.0,371.0,375.0,383.0,376.0,394.0,380.0,385.0,31.0,null,null,null,null,null,null,null,null,null,null,null,428.0,463.0,456.0,437.0,460.0,452.0,442.0,461.0,465.0,442.0,438.0,466.0,448.0,340.0,null,6.0,484.0,469.0,488.0,null,null,null,null,null,436.0,429.0,null,null,109.0,null,null,386.0,383.0,380.0,379.0,388.0,366.0,25.0,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,43.0,66.0,65.0,54.0,41.0,46.0,34.0,25.0,20.0,17.0,72.0,null,null,73.0,56.0,63.0,7.0,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,7.0,7.0,null,null,11.0,56.0,52.0,48.0,53.0,62.0,51.0,null,18.0,288.0,314.0,208.0,342.0,331.0,335.0,364.0,362.0,344.0,341.0,215.0,229.0,187.0,284.0,292.0,283.0,419.0,332.0,391.0,228.0,421.0,317.0,187.0,333.0,221.0,421.0,424.0,392.0,430.0,289.0,287.0,347.0,250.0,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,null,22.0,30.0,24.0,null,null,null,null,null,null,null,null,null,33.0,84.0,41.0,49.0,57.0,55.0,65.0,74.0,71.0,80.0,null,null,null,null,null,90.0,106.0,104.0,113.0,119.0,110.0,147.0,139.0,150.0,153.0,151.0,152.0,146.0,164.0,161.0,155.0,165.0,170.0,167.0,178.0,181.0,179.0,188.0,194.0,181.0]}";
        } catch (...) {
          log_error("exception caught getting scan json");
        }

        socket_server.send_response(result);
        break;
      } else {
        this_thread::sleep_for(chrono::milliseconds(1));
      }
    }
  }
  if (!found) {
    if(scan_to_skip==0) {
      this_thread::sleep_for(chrono::milliseconds(500));
    }
    LidarScan fake;
    socket_server.send_response(fake.get_json().dump());
  }
}


void Car::process_socket() {
  //static PerformanceData data("Car::process_socket");
  //MethodTracker tracker(data);

  while (true) {
      
    string full_request = socket_server.get_request();
    if (full_request.length() == 0) return;
    auto params = split(full_request, ',');
    string request = params[0];
    if (request == "get_scan") {
      socket_get_scan(params);
    } else if (request == "get_state") {
      nlohmann::json j;

      j["velocity"] = get_velocity();
      j["v_bat"] = get_voltage();
      // https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Register-Map.pdf
      // p.6
      j["temperature"] =
          get_temperature() / 340.0 +
          35.0;  // constants from https://playground.arduino.cc/Main/MPU-9150
      // j["temperature"] = (get_temperature() + 12412.0)/340.0; // constants
      // from https://playground.arduino.cc/Main/MPU-9150
      j["mode"] = get_mode();
      j["heading"] = get_heading().radians();
      j["fl"] = get_front_left_wheel().get_json_state();
      j["fr"] = get_front_right_wheel().get_json_state();
      j["motor"] = get_motor().get_json_state();
      j["front_x"] = get_front_position().x;
      j["front_y"] = get_front_position().y;
      j["go_enabled"] = get_go_enabled();

      auto s = j.dump();
      socket_server.send_response(s);
    } else if (request == "go") {
      log_info("go");
      command_from_socket = "go";
      socket_server.send_response("ok");
    } else if (request == "stop") {
      command_from_socket = "stop";
      log_info("stop requested from socket");
      set_manual_mode();
      socket_server.send_response("ok");
    } else if (request == "record") {
      command_from_socket = ("record");
      log_info("record requested from socket");
      set_manual_mode();
      socket_server.send_response("ok");
    } else if (request == "reset_odometer") {
      log_info("reset_odometer requested from socket");
      reset_odometry();
      socket_server.send_response("ok");
    } else {
      socket_server.send_response("Error, unknown command: " + request);
    }
  }
}

void Car::lidar_thread_start() {
  // was 6% cpu time
  log_entry_exit w("lidar thread");
  lidar.run();
  while (!quit) {
    try {
      bool got_scan = lidar.try_get_scan(1);
      if (got_scan) {
        int scan_number;
        {
          // lock_guard<std::mutex> lock(recent_scans_mutex);
          recent_scans.emplace_front(lidar.current_scan);
          scan_number = lidar.current_scan.scan_number;
          while (recent_scans.size() > 10) {
            recent_scans.pop_back();
          }
        }
        // bug: omitting this line, or log_info crashes startup
        // this_thread::sleep_for(chrono::milliseconds(1));
        // log_info("got scan " + to_string(scan_number));
      }
      else {
        this_thread::sleep_for(chrono::milliseconds(1));
      }
    } catch (string error_string) {
      log_error("error caught in lidar_thread_start" + error_string);
    } catch (...) {
      log_error("error caught in lidar_thread_start");
    }
  }
  if (quit) {
    log_info("lidar_thread exiting because quit == true");
  }
}

void Car::usb_thread_start() {
  try {
    log_entry_exit w("usb thread");
    usb.add_line_listener(&usb_queue);
    usb.write_on_connect("\ntd+\n");

    // simlink to /dev/ttyA* enabled by adding the
    // following to the end of /etc/udev/rules.d/49-teensy.rules
    // ATTRS{idVendor}=="16c0", , ATTRS{idProduct}=="04[789B]?",
    // SYMLINK+="teensy$attr{serial}"

    string device_path;
#if defined(blue_car)
    device_path = "/dev/teensy4317960"; // for blue-crash
#elif defined(orange_car)
    device_path = "/dev/teensy1301550"; // orange-crash
#else
#error "Unknown robot"
#endif
    usb.run(device_path);
    
    while (!quit) {
      try {
        static uint32_t processed_count = 0;
        StampedString line;
        if (usb_queue.try_pop(line, 1)) {
          log_warning_if_duration_exceeded w("processing usb line", 10ms);
          size_t remaining = usb_queue.size();
          if(remaining > 0) {
            static uint32_t count = 0;
            count++;
            if(count%100 == 1) {
              log_warning("car usb queue not empty after pop, remaining:  "+to_string(remaining) + " count: " + to_string(count));
            }
            //while(usb_queue.size() > 0) {
            //  // take off excess lines
            //  StampedLine l2;
            //  usb_queue.try_pop(l2, 0);
            //}
          }
          process_line_from_log(line);
          ++processed_count;
        }
        if (online && lidar.is_running &&
            last_scan_request_time + 10s < system_clock::now()) {
          lidar.motor_off();
        }
      } catch (string error_string) {
        log_error("error caught in usb loop" + error_string);
      } catch (std::exception& e) {
        log_error((string) "std::exception caught in usb loop" + e.what());
      } catch (...) {
        log_error("error caught in usb loop");
      }
    }
  } catch (string error_string) {
    log_error("error caught in usb_thread_start" + error_string);
  } catch (...) {
    log_error("unknown error caught in usb_thread_start");
  }
}

void Car::socket_thread_start() {
  try {
    socket_server.open_socket(5571);

    while (!quit) {
      process_socket();
      this_thread::sleep_for(chrono::milliseconds(5));
    }
  } catch (string error_string) {
    log_error("error caught in socket_thread_start" + error_string);
  } catch (...) {
    log_error("unknown error caught in socket_thread_start");
  }
}

void Car::get_state_handler(const Request &, Response & response) {
      // todo: make static?
      nlohmann::json j;

      j["velocity"] = get_velocity();
      j["v_bat"] = get_voltage();
      // https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Register-Map.pdf
      // p.6
      j["temperature"] =
          get_temperature() / 340.0 +
          35.0;  // constants from https://playground.arduino.cc/Main/MPU-9150
      // j["temperature"] = (get_temperature() + 12412.0)/340.0; // constants
      // from https://playground.arduino.cc/Main/MPU-9150
      j["mode"] = get_mode();
      j["heading"] = get_heading().radians();
      j["fl"] = get_front_left_wheel().get_json_state();
      j["fr"] = get_front_right_wheel().get_json_state();
      j["motor"] = get_motor().get_json_state();
      j["front_x"] = get_front_position().x;
      j["front_y"] = get_front_position().y;
      j["go_enabled"] = get_go_enabled();

      auto s = j.dump();
      response.write_content("application/json", s.c_str(), s.size());
}

void Car::get_scan_handler(const Request &request, Response & response) {
  auto it = request.params.find("since");
  int since;
  if(it != request.params.end()) {
    since = atoi(it->second.c_str());
  } else {
    since = -1;
  }
  string s = get_scan_json(since);
  response.write_content("application/json", s.c_str(), s.size());
}

void index_handler(const Request &, Response & response) {
  string content = "<html><body>Hello from car.cpp</body></html>\r\n";
  response.write_content("text/html", content.c_str(), content.size());
}

void Car::video_handler(const Request &request, Response & response) {
  const bool trace = false;
  pthread_setname_np(pthread_self(), "car-vid-handler");
  WorkQueue<cv::Mat> queue{1};
  int camera_number = 0;
  auto it = request.params.find("camera_number");
  if(it != request.params.end()) {
    camera_number = atoi(it->second.c_str());
  }
  if(camera.cameras.size() < camera_number + 1) {
    response.write_status(404, "camera number not found");
    response.end();
    return;
  }

  camera.cameras[camera_number]->grabber.frames_topic.add_listener(&queue);
  //camera.left_camera.grabber.frames_topic.add_listener(&queue);
  try {

  //pthread_setname_np(pthread_self(), "car-vid-handler");
  cout << "video_handler" << endl;
  response.enable_multipart();
  cv::Mat frame;
  while(!response.is_closed()) {
    static int frame_number;
    if(trace) cout << "writing a frame of video" << endl;
    cv::Mat grabber_frame;
    if(!queue.try_pop(grabber_frame, 1000)) {
      if(response.is_closed()) {
        cout << "leaving video_handler, client closed connection" << endl;
        break;
      }
      continue;
    } 
    if(frame.empty()) {
      frame = grabber_frame.clone();
    } else {
      grabber_frame.copyTo(frame);
    }


    ++frame_number;
    string text = "Sent frame number: " + to_string(frame_number);
    cv::putText(frame, 
                text.c_str(),
                cv::Point(50,50), // Coordinates
                cv::FONT_HERSHEY_COMPLEX_SMALL, // Font
                1.0, // Scale. 2.0 = 2x bigger
                cv::Scalar(255,255,255), // BGR Color
                1);//, // Line Thickness (Optional)
                //cv::CV_AA); // Anti-alias (Optional)

    std::vector<uchar> buff;//buffer for coding
    std::vector<int> param(2);
    param[0] = cv::IMWRITE_JPEG_QUALITY;
    param[1] = 80;//default(95) 0-100
    cv::imencode(".jpg", frame, buff, param);


    int bytes_pending = response.bytes_pending();
    while(bytes_pending > 0 && ! response.is_closed()) {
      cout << "bytes pending: " << bytes_pending << " waiting to send next video frame" << endl;
      usleep(1000);
      bytes_pending = response.bytes_pending();
    }

    response.write_content("image/jpeg", (char *)&buff[0], buff.size());
  }
  } catch (...) {
    log_error("unknown error in video_handler");
  }
  camera.cameras[camera_number]->grabber.frames_topic.remove_listener(&queue);
}


void Car::web_server_thread_start() {
  try {
    log_info("Starting web server");
    WebServer server;
    server.add_handler("GET", "/", index_handler);
    server.add_handler("GET", "/get_state", [this](const Request & request, Response & response) {
      this->get_state_handler(request, response);
    });
    server.add_handler("GET", "/get_scan", [this](const Request & request, Response & response) {
      this->get_scan_handler(request, response);
    });
    server.add_handler("GET", "/video", [this](const Request & request, Response & response) {
      this->video_handler(request, response);
    });
    server.run(8081);
  }
  catch(string s) {
    log_error("Exception caught in web server"+s);
  }

}

// returns true if the line is a valid TD
bool Car::process_line_from_log(const StampedString& msg) {
  if (input_recording_file) {
    log_warning_if_duration_exceeded w("write input_recording_file", 2ms);
    *input_recording_file << msg.to_string() << '\n';  // todo: make
                                                       // non-blocking
  }
  auto split_message = split(msg.message, ',', false);
  if (split_message.size() < 2) return false;

  string message_type = split_message[0];
  string body = trimmed(split_message[1]);
  if (message_type != "TD2") {
    return false;
  }
  Dynamics2 d;
  StringInTransfer stream(body.c_str());
  d.transfer(stream);
  if (stream.ok) {
    // log_info((string)"got td ms: " + to_string(d.ms));
    apply_dynamics(d);
    dynamics2_topic.send(d);
  } else {
    ++usb_error_count;
    log_warning((string) "bad dynamics: " + msg.to_string());
    log_warning((string) "message: " + stream.error_message);
  }
  return stream.ok;
}

/*
  Dynamics td;
  bool ok = Dynamics::from_log_string(d,msg);
  if(ok) {
    // log_info((string)"got td ms: " + to_string(d.ms));
    apply_dynamics(d);
    {
      lock_guard<mutex> lock(listeners_mutex);
      for(auto listener:listeners) {
        listener->push(d);
      }
    }
  }
  else {
    ++usb_error_count;
    log_warning((string) "bad dynamics: " + msg.to_string());
  }
  return ok;
*/

void Car::send_command(string command) { usb.write_line(command); }

void Car::set_rc_mode() {
  rc_mode_enabled = true;
  send_command("rc");
}

void Car::set_manual_mode() {
  rc_mode_enabled = false;
  send_command("m");
}

void Car::set_esc_and_str(unsigned esc, unsigned str) {
  if (!rc_mode_enabled) {
    throw string("autonomous mode disabled");
  }
  send_command((string) "pse " + to_string(str) + "," + to_string(esc));
  commanded_esc = esc;
  commanded_str = str;
}

void Car::beep() { send_command((string) "beep"); }

void Car::begin_recording_state(string path) {
  end_recording_state();
  this->state_buf.reset(new async_buf(path));
  state_recording_file.reset(new ostream(state_buf.get()));

  //*state_recording_file << Dynamics::csv_field_headers();
  *state_recording_file << "v_smooth,a_smooth,v_fl_smooth,a_fl_smooth,v_fr_"
                           "smooth,a_fr_smooth,commanded_esc,commanded_str"
                        << endl;
}

void Car::write_state() {
  log_warning_if_duration_exceeded w("write_state", 10ms);
  if (state_recording_file) {
    *state_recording_file /*<< current_dynamics.csv_fields()*/ << ","
                          << get_velocity() << "," << get_smooth_acceleration()
                          << "," << front_left_wheel.get_smooth_velocity()
                          << "," << front_left_wheel.get_smooth_acceleration()
                          << "," << front_right_wheel.get_smooth_velocity()
                          << "," << front_right_wheel.get_smooth_acceleration()
                          << commanded_esc << "," << commanded_str << '\n';
  }
}

void Car::end_recording_state() {
  state_recording_file.reset(nullptr);
  state_buf.reset(nullptr);  // causes flush
}

void Car::begin_recording_input(string path) {
  end_recording_input();
  this->input_recording_buf.reset(new async_buf(path));
  input_recording_file.reset(new ostream(input_recording_buf.get()));
}

void Car::end_recording_input() {
  input_recording_file.reset(nullptr);
  input_recording_buf.reset(nullptr);  // causes flush
}

void Car::apply_dynamics(Dynamics2& d) {
  log_warning_if_duration_exceeded w("apply_dynamics", 10ms);
  // set all the dynamics variables
  reading_count++;

  Dynamics2 previous = current_dynamics;
  current_dynamics = d;
  if (reading_count == 1) {
    original_dynamics = d;
    ackerman = Ackerman(front_wheelbase_width, wheelbase_length, Point(0, 0),
                        get_heading());  // ackerman needs first heading reading
    return;
  }

  // update state
  if (d.mode == "manual" && previous.mode != "manual") {
    rc_mode_enabled = false;
  }

  // correct heading with adjustment factor
  //Angle d_theta = Angle::degrees(d.mpu_deg_yaw - previous.mpu_deg_yaw);
  //d_theta.standardize();
  // heading_adjustment += Angle::radians(d_theta.radians() *
  // gyro_adjustment_factor);

  // if wheels have moved, update ackerman
  front_left_wheel.update_from_sensor(d.us, d.odo_fl_a, d.odo_fl_a_us,
                                      d.odo_fl_b, d.odo_fl_b_us);
  motor.update_from_sensor(d.us, d.spur_odo, d.spur_us);
  double wheel_distance_meters = front_right_wheel.update_from_sensor(
      d.us, d.odo_fr_a, d.odo_fr_a_us, d.odo_fr_b, d.odo_fr_b_us);

  if (reading_count > 2 &&
      fabs(wheel_distance_meters) >
          0.) {  // adding 2 keeps out the big jump after a reset
    Angle outside_wheel_angle = angle_for_steering(previous.rx_str);
    ackerman.move_right_wheel(outside_wheel_angle, wheel_distance_meters,
                              get_heading().radians());
  }

  // tell lidar we have moved
  {
    Point p = get_rear_position();
    float lidar_d = 0.12; // meters from back
    float theta = get_heading().radians();
    lidar.set_pose(p.x + lidar_d * cos(theta), p.y + lidar_d * sin(theta), theta);
  }
  write_state();
}

void Car::add_listener(WorkQueue<Dynamics2>* listener) {
  dynamics2_topic.add_listener(listener);
}

void Car::remove_listener(WorkQueue<Dynamics2>* listener) {
  dynamics2_topic.remove_listener(listener);
}

void Car::read_configuration(string path) {
  Config config;
  config.load_from_file(path);

  log_info("reading odometery");

  // odometery
  meters_per_odometer_tick = config.get_double("meters_per_odometer_tick");
  rear_meters_per_odometer_tick =
      config.get_double("rear_meters_per_odometer_tick");
  motor_meters_per_odometer_tick =
      config.get_double("motor_meters_per_odometer_tick");
  gyro_adjustment_factor = config.get_double("gyro_adjustment_factor");

  // esc and steering
  center_steering_us = config.get_double("center_steering_us");
  min_forward_esc = config.get_int("min_forward_esc");
  min_reverse_esc = config.get_int("min_reverse_esc");
  reverse_center_steering_us = config.get_int("reverse_center_steering_us");

  // car dimensions
  front_wheelbase_width = config.get_double("front_wheelbase_width_in_meters");
  rear_wheelbase_width = config.get_double("rear_wheelbase_width_in_meters");
  wheelbase_length = config.get_double("wheelbase_length_in_meters");

  log_info("configuration set");
}

void Car::reset_odometry(double start_offset) {
  // dynamics = Dynamics();
  original_dynamics = current_dynamics;

  set_zero_heading();

  ackerman = Ackerman(front_wheelbase_width, wheelbase_length,
                      Point(0, start_offset), Angle::degrees(0));
}

void Car::set_zero_heading() { 
  if(!isnan(current_dynamics.mpu_deg_yaw)) {
    zero_heading_angle = Angle::degrees(current_dynamics.mpu_deg_yaw);
  } else {
    zero_heading_angle = Angle::degrees(0);
  }
}

Angle Car::get_zero_heading() const { 
  return zero_heading_angle;
}

Angle Car::get_heading() const {
  Angle a = Angle::degrees(current_dynamics.mpu_deg_yaw) - zero_heading_angle;
  return a;
}

double Car::get_acceleration() { return current_dynamics.ax; }

double Car::get_smooth_acceleration() {
  return (get_front_left_wheel().get_smooth_acceleration() +
          get_front_right_wheel().get_smooth_acceleration()) /
         2.0;
}

double Car::get_time_in_seconds() {
  return current_dynamics.ms * 1.0E-3 + (current_dynamics.us % 1000) * 1.0E-6;
}

int Car::steering_for_curvature(Angle theta_per_meter) {
  static const LookupTable t({{-85.1, 1929},
                              {-71.9, 1839},
                              {-58.2, 1794},
                              {-44.1, 1759},
                              {-29.6, 1678},
                              {-14.8, 1599},
                              {0, 1521},
                              {14.8, 1461},
                              {29.6, 1339},
                              {44.0, 1306},
                              {58.2, 1260},
                              {71.9, 1175},
                              {85.1, 1071}

  });
  return (int)t.lookup(theta_per_meter.degrees());
}

int Car::steering_for_angle(Angle theta) {
  static const LookupTable t({{-30, 1929},
                              {-25, 1839},
                              {-20, 1794},
                              {-15, 1759},
                              {-10, 1678},
                              {-5, 1599},
                              {0, 1521},
                              {5, 1461},
                              {10, 1339},
                              {15, 1306},
                              {20, 1260},
                              {25, 1175},
                              {30, 1071}

  });
  return (int)t.lookup(theta.degrees());
}

Angle Car::angle_for_steering(int str) {
  static const LookupTable t({{1071, 30},
                              {1175, 25},
                              {1260, 20},
                              {1306, 15},
                              {1339, 10},
                              {1461, 5},
                              {1521, 0},
                              {1599, -5},
                              {1678, -10},
                              {1759, -15},
                              {1794, -20},
                              {1839, -25},
                              {1929, -30}});

  return Angle::degrees(t.lookup(str));
}

#include <array>
#include <vector>
#include "lookup-table.h"

int Car::esc_for_velocity(double v) {
  static const LookupTable t({
      {-2., 1300},  // {-2., 1200},
      {-1., 1400},  // {-1., 1250},
      {-.1, 1450},  // {-.1, 1326},
      {0.0, 1500},  // {0.0,  1500},
      {0.1, 1550},  // {0.1,  1610},
      {0.5, 1560},  // {0.5, 1620},
      {2.0, 1570},  // {2.0, 1671},
      {3.3, 1630},  // {3.3, 1700},
      {4.1, 1680},  // {4.1, 1744},
      {5, 1710},    // {5, 1770},
      {7, 1827},    // {7, 1887},
      {9.5, 1895},  // {9.5,1895},
      {20, 2000}    // {20, 2000}
  });
  return t.lookup(v);

  /* old, ts table
  static const LookupTable t(
  {
    {-2., 1200},
    {-1., 1250},
    {0.0,  1500},
    {0.1, 1645},
    {0.34, 1659},
    {0.85, 1679},
    {1.2, 1699},
    {1.71, 1719},
    {1.88, 1739},
    {2.22, 1759},
    {2.6, 1779},
    {3.0, 1799},
    {14.0, 2000}
  });
  return t.lookup(v);
  */
}

void test_car() {
  Car car;
  for (auto i : linspace(-35, 25, 5)) {
    cout << "degrees: " << i
         << " steering: " << car.steering_for_angle(Angle::degrees(i)) << endl;
  }
}