#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <string>
#include "EEPROM.h"
#include <OneButton.h>
#include <SPI.h>
#include<BH1750.h>
#include<Wire.h>
#include<Arduino.h>
#include<U8g2lib.h>
#include<math.h>
#include "driver/rtc_io.h"
// Declaration for SSD1306 display connected using software SPI (default case):
#define OLED_MOSI  17
#define OLED_CLK   18
#define OLED_DC    8
#define OLED_CS    20
#define OLED_RESET 19
#define BTC 3


//主板BLE服务的UUID码，来自于CH9141芯片手册
#define SERVICE_UUID "0000fff0-0000-1000-8000-00805f9b34fb"
#define CHARACTERISTIC_UUID "0000fff2-0000-1000-8000-00805f9b34fb"
#define SERVICE_UUID_LOCAL "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID_LOCAL "beb5483e-36e1-4688-b7f5-ea07361b26a8"
#define CHARACTERISTIC_UUID1 "0000fff1-0000-1000-8000-00805f9b34fb"

#define DOSCAN 4
#define CopeSwitch 38
#define CONNECTED  6

#define DelaySignal 5
#define DialSwitchA 42
#define DialSwitchB 41
#define DialSwitchC 2
#define ModelSwitch 39

volatile int flag = 0;
volatile int alv = 0;
volatile int blv = 0;
volatile bool CW_1 = 0;
volatile bool CW_2 = 0;
int Time_C = 0;
boolean doSacn = true;
boolean doConnect = false;
boolean multiple = false;
boolean Model;
boolean isSleep = false;
boolean isAF = false;
boolean isApateur = false;
boolean isMatch = false;
boolean iso;
int previousPoint = 1010;
int previousVoltage = 0;
int multiple_count = 0;
boolean RST = true;
//旋转编码器控制
volatile boolean connected = false;
volatile boolean locked = false;
volatile int ShutterPoint = 0;
volatile int DelayPoint = 0 ;
volatile bool Switch = true;
String SA;

RTC_DATA_ATTR int bootCount = 0;

//蓝牙模块初始设置
BLEAdvertisedDevice* pServer;
BLERemoteCharacteristic* pRemoteCharacteristic;
BLERemoteCharacteristic* pRemoteCharacteristic1;
BLEScanResults foundDevices;
//OLED屏幕设置
U8G2_SSD1306_64X48_ER_F_4W_SW_SPI u8g2(U8G2_R0,OLED_CLK,OLED_MOSI,OLED_CS,OLED_DC,OLED_RESET);
//定义两个按键，编码器的选择开关以及机身复位按钮
OneButton button(DialSwitchC,true,true);
//字符串存蓝牙设备的MAC地址
String ServeAddress;
//用EEPROM存储蓝牙设备地址，上电不会被擦掉
EEPROMClass ServeAddress1("eeprom0");
BH1750 lightMeter(0x23);

//初次使用时，先进行配对
//广播一个BLE服务，用户向该服务中写入相机主板的MAC地址
//String Resign(void){
  //BLEServer *pServer = BLEDevice::createServer();
  //BLEService *pService = pServer->createService(SERVICE_UUID_LOCAL);
  //BLECharacteristic *pCharacteristic = pService->createCharacteristic(
                                        // CHARACTERISTIC_UUID_LOCAL,
                                        // BLECharacteristic::PROPERTY_READ |
                                         //BLECharacteristic::PROPERTY_WRITE
                                       //);
  //pCharacteristic->setValue("");
  //pService->start();
  //BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  //pAdvertising->addServiceUUID(SERVICE_UUID_LOCAL);
  //pAdvertising->setScanResponse(true);
  //pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
  //pAdvertising->setMinPreferred(0x12);
  //BLEDevice::startAdvertising();
  //digitalWrite(DOSCAN,HIGH);
  //String Name = pCharacteristic->getValue().c_str();
  //while(Name.length() == 0){
    //Name = pCharacteristic->getValue().c_str();
    //delay(500);
    //u8g2.clearBuffer();
    //u8g2.setFont(u8g2_font_luBIS10_tf);
    //u8g2.setFontRefHeightExtendedText();
    //u8g2.setDrawColor(1);
    //u8g2.setFontPosTop();
    //u8g2.setFontDirection(0);
    //u8g2.drawStr(0,8,"MATCH");
    //u8g2.sendBuffer();
  //}
  //digitalWrite(DOSCAN,LOW);
  //BLEDevice::stopAdvertising();
  //return Name;
