#include <ArduinoWebsockets.h>
#include <WiFi.h>
#include "ESPAsyncWebServer.h"
#include "esp_camera.h"
#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h";
#include <DifferentialSteering.h>

int index_html_gz_len = 3863;
uint8_t index_html_gz[] = {
  0x1f,0x8b,0x08,0x08,0xb8,0x93,0x74,0x62,0x00,0xff,0x69,0x6e,0x64,0x65,0x78,0x2e,0x68,0x74,0x6d,0x6c,0x2e,0x67,0x7a,0x00,0x95,0x58,0x7b,0x6f,0xdb,0xba,0x15,0xff,0x3b,0xfe,0x14,0xac,0x8b,0x55,0xf2,0x6a,0x4b,0x4e,0xd2,0x6e,0xad,0x6b,0x67,0x40,0x97,0x62,0x2d,0xd0,0xa1,0xc5,0xd2,0xbb,0x26,0xb8,0x18,0x02,0x5a,0xa2,0x6d,0xb6,0x94,0xa8,0x4b,0x52,0x7e,0xec,0x36,0xdf,0xbd,0xe7,0x50,0x94,0x44,0xd9,0x4e,0x90,0xda,0x80,0x25,0x91,0xe7,0x77,0x5e,0x3c,0x2f,0x79,0xba,0x32,0x99,0xb8,0xe8,0x4d,0x57,0x8c,0xa6,0x70,0xc9,0x98,0xa1,0x24,0xa7,0x19,0x9b,0x05,0x6b,0xce,0x36,0x85,0x54,0x26,0x20,0xf5,0x27,0x91,0xb9,0x61,0xb9,0x99,0x05,0x1b,0x9e,0x9a,0xd5,0x2c,0x65,0x6b,0x9e,0xb0,0x91,0x7d,0x18,0x36,0x44,0x3c,0xe7,0x86,0x53,0x31,0xd2,0x09,0x15,0x6c,0x76,0x1a,0x8d,0xdb,0xad,0x52,0x33,0x65,0xd7,0xe9,0x1c,0xb6,0x72,0x19,0x90,0x18,0x44,0x1a,0x6e,0x04,0xbb,0xf8,0x17,0x4d,0x97,0xcc,0x90,0x7f,0x82,0x08,0x25,0x05,0xf9,0x4c,0x73,0x26,0xa6,0x71,0xb5,0xd7,0x9b,0x6a,0xb3,0x83,0x2b,0xe9,0x3d,0xd5,0x46,0x31,0x9a,0x8d,0x50,0x13,0xca,0x73,0xa6,0xfe,0xec,0x59,0xce,0x2b,0xc6,0x97,0x2b,0x33,0x21,0x2f,0x5e,0xaf,0x57,0x6f,0xaa,0xa5,0x82,0xa6,0x29,0xcf,0x97,0x93,0xb1,0x7b,0xce,0xa8,0x5a,0xf2,0x7c,0xef,0x71,0x34,0x17,0x32,0xf9,0x3e,0xd2,0x86,0x2a,0x73,0x7c,0x8b,0xe5,0xe9,0xfe,0x06,0xcf,0x05,0x88,0x3e,0x0e,0x72,0x7b,0x16,0x45,0x7a,0x77,0x87,0x0a,0x13,0x9e,0x2d,0x9d,0xd2,0x29,0xd7,0x85,0xa0,0xbb,0x89,0x15,0xd4,0xb0,0xd9,0x56,0x0e,0x9d,0x9c,0x8e,0xc7,0x7f,0xa9,0x17,0x81,0xb1,0xb3,0xd0,0x33,0x70,0x2e,0x55,0x0a,0xfe,0x54,0x34,0xe5,0xa5,0x9e,0xbc,0x28,0xb6,0x5d,0x4d,0x8c,0x2c,0x26,0xaf,0x8a,0xad,0x55,0xa2,0x95,0xee,0x44,0x57,0x22,0x88,0x27,0xe3,0x88,0x07,0xe7,0x34,0xf9,0xbe,0x54,0xb2,0xcc,0x53,0x50,0x5f,0x48,0x35,0x21,0x4f,0x2f,0x4f,0xf1,0xeb,0xf6,0x6b,0xfd,0xc9,0x42,0xb0,0x5a,0x38,0x15,0x7c,0x09,0x5e,0x30,0x2c,0xd3,0x13,0x92,0x40,0xb0,0x30,0xe5,0x76,0xbe,0x95,0xda,0xf0,0xc5,0xce,0x7a,0x02,0xd6,0xf7,0x76,0xe5,0x9a,0xa9,0x85,0x90,0x9b,0x09,0x59,0xf1,0x34,0x65,0xf9,0x51,0x1b,0xc9,0xdf,0x1b,0x23,0x8d,0x2c,0x93,0xd5,0x88,0x26,0x86,0xcb,0x7c,0x42,0x72,0x99,0xb3,0x37,0x68,0x28,0x0a,0x3e,0xb4,0xb1,0x41,0xd5,0x46,0xfa,0x6b,0x87,0x56,0xaa,0xe5,0x3c,0x7c,0x75,0x3a,0x24,0x2f,0x20,0x74,0x4f,0xcf,0xff,0x36,0xe8,0xe8,0x82,0x60,0xf0,0xaa,0x96,0x82,0xa7,0x48,0x49,0x43,0x20,0xb1,0x74,0xee,0x27,0x7a,0xd9,0x05,0x34,0xca,0xbf,0x6c,0x7c,0x7d,0x4c,0x79,0x2f,0x41,0x98,0x60,0x09,0x28,0xd9,0x31,0x6a,0x82,0xd4,0x6b,0x56,0xdb,0x76,0x54,0xe9,0xe3,0xaa,0xd4,0x0c,0x56,0xe8,0xe2,0x1a,0x9f,0x94,0x4a,0x23,0xaa,0x90,0xdc,0x3b,0x04,0xa7,0xb0,0x73,0xdc,0x99,0xf5,0x11,0xc0,0x29,0x44,0x70,0x0d,0x2c,0xa4,0x86,0xf4,0x46,0xb5,0x17,0x7c,0xcb,0x52,0x07,0x54,0x95,0x5b,0xeb,0x5c,0xc0,0xd0,0xc3,0x87,0xbb,0x1e,0x7c,0xa6,0x71,0x95,0xbc,0x70,0xe3,0x6a,0xcc,0x5c,0xa6,0x3b,0xb8,0xf0,0xbc,0x28,0x0d,0x31,0xbb,0x02,0x6a,0x8d,0x61,0x5b,0xc8,0x26,0x46,0x03,0xc2,0xd3,0x59,0x50,0xdd,0x41,0x78,0x61,0x9d,0x48,0x6d,0x91,0x48,0xf9,0xda,0x6e,0xc9,0x12,0xd4,0xc5,0x22,0x61,0xc3,0x39,0xb8,0x00,0x81,0xf5,0x5e,0x7f,0x3f,0xd5,0xfa,0x24,0x11,0x54,0xeb,0x59,0x9f,0x67,0x74,0xc9,0xbc,0x75,0x44,0x91,0x29,0x24,0xa2,0x07,0xeb,0x13,0xad,0x92,0x59,0xdf,0xee,0x4d,0x63,0x60,0xe9,0xb3,0x0e,0x1a,0xac,0x95,0xe8,0x6d,0xa0,0x6b,0x83,0x0b,0x0f,0x51,0xdd,0x80,0xb1,0xf6,0x3a,0xd5,0x89,0xe2,0x85,0xb9,0xe8,0x01,0x03,0x6d,0x08,0x96,0x54,0x32,0x23,0xa9,0x4c,0xca,0x0c,0xa2,0x3f,0x82,0x8a,0xf7,0x4e,0x30,0xbc,0x7d,0xbb,0xfb,0x90,0x86,0x41,0xa5,0x4a,0x00,0xa7,0x56,0xd1,0x7f,0xbd,0xba,0xfd,0xed,0x3f,0x1f,0x01,0xd1,0xdf,0xe8,0x49,0x1c,0xf7,0xc9,0x73,0x88,0xea,0x3c,0x95,0x9b,0x08,0x4a,0x06,0xc5,0x73,0x88,0x56,0x12,0xe8,0x9e,0x93,0xfe,0xe4,0xd5,0x59,0xbf,0x86,0x6d,0x34,0x40,0x72,0x10,0xf5,0x95,0xcd,0xaf,0xa0,0xb6,0x30,0x13,0x56,0x9c,0x80,0xf1,0xc9,0x49,0x6f,0xa3,0x23,0x99,0x67,0x4c,0x6b,0xf0,0x0a,0x10,0x36,0x77,0x17,0x70,0xc8,0x27,0x7c,0x41,0x42,0xb7,0x12,0xa5,0x14,0x9a,0x01,0x07,0x8e,0x34,0x4f,0x98,0x5c,0x90,0xb7,0x42,0xce,0x07,0x48,0x44,0xc8,0x9a,0x2a,0x52,0x2a,0xf1,0x69,0xfe,0x0d,0x62,0x15,0x98,0x00,0xf3,0x28,0x01,0xe5,0x0d,0xab,0x96,0xe0,0xb9,0xc3,0x06,0x25,0x03,0x0a,0xec,0x8f,0xc0,0xcf,0x00,0x68,0xc0,0xb0,0x71,0xd7,0xbb,0x7b,0xd3,0xeb,0x21,0xcb,0x54,0xd1,0xe5,0x07,0xcc,0x61,0xcf,0x49,0x7f,0x94,0x4c,0xed,0xae,0x6c,0x52,0x48,0x15,0x06,0x36,0x9a,0xd1,0x45,0x48,0xdf,0x16,0xb6,0x07,0x00,0xed,0xe1,0xed,0xa3,0xbe,0x62,0xa4,0x03,0xb4,0x59,0x88,0xe4,0x62,0xa1,0x99,0xb1,0xeb,0x7b,0xb4,0xef,0x6d,0xed,0x38,0x42,0x5c,0x6d,0x54,0xd4,0x50,0xbe,0x2f,0xc1,0x84,0xf7,0x52,0xf1,0xff,0x23,0x99,0xf0,0xe9,0x2b,0x69,0x31,0x39,0xeb,0xd0,0xfe,0x97,0x29,0xc3,0xa1,0x27,0xfa,0x94,0x4e,0x96,0x25,0xbd,0x37,0x58,0x6c,0x96,0x0c,0xa2,0x35,0x15,0x25,0x1e,0x63,0xd5,0x94,0x27,0x24,0x80,0x68,0x38,0x54,0xe3,0x39,0xac,0xd7,0xd5,0xcf,0xa3,0xa8,0x85,0x57,0x1a,0xb9,0x1a,0x33,0x23,0x0b,0x2a,0x34,0xd4,0x1e,0x6b,0x7f,0xa9,0x14,0x88,0xbc,0xee,0x3c,0xdd,0x54,0x4f,0xae,0xd5,0xbb,0x3d,0xf7,0xe4,0xf6,0xb6,0x9f,0xac,0x73,0x80,0x19,0x14,0x02,0x5c,0xd8,0xed,0x2f,0x40,0x6e,0x9a,0x2f,0x90,0xf7,0x43,0x7b,0x77,0x05,0x2d,0x73,0x48,0x34,0xfc,0x7e,0xe1,0x19,0x83,0x34,0xb7,0xc1,0xec,0x1c,0x0d,0xad,0xfc,0xdd,0x1a,0xe4,0x7e,0xe4,0x1a,0x7a,0x07,0x83,0x43,0xb5,0x25,0xd4,0x76,0xe0,0x60,0x68,0xa3,0xe6,0x0a,0xef,0x87,0x95,0xe6,0x55,0xfe,0x3c,0x8c,0x05,0x41,0x0e,0xf9,0x0e,0x05,0x3f,0x1a,0x97,0x41,0x19,0x75,0xc0,0xc7,0xa1,0x32,0x09,0x55,0x1d,0x52,0x36,0xff,0x75,0x45,0x2d,0xb4,0x2c,0x7e,0x55,0x4f,0x0b,0x3b,0xaa,0x67,0x6f,0x51,0xe6,0xb6,0xeb,0xb4,0x9a,0x84,0x0c,0x33,0xda,0x0e,0x6e,0x90,0xf9,0x2c,0xc2,0x7a,0x4c,0x66,0x33,0x88,0x27,0xcf,0xc5,0x35,0x89,0x37,0xdf,0x5d,0xc3,0x41,0x02,0xb5,0x75,0xa5,0xfe,0x7d,0xfc,0xbf,0x28,0x11,0x1c,0xe3,0x84,0x8c,0xea,0xa3,0x77,0xcd,0xa0,0x85,0xdc,0x1c,0x87,0xdc,0x00,0xc4,0x05,0x47,0x05,0xb9,0x23,0x0c,0xd4,0xbd,0x4f,0xe4,0x63,0xe5,0xdc,0xc7,0xdc,0xb7,0x15,0x26,0x25,0x8c,0x48,0xb0,0xb6,0x2e,0x3c,0xbe,0xa5,0x4d,0x3a,0x18,0x55,0xba,0x0e,0x0d,0x95,0x6a,0xcf,0x89,0x70,0x2a,0x9e,0x0b,0x5b,0x45,0xdb,0xbc,0xd9,0xd3,0xac,0x4d,0xa1,0x8e,0x0c,0x97,0x72,0x27,0x98,0x01,0x61,0x30,0x1e,0x8e,0xb1,0x58,0xed,0x0b,0xeb,0x1e,0x56,0x85,0xf5,0x35,0x66,0x51,0xa1,0x18,0x06,0xc3,0x25,0x5b,0xd0,0x52,0x98,0xb0,0x1e,0x45,0xee,0x3b,0x5e,0x1b,0x25,0x3e,0x07,0x37,0x1c,0x58,0xd5,0xef,0x3f,0xe1,0x36,0xf1,0x8f,0xe0,0x1e,0x38,0xe6,0xb6,0x44,0xd4,0x98,0x83,0xb3,0x3e,0x50,0xe0,0x97,0xa4,0x3e,0x28,0xaa,0xb9,0x6b,0x6b,0x53,0xf7,0x94,0xf0,0xd3,0x96,0xa9,0xee,0x41,0xd5,0x2e,0xfc,0x37,0x35,0xab,0x88,0xce,0x75,0x58,0x6f,0x0f,0xc8,0xf4,0xa0,0x94,0x3f,0x7b,0x46,0xf6,0xe9,0xae,0x3d,0xba,0xb6,0x2e,0xef,0xbb,0x1e,0x24,0x7f,0x51,0x34,0xd7,0x02,0x1a,0x69,0x83,0x1c,0x36,0xaa,0x54,0xf9,0x6c,0xe3,0xf4,0x98,0x5d,0x8f,0x6e,0x15,0xd7,0x55,0x13,0x68,0xfc,0x8c,0xdd,0xe1,0xa6,0xb3,0xe6,0xac,0xb6,0xf1,0x1e,0xc7,0x44,0xf0,0x8c,0x1b,0x5b,0x9d,0xe1,0x95,0x0a,0xe6,0x3b,0x02,0xb3,0x69,0x33,0x38,0x40,0xc4,0xa9,0x1d,0x39,0x1f,0x93,0x4c,0x23,0xed,0xca,0x98,0x02,0xa7,0x95,0x25,0x37,0xab,0x72,0x1e,0x25,0x32,0x8b,0x73,0x26,0x73,0x0e,0x55,0x29,0x86,0x09,0xff,0x9b,0xbe,0x65,0xba,0x38,0x3f,0xbb,0x65,0x5b,0x9a,0x15,0x82,0xe9,0x18,0xde,0x7f,0xe6,0x71,0x06,0x3d,0x80,0xa9,0x86,0xf2,0x16,0x04,0xc4,0x09,0x13,0xa2,0x58,0xc1,0xdd,0x2d,0x96,0x3b,0x78,0x2d,0x84,0x69,0xf0,0x56,0x25,0xb7,0x09,0x55,0xf1,0x66,0xb3,0x89,0x61,0x14,0x62,0xdb,0x08,0x5f,0x60,0xdb,0x34,0xb1,0xf9,0x63,0xb6,0xa6,0xf6,0x2c,0x76,0x9a,0x5c,0xe2,0xd4,0x85,0x03,0xd1,0x25,0xfa,0x75,0x80,0xfe,0xc1,0x2e,0x53,0xa7,0x07,0x5f,0x84,0x75,0x0b,0xb2,0xb9,0x01,0x63,0x35,0x5b,0x40,0x71,0x4d,0xc9,0x8f,0x1f,0x16,0x3c,0x6a,0x5a,0x14,0xb9,0x98,0x81,0xa5,0xfe,0xb1,0x19,0xb0,0xbd,0x7b,0x88,0x30,0x5f,0x35,0x5a,0x74,0x63,0xb5,0x95,0xf2,0x80,0x36,0xf5,0x47,0x31,0x53,0xaa,0xfa,0x45,0xc8,0xa6,0x0a,0x8c,0x7c,0xc9,0xaa,0xad,0x01,0xde,0x6b,0x39,0xbc,0x8e,0x30,0x98,0x09,0xb1,0x40,0xec,0x07,0x46,0xf5,0x5b,0x37,0x5b,0xac,0x66,0x5b,0x57,0x0a,0xc1,0xec,0x27,0x5e,0xc3,0xf5,0xd9,0xda,0xf9,0x04,0xa7,0xc8,0x46,0xe3,0x27,0x1d,0xbf,0xfc,0x03,0x8f,0x7b,0x44,0xc2,0xae,0x73,0x06,0x64,0x02,0xeb,0x7e,0xbe,0x84,0xc0,0x64,0x4a,0xc6,0x83,0x8e,0xbe,0x96,0xb1,0x47,0xe6,0xe9,0x00,0x1b,0x98,0x02,0xd5,0x43,0x18,0x0e,0xaa,0xa1,0xb4,0x9b,0x21,0x3e,0x71,0x5e,0x0a,0x98,0x5d,0xf6,0xf7,0xed,0x69,0xa2,0xb5,0xbe,0x33,0x86,0x20,0xd7,0x3d,0x77,0xab,0x78,0x27,0xe7,0xb6,0x9f,0xa5,0x1e,0x92,0x9d,0xfd,0x65,0x4d,0x7a,0x32,0x11,0xd9,0x97,0x98,0xc8,0x20,0xe1,0x42,0x2a,0x1c,0x50,0x03,0xfb,0x80,0xa8,0x73,0x48,0x31,0x48,0x1d,0xc4,0x62,0x2a,0x15,0xdb,0xa1,0x4d,0x25,0xe4,0x52,0x3f,0x8f,0x07,0x41,0x25,0x1b,0x3d,0x5b,0xd0,0xfc,0x92,0x2d,0x15,0x63,0xe8,0x08,0x8b,0xfa,0x2b,0x79,0x3d,0x86,0x71,0xef,0xa0,0x3e,0xb4,0x18,0xc3,0x05,0x54,0xf5,0x1a,0x64,0x59,0x77,0x41,0xed,0x28,0x57,0xf5,0x10,0x4f,0x08,0xe8,0x30,0x44,0x85,0x3c,0x1e,0xb6,0xb9,0xc0,0xab,0x99,0x7b,0x3d,0x99,0xc6,0xee,0xa5,0x2c,0xb6,0x7f,0x07,0xfd,0x04,0xb9,0x1c,0xb4,0x51,0x15,0x12,0x00,0x00
};