//}


// 搜索到目标设备地址时回调功能
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    void onResult(BLEAdvertisedDevice advertisedDevice) {
     
      }
     
};

          

// 客户端与服务器连接与断开指示
class MyClientCallback : public BLEClientCallbacks {
  void onConnect(BLEClient* pclient) {}
  void onDisconnect(BLEClient* pclient) {
    doSacn = true;
    connected = false;
    
  }
};





// 用来连接设备获取其中的服务与特征
bool ConnectToServer(void) {
    BLEClient* pClient  = BLEDevice::createClient(); // 创建客户端
    pClient->setClientCallbacks(new MyClientCallback()); // 添加客户端与服务器连接与断开回调功能
    if (!pClient->connect(pServer)) { // 尝试连接设备
      return false;
    }
    

    BLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID); // 尝试获取设备中的服务
    if (pRemoteService == nullptr) {
      pClient->disconnect();
      return false;
    }
    

    pRemoteCharacteristic = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID); // 尝试获取服务中的特征
    if (pRemoteCharacteristic == nullptr) {
      pClient->disconnect();
      return false;
    }
    pRemoteCharacteristic1 = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID1);
    pRemoteCharacteristic1->registerForNotify(notifyCallback);
    return true;
}

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
  Time_C = millis();
}


//显示快门速度，延迟时间，以及指示器
void DisplayData(String Shutter,String Apateur,String DLT,bool Dot){
  u8g2.clearBuffer();
  int voltage = Batterycheck();
  if(voltage > 1950){
    u8g2.drawLine(0, 24, 64, 24);
    u8g2.drawLine(0, 25, 64, 25);
  }else if((voltage<1950)&&(voltage>1850)){
    u8g2.drawLine(0, 24, 48, 24);
    u8g2.drawLine(0, 25, 48, 25);
  }else if((voltage<1850)&&(voltage>1750)){
    u8g2.drawLine(0, 24, 32, 24);
    u8g2.drawLine(0, 25, 32, 25);
  }else {
    u8g2.drawLine(0, 24, 16, 24);
    u8g2.drawLine(0, 25, 16, 25);
  }
  int LS = MeasureLight();
  if (LS == 0){
     u8g2.setFont(u8g2_font_7x13_t_symbols);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawGlyph(0,8,0x25cf);
  }else if(LS == -1){
     u8g2.setFont(u8g2_font_7x13_t_symbols);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawGlyph(0,8,0x25b2);
  }else if(LS == 1){
     u8g2.setFont(u8g2_font_7x13_t_symbols);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawGlyph(0,8,0x25bc);
  }
  if(Dot == true){
     u8g2.setFont(u8g2_font_luBIS10_tf);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     if(isApateur == false){
      u8g2.drawStr(10,8,Shutter.c_str());
     }else{
      u8g2.drawStr(10,8,Apateur.c_str());
     }
     //u8g2.drawStr(10,8,Shutter.c_str());
     u8g2.setFont(u8g2_font_luBIS10_tf);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawStr(0,33,DLT.c_str());
     //u8g2.drawLine(0, 24, 64, 24);
     //u8g2.drawLine(0, 25, 64, 25);
     u8g2.setFont(u8g2_font_7x13_t_symbols);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawGlyph(57,8,0x25c2);
     //u8g2.drawGlyph(47,33,0x23f4);
  } else {
    u8g2.setFont(u8g2_font_luBIS10_tf);
    u8g2.setFontRefHeightExtendedText();
    u8g2.setDrawColor(1);
    u8g2.setFontPosTop();
    u8g2.setFontDirection(0);
    if(isApateur == false){
      u8g2.drawStr(10,8,Shutter.c_str());
    }else{
      u8g2.drawStr(10,8,Apateur.c_str());
    }
    //u8g2.drawStr(10,8,Shutter.c_str());
    u8g2.setFont(u8g2_font_luBIS10_tf);
    u8g2.setFontRefHeightExtendedText();
    u8g2.setDrawColor(1);
    u8g2.setFontPosTop();
    u8g2.setFontDirection(0);
    u8g2.drawStr(0,33,DLT.c_str());
    //u8g2.drawLine(0, 24, 64, 24);
    //u8g2.drawLine(0, 25, 64, 25);
    u8g2.setFont(u8g2_font_7x13_t_symbols);
    u8g2.setFontRefHeightExtendedText();
    u8g2.setDrawColor(1);
    u8g2.setFontPosTop();
    u8g2.setFontDirection(0);
    //u8g2.drawGlyph(47,8,0x23f4);
    u8g2.drawGlyph(57,33,0x25c2);
  }
  u8g2.sendBuffer();
}

//状态显示
void StateDisplay(void){
  digitalWrite(DOSCAN,doSacn);
  digitalWrite(CONNECTED,connected);
}
void shutdownled(void){
  digitalWrite(DOSCAN,false);
  digitalWrite(CONNECTED,false);
}
//机身指令集：
uint8_t AutoFocus[4] = {0xAF,0xAF,0xAF,0xAF};//自动对焦
uint8_t AutoFocusCancel[4] = {0xCF,0xCF,0xCF,0xCF};//镜头复位
uint8_t Tshutter[4] = {0xFF,0xFF,0x00,0x02};//T门
uint8_t Shot[4] = {0xEE,0xEE,0xEE,0xEE};//击发快门
uint8_t Iso640[4] = {0xFE,0xFE,0xFE,0xFE};
uint8_t Iso125[4] = {0xFD,0xFD,0xFD,0xFD};


//快门速度指令集
uint8_t SS_AUTO[4] = {0xFF,0xFF,0x00,0x01};//AUTO
uint8_t SS_30[4] = {0xFF,0xFF,0x75,0x30};//30000ms
uint8_t SS_25[4] = {0xFF,0xFF,0x61,0xA8};//25000ms
uint8_t SS_20[4] = {0xFF,0xFF,0x4E,0x20};//20000ms
uint8_t SS_15[4] = {0xFF,0xFF,0x3A,0x98};//15000ms
uint8_t SS_8[4] = {0xFF,0xFF,0x1F,0x40};//8000ms
uint8_t SS_4[4] = {0xFF,0xFF,0x0F,0xA0};//4000ms
uint8_t SS_2[4] = {0xFF,0xFF,0x07,0xD0};//2000ms
uint8_t SS_1[4] = {0xFF,0xFF,0x03,0xE8};//1000ms
uint8_t SS_12[4] = {0xFF,0xFF,0x01,0xF4};//500ms
uint8_t SS_14[4] = {0xFF,0xFF,0x00,0xFA};//250ms
uint8_t SS_18[4] = {0xFF,0xFF,0x00,0x7D};//125ms~
uint8_t SS_110[4] = {0xFF,0xFF,0x00,0x64};//100msF8
uint8_t SS_115[4] = {0xFF,0xFF,0x00,0x42};//66ms F9.5
uint8_t SS_125[4] = {0xFF,0xFF,0x00,0x38};//56msF10
uint8_t SS_140[4] = {0xFF,0xFF,0x00,0x36};//54msF10
uint8_t SS_150[4] = {0xFF,0xFF,0x00,0x34};//52msF11~
uint8_t SS_190[4] = {0xFF,0xFF,0x00,0x32};//50msF13
uint8_t SS_1125[4] = {0xFF,0xFF,0x00,0x30};//48msF16~
uint8_t SS_1180[4] = {0xFF,0xFF,0x00,0x2E};//46msF16
uint8_t SS_1250[4] = {0xFF,0xFF,0x00,0x2C};//44msF18
uint8_t SS_1400[4] = {0xFF,0xFF,0x00,0x29};//41msF18
uint8_t SS_1500[4] = {0xFF,0xFF,0x00,0x26};//38msF20
uint8_t SS_1640[4] = {0xFF,0xFF,0x00,0x21};//35msF20
uint8_t SS_11000[4] = {0xFF,0xFF,0x00,0x20};//32msF22~
uint8_t SS_11500[4] = {0xFF,0xFF,0x00,0x1D};//29msF24
uint8_t SS_12000[4] = {0xFF,0xFF,0x00,0x1A};//26msF28
uint8_t SS_13200[4] = {0xFF,0xFF,0x00,0x17};//23msF32
uint8_t SS_14000[4] = {0xFF,0xFF,0x00,0x14};//20msF32~
uint8_t SS_16400[4] = {0xFF,0xFF,0x00,0x12};//18msF36
uint8_t SS_18000[4] = {0xFF,0xFF,0x00,0x10};//16msF44
//显示信号集
uint8_t* Signal[31] = {SS_AUTO,Tshutter,SS_25,SS_20,SS_15,SS_8,SS_4,SS_2,SS_1,SS_12,SS_14,SS_18,SS_110,SS_115,SS_125,SS_140,SS_150,SS_190,SS_1125,SS_1180,SS_1250,SS_1400,SS_1500,SS_1640,SS_11000,SS_11500,SS_12000,SS_13200,SS_14000,SS_16400,SS_18000};
float   signalf[31] = {0,0,25,20,25,8,4,2,1,0.5,0.25,0.15,0.1,0.075,0.04,0.025,0.02,0.01,0.008,0.005,0.004,0.0025,0.002,0.0015,0.001,0.00075,0.0005,0.00035,0.00025,0.000175,0.000125};
String  SignalDisplay[31] = {"A","T","25s","20s","15s","8s","4s","2s","1s","2","4","8","10","15","25","40","50","90","125","180","250","400","500","640","1000","1500","2000","3200","4000","6400","8000"};
String  ApateurDisplay[31] = {"A","F8","F8","F8","F8","F8","F8","F8","F8","F8","F8","F8","F8","F8","F10","F10","F11","F13","F16","F16","F18","F18","F20","F20","F22","F24","F28","F32","F32","F36","F44"};
int DelayTime[31] ={0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30};
String DelayTimeDisplay[31]={"0","1","2","3","4","5","6","7","8","9","10","11","12","13","14","15","16","17","18","19","20","21","22","23","24","25","26","27","28","29","30"};

//发送信号
bool Send(int i){
    if(previousPoint != i) {
      pRemoteCharacteristic->writeValue(Signal[i],4,true);
      digitalWrite(CONNECTED,HIGH);
      delay(50);
      digitalWrite(CONNECTED,LOW);
      delay(50);
      digitalWrite(CONNECTED,HIGH);
      delay(50);
      digitalWrite(CONNECTED,LOW);
      delay(50);
      previousPoint = i;
      return true;
    }else{
      return false;
    }
}

bool SetIso(bool iso){
  if(iso == false){
     pRemoteCharacteristic->writeValue(Iso640,4,true);
     digitalWrite(CONNECTED,HIGH);
     delay(50);
     digitalWrite(CONNECTED,LOW);
     delay(50);
     digitalWrite(CONNECTED,HIGH);
     delay(50);
     digitalWrite(CONNECTED,LOW);
     delay(50);
     u8g2.clearBuffer();
     u8g2.setFont(u8g2_font_luBIS10_tf);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawStr(0,8,"ISO-640");
     u8g2.sendBuffer();
     delay(800);
     return false;
  } else{
     pRemoteCharacteristic->writeValue(Iso125,4,true);
     digitalWrite(CONNECTED,HIGH);
     delay(50);
     digitalWrite(CONNECTED,LOW);
     delay(50);
     digitalWrite(CONNECTED,HIGH);
     delay(50);
     digitalWrite(CONNECTED,LOW);
     delay(50);
     u8g2.clearBuffer();
     u8g2.setFont(u8g2_font_luBIS10_tf);
     u8g2.setFontRefHeightExtendedText();
     u8g2.setDrawColor(1);
     u8g2.setFontPosTop();
     u8g2.setFontDirection(0);
     u8g2.drawStr(0,8,"ISO-125");
     u8g2.sendBuffer();
     delay(800);
     return true;
  }
}

//延时快门自拍
void SendDelay(int i,bool M){
  bool shutterPress = digitalRead(CopeSwitch);
  if(shutterPress == false){
    if(pRemoteCharacteristic->canWrite()){
    if(i != 0){
      digitalWrite(DelaySignal,LOW);
      delay(500);
      digitalWrite(DelaySignal,HIGH);
      delay(500);
      digitalWrite(DelaySignal,LOW);
      delay(500);
      digitalWrite(DelaySignal,HIGH);
      delay(500);
      delay((i-5)*1000);
      digitalWrite(DelaySignal,LOW);
      delay(1000);
      digitalWrite(DelaySignal,HIGH);
      pRemoteCharacteristic->writeValue(AutoFocus,4);
      delay(300);
      pRemoteCharacteristic->writeValue(Shot,4);
      delay(1200);
      digitalWrite(DelaySignal,LOW);
      pRemoteCharacteristic->writeValue(AutoFocusCancel,4);
      delay(500);
      isAF = false;
      Time_C = millis();
    }
   }
  }
}


//遥控快门
void SendRemo(bool M){
  if(pRemoteCharacteristic->canWrite()){
    delay(100);
    bool shutterPress = digitalRead(CopeSwitch);
    if(shutterPress == false){
      delay(500);
      pRemoteCharacteristic->writeValue(AutoFocus,4);
      delay(300);
      pRemoteCharacteristic->writeValue(Shot,4);
      delay(1200);
      pRemoteCharacteristic->writeValue(AutoFocusCancel,4);
      Time_C = millis();
    }
   }
}

void SendT(void){
  if(pRemoteCharacteristic->canWrite()){
    boolean shutterPress = digitalRead(CopeSwitch);
    if(shutterPress == false){
      int Time_T = millis();
      delay(200);
      pRemoteCharacteristic->writeValue(AutoFocus,4);
      delay(300);
      pRemoteCharacteristic->writeValue(Shot,4);
      while(digitalRead(CopeSwitch) != false){
        int Time_U = (millis() - Time_T)/1000;
        u8g2.clearBuffer();
        u8g2.setFont(u8g2_font_luBIS14_tf);
        u8g2.setFontRefHeightExtendedText();
        u8g2.setDrawColor(1);
        u8g2.setFontPosTop();
        u8g2.setFontDirection(0);
        u8g2.drawStr(0,8,std::to_string(Time_U).c_str());
        u8g2.sendBuffer();
      }
      pRemoteCharacteristic->writeValue(Shot,4);
      shutterPress = !shutterPress;
      delay(600);
    }
  }
}
//长按之后松手发送指令到相机，防止误操作
bool ShutterAction(void){
  button.tick(); 
  if(DelayTime[DelayPoint] == 0 && Signal[ShutterPoint] != Tshutter){
    SendRemo(Model);
  }else if(DelayTime[DelayPoint] != 0 && Signal[ShutterPoint] != Tshutter){
    SendDelay(DelayTime[DelayPoint],Model);
  }else{
    SendT();
  }
  if(Send(ShutterPoint)){
      return true;
  }else{
    return false;
  }
}


//单击更换输入对象（快门/延迟时间）
void SingleClick(void){
  Switch = !Switch;
}


//双击复位（快门自动/不延时）
void DoubleClick(void){
  Switch = true;
  ShutterPoint = 0;
  DelayPoint = 0;
}


//点击复位按键之后的注册程序


void AF(void){
  if(pRemoteCharacteristic->canWrite()){
    if(isAF == true){
       pRemoteCharacteristic->writeValue(AutoFocusCancel,4);
    }else{
       pRemoteCharacteristic->writeValue(AutoFocus,4);
    }
     isAF = !isAF;
  }
}

int Batterycheck(void){
  int V = analogReadMilliVolts(BTC);
  int gap = previousVoltage - V;
  if((gap < 50)&&(gap >= 0)){
    previousVoltage = V;
  }
  return previousVoltage;
}

int MeasureLight(void){
  float outputrate;
  float lux = lightMeter.readLightLevel();
  float ev_now = log(lux/10)/(log(2)) + 9.575;
  if(iso == true){
    ev_now = ev_now - 3.08;
  } 
  if((ev_now > 13)&&(ShutterPoint != 0)){
    ev_now = ev_now + 0.45;
    float ev_set = ((log(64) - log(signalf[ShutterPoint])))/(log(2)) + 2;
    outputrate = ev_now - ev_set;
    if(abs(outputrate) <= 0.42 ){
      return 0;
    }else if(outputrate > 0){
      return -1;
    }else if (outputrate < 0){
      return 1;
    }
  }else if((ShutterPoint != 0)){
    float ev_set = ((log(64) - log(signalf[ShutterPoint])))/(log(2)) + 2;
    ev_now = ev_now - 1;
    outputrate = ev_now - ev_set;
    if(abs(outputrate) <= 0.42 ){
      return 0;
    }else if(outputrate > 0){
      return -1;
    }else if (outputrate < 0){
      return 1;
    }
  }
}

void isAP(void){
  isApateur = !isApateur;
}

void setup() {
  //设置输入输出引脚
  Serial.begin(115200);
  pinMode(DOSCAN,OUTPUT);
  pinMode(CONNECTED,OUTPUT);
  pinMode(DelaySignal,OUTPUT);
  pinMode(DialSwitchB,INPUT_PULLUP);
  pinMode(DialSwitchA,INPUT_PULLUP);
  pinMode(ModelSwitch,INPUT_PULLUP);
  pinMode(CopeSwitch,INPUT_PULLUP);
  //显示设置初始化
  u8g2.begin();
  //用EEProm存储设备MAC地址：初始化
  ServeAddress1.begin(2048);
  //设置按键：双击归零
  button.attachDoubleClick(DoubleClick);
  //设置按键：单击选择调整的数据
  button.attachClick(isAP);
  //button2.attachClick(isAP);
  //设置按键2：单机进入注册
  button.attachLongPressStop(SingleClick);
  //设置复位按键：单击进入复位注册程序
  attachInterrupt(DialSwitchA,SWH,CHANGE);
  //初始化蓝牙，设置名字为：
  Model = digitalRead(ModelSwitch);
  analogSetAttenuation(ADC_11db);
  analogReadResolution(12);
  BLEDevice::init("My Polaroid Controller");
  Wire.begin(13,14);
  while (!lightMeter.begin(BH1750::CONTINUOUS_LOW_RES_MODE)){
    
  }
  RST = digitalRead(CopeSwitch);
  String CHECK = ServeAddress1.readString(0);
  if(RST == false || CHECK.length() == 0){
    ServeAddress = "";
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_luBIS10_tf);
    u8g2.setFontRefHeightExtendedText();
    u8g2.setDrawColor(1);
    u8g2.setFontPosTop();
    u8g2.setFontDirection(0);
    u8g2.drawStr(0,8,"Match");
    u8g2.sendBuffer();
  }else{
    ServeAddress = ServeAddress1.readString(0);
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_luBIS10_tf);
    u8g2.setFontRefHeightExtendedText();
    u8g2.setDrawColor(1);
    u8g2.setFontPosTop();
    u8g2.setFontDirection(0);
    u8g2.drawStr(0,8,"Scan");
    u8g2.sendBuffer();
  }
  BLEScan* pBLEScan = BLEDevice::getScan(); 
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks()); 
  pBLEScan->setActiveScan(true); 
  pBLEScan->setInterval(100); 
  pBLEScan->setWindow(80); 
  digitalWrite(DOSCAN,HIGH);
  //BLEScanResults foundDevices = pBLEScan->start(3,false);
  digitalWrite(CONNECTED,HIGH);
  while(ServeAddress.length() == 0){
    BLEScanResults foundDevices = pBLEScan->start(3,false);
    int count = 0;
    int Mark;
    for(uint32_t k=0;k<foundDevices.getCount();k++){
      BLEAdvertisedDevice Temp =foundDevices.getDevice(k);
      Serial.printf("\n");
      Serial.print(Temp.getAddress().toString().c_str());
      Serial.printf("\n");
      //for(int t=0;t<Temp.getManufacturerData().length();t++){
      int ID = Temp.getManufacturerData()[2];
      //Serial.printf("%x ",ID);
      //}
      if(ID == 0x31){
        count++;
        Mark = k;
      }
      //Serial.printf("\n");
      //delay(200);
      //delay(50);    
     }
     while(count != 1 && count != 0){
        u8g2.clearBuffer();
        u8g2.setFont(u8g2_font_luBIS10_tf);
        u8g2.setFontRefHeightExtendedText();
        u8g2.setDrawColor(1);
        u8g2.setFontPosTop();
        u8g2.setFontDirection(0);
        u8g2.drawStr(0,8,"Multiple");
        u8g2.drawStr(0,33,"Devices");
        Serial.println("too many");
        u8g2.sendBuffer();
        if(digitalRead(CopeSwitch)== false){
          u8g2.clearBuffer();
          u8g2.setFont(u8g2_font_luBIS10_tf);
          u8g2.setFontRefHeightExtendedText();
          u8g2.setDrawColor(1);
          u8g2.setFontPosTop();
          u8g2.setFontDirection(0);
          u8g2.drawStr(0,8,"Match");
          u8g2.sendBuffer();
          break;
        }
      }
      while(count == 0){
        u8g2.clearBuffer();
        u8g2.setFont(u8g2_font_luBIS10_tf);
        u8g2.setFontRefHeightExtendedText();
        u8g2.setDrawColor(1);
        u8g2.setFontPosTop();
        u8g2.setFontDirection(0);
        u8g2.drawStr(0,8,"No");
        u8g2.drawStr(0,33,"Found");
        u8g2.sendBuffer();
        //Serial.println("no foundw");
        if(digitalRead(CopeSwitch)== false){
          u8g2.clearBuffer();
          u8g2.setFont(u8g2_font_luBIS10_tf);
          u8g2.setFontRefHeightExtendedText();
          u8g2.setDrawColor(1);
          u8g2.setFontPosTop();
          u8g2.setFontDirection(0);
          u8g2.drawStr(0,8,"Match");
          u8g2.sendBuffer();
          break;
        }
      }
      if(count == 1){
        pServer = new BLEAdvertisedDevice(foundDevices.getDevice(Mark));
        String A = foundDevices.getDevice(Mark).getAddress().toString().c_str();
        ServeAddress1.writeString(0,A);
        ServeAddress1.commit();
        doConnect = true;
        doSacn = false;
        ServeAddress = A;
        Serial.println(ServeAddress);
      }
  }
}

void loop() {
  int Time_E = millis();
  while(doSacn == true){
    BLEDevice::getScan()->clearResults();
    BLEScanResults foundDevices = BLEDevice::getScan()->start(3,false);
    for(uint32_t k=0;k<foundDevices.getCount();k++){
      String C = foundDevices.getDevice(k).getAddress().toString().c_str();
      Serial.println(C);
      Serial.println("!");
      Serial.println(ServeAddress);
      if(C == ServeAddress){
        pServer = new BLEAdvertisedDevice(foundDevices.getDevice(k));
        doConnect = true;
        doSacn = false;
        break;
       }
     }
     if(doSacn == true){
      u8g2.clearBuffer();
      u8g2.setFont(u8g2_font_luBIS10_tf);
      u8g2.setFontRefHeightExtendedText();
      u8g2.setDrawColor(1);
      u8g2.setFontPosTop();
      u8g2.setFontDirection(0);
      u8g2.drawStr(0,8,"Connect");
      u8g2.drawStr(0,33,"Failed");
      u8g2.sendBuffer();
      if(millis() - Time_E > 30000){
         isSleep = true;
         break;
      }
     }
     StateDisplay();
  }
  StateDisplay();
  //需要扫描时，进行扫描
  if(!isSleep == true) {
    int voltage = analogReadMilliVolts(BTC);
    previousVoltage = voltage;
    if(voltage > 1950){
      u8g2.drawLine(0, 24, 64, 24);
      u8g2.drawLine(0, 25, 64, 25);
    }else if((voltage<1950)&&(voltage>1850)){
      u8g2.drawLine(0, 24, 48, 24);
      u8g2.drawLine(0, 25, 48, 25);
    }else if((voltage<1850)&&(voltage>1750)){
      u8g2.drawLine(0, 24, 32, 24);
      u8g2.drawLine(0, 25, 32, 25);
    }else {
      u8g2.drawLine(0, 24, 16, 24);
      u8g2.drawLine(0, 25, 16, 25);
    }
    u8g2.sendBuffer();//扫描时间30秒，超时挂起，建议重启或者复位
 
    if(previousPoint == 1010){
      DoubleClick();
    }
    previousPoint = 1010;
  }
  // 如果找到设备就尝试连接设备
  StateDisplay();
  if (doConnect) {
    if (ConnectToServer()) { 
      connected = true;
    }
    else {
      doSacn = true;
    }
    doConnect = false;
  }
  // 已经连接就可以操纵宝丽来了
  StateDisplay();
  iso = SetIso(digitalRead(ModelSwitch)); 
  while (connected) {
    if(ShutterAction()){
      Time_C = millis();
    }
    int Time_D = millis();
    DisplayData(SignalDisplay[ShutterPoint],ApateurDisplay[ShutterPoint],DelayTimeDisplay[DelayPoint],Switch);
    StateDisplay();
    if(!pRemoteCharacteristic->canWrite()){
      connected = !connected;
    }
    if((Time_D-Time_C) > 300000){
       isSleep = true;
       connected = !connected;
       break;
    }
  }
    if(isSleep == true){
      u8g2.clearBuffer();
      u8g2.setFont(u8g2_font_luBIS10_tf);
      u8g2.setFontRefHeightExtendedText();
      u8g2.setDrawColor(1);
      u8g2.setFontPosTop();
      u8g2.setFontDirection(0);
      u8g2.drawStr(0,8,"Zzz");
      u8g2.sendBuffer();
      delay(50);
      isSleep = false;
      gpio_wakeup_enable(GPIO_NUM_2,GPIO_INTR_LOW_LEVEL);
      delay(50);
      esp_sleep_enable_gpio_wakeup();
      delay(50);
      shutdownled();
      delay(50);
      u8g2.setPowerSave(1);
      //rtc_gpio_pullup_en(GPIO_NUM_2);
      delay(50);
      esp_light_sleep_start();
      delay(100);
      //rtc_gpio_deinit(GPIO_NUM_2);
      gpio_wakeup_disable(GPIO_NUM_2);
      delay(50);
      u8g2.setPowerSave(0);
    }
}

//设置旋转编码器程序，使用外部中断
//只有连接上相机时，才能更改设置
void SWH(void){
  alv = digitalRead(DialSwitchA);
  blv = digitalRead(DialSwitchB);
  if((Switch == true)&&connected){
    if (flag == 0 && alv == LOW) {
    CW_1 = blv;
    flag = 1;
   }
    if (flag && alv) {
     CW_2 = !blv;  //取反是因为 alv,blv必然异步，一高一低。
    if (CW_1 && CW_2) {
      ShutterPoint++;
      if(ShutterPoint == 31){
        ShutterPoint = 0;
      }
    }
    if (CW_1 == false && CW_2 == false) {
      ShutterPoint--;
      if(ShutterPoint == -1){
        ShutterPoint = 30;
      }
    }
    flag = 0;
   }
  }else if((Switch == false)&&connected){
    if (flag == 0 && alv == LOW) {
    CW_1 = blv;
    flag = 1;
   }
    if (flag && alv) {
     CW_2 = !blv;  //取反是因为 alv,blv必然异步，一高一低。
    if (CW_1 && CW_2) {
      DelayPoint++;
      if(DelayPoint == 31){
        DelayPoint = 0;
      }
    }
    if (CW_1 == false && CW_2 == false) {
      DelayPoint--;
      if(DelayPoint == -1){
        DelayPoint = 30;
      }
    }
    flag = 0;
   }
  }
}