DifferentialSteering DiffSteer;
int fPivYLimit = 32;
//int idle = 4915;
int idle = 0;

unsigned long previousMillis = 0;
const long interval = 1000;

long duration;

// Set your access point network credentials
const char* ssid = "Gadget";

#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27
#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22

camera_fb_t * fb = NULL;

using namespace websockets;
WebsocketsServer WSserver;
AsyncWebServer webserver(80);

void setup() {
  DiffSteer.begin(fPivYLimit);

  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector
  Serial.begin(115200);
  // Ai-Thinker: pins 2 and 12
  ledcSetup(2, 50, 16); //channel, freq, resolution
  ledcAttachPin(12, 2); // pin, channel
  ledcSetup(4, 50, 16); //channel, freq, resolution
  ledcAttachPin(2, 4); // pin, channel

  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  //init with high specs to pre-allocate larger buffers
  if (psramFound()) {
    config.frame_size = FRAMESIZE_UXGA;
    config.jpeg_quality = 10;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_SVGA;
    config.jpeg_quality = 12;
    config.fb_count = 1;
  }

  // camera init
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }

  sensor_t * s = esp_camera_sensor_get();
  s->set_framesize(s, FRAMESIZE_QVGA);
  s->set_vflip(s, 0); //flip camera image
  s->set_hmirror(s, 0);  //mirror camera image

  // Wi-Fi connection
  Serial.println("Creating WiFi AP");
  // Remove the password parameter, if you want the AP (Access Point) to be open
  //  WiFi.softAP(ssid, password);
  WiFi.softAP(ssid);

  IPAddress IP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(IP);

  Serial.println("");
  Serial.println("WiFi connected");


  webserver.on("/", HTTP_GET, [](AsyncWebServerRequest * request) {
    AsyncWebServerResponse *response = request->beginResponse_P(200, "text/html", index_html_gz, sizeof(index_html_gz));
    response->addHeader("Content-Encoding", "gzip");
    request->send(response);
  });

  webserver.begin();
  WSserver.listen(82);
}

void handle_message(WebsocketsMessage msg) {
  int commaIndex = msg.data().indexOf(',');
  int panValue = msg.data().substring(0, commaIndex).toInt();
  int tiltValue = msg.data().substring(commaIndex + 1).toInt();

  if (panValue > -20 && panValue < 20) {
    panValue = 0;
  }
  if (tiltValue > -20 && tiltValue < 20) {
    tiltValue = 0;
  }
  panValue = map(panValue, -90, 90, -127, 127); // 0-180
  tiltValue = map(tiltValue, -90, 90, 127, -127); // 0-180 reversed
  differential_steer(panValue, tiltValue);
}

void differential_steer(int XValue, int YValue) {
  // Outside no action limit joystick
  if (!((XValue > -20) && (XValue < 20) && (YValue > -20) && (YValue < 20)))
  {
    DiffSteer.computeMotors(XValue, YValue);
    int leftMotor = DiffSteer.computedLeftMotor();
    int rightMotor = DiffSteer.computedRightMotor();

    // map motor outputs to your desired range
    leftMotor = map(leftMotor, -127, 127, 3276, 6554); // 0-180
    rightMotor = map(rightMotor, -127, 127, 6554, 3276); // 0-180 reversed
    ledcWrite(2, rightMotor); // channel, value
    ledcWrite(4, leftMotor); // channel, value

  } else
  {
    ledcWrite(2, idle);
    ledcWrite(4, idle); // channel, value
//    Serial.println("idle");
  }
}
void loop() {
  auto client = WSserver.accept();
  client.onMessage(handle_message);
  while (client.available()) {
    client.poll();
    fb = esp_camera_fb_get();
    client.sendBinary((const char *)fb->buf, fb->len);
    esp_camera_fb_return(fb);
    fb = NULL;
  }
}