#include "sbus.h"
#include <HardwareSerial.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>

#define CH1 15
#define CH2 13
#define CH3 14
#define CH4 27
#define CH5 26
#define CH6 25
#define CH7 33
#define CH8 32
#define CH9 35
#define CH10 34
#define CH11 39
#define CH12 36
#define CH13 2
#define CH14 4
#define CH15 5
#define CH16 18
#define BTN1 19
#define BTN2 23

int potValue = 0;

bfs::SbusTx sbus_tx(&Serial2, 16, 17, true);
bfs::SbusData data, data0;
bool statusCH15 = false, statusCH16=false;

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

unsigned long tRead = 0;
unsigned long tShow = 0;
unsigned long tBTN = 0;
int tShowDisplay = 1000;
int CHShow = 0;

void calib()
{
  potValue = analogRead(CH1);
  data.ch[0] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH2);
  data.ch[1] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH3);
  data.ch[2] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH4);
  data.ch[3] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH5);
  data.ch[4] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH6);
  data.ch[5] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH7);
  data.ch[6] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH8);
  data.ch[7] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH9);
  data.ch[8] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH10);
  data.ch[9] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH11);
  data.ch[10] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH12);
  data.ch[11] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH13);
  data.ch[12] = map(potValue, 0,4095, 1100, 1900);
  potValue = analogRead(CH14);
  data.ch[13] = map(potValue, 0,4095, 1100, 1900);
  data.ch[14] = statusCH15? 1100:1900;
  data.ch[15] = statusCH16? 1100:1900;
  delay(20);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(CH1, INPUT);pinMode(CH2, INPUT);pinMode(CH3, INPUT);
  pinMode(CH4, INPUT);pinMode(CH5, INPUT);pinMode(CH6, INPUT);
  pinMode(CH7, INPUT);pinMode(CH8, INPUT);pinMode(CH9, INPUT);
  pinMode(CH10, INPUT);pinMode(CH11, INPUT);pinMode(CH12, INPUT);
  pinMode(CH13, INPUT);pinMode(CH14, INPUT);pinMode(CH15, INPUT);
  pinMode(CH16, INPUT);pinMode(BTN1, INPUT);pinMode(BTN2, INPUT);

  sbus_tx.Begin();

  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }

  display.clearDisplay();
  tRead = millis();
  tShow = tRead;
  tBTN = tShow;
}

void loop() {
  // put your main code here, to run repeatedly:
  
  if(digitalRead(CH15)==HIGH && (micros()-tBTN) > 500)
  {
    statusCH15 = !statusCH15;
    Serial.println("ch15");
    tBTN = millis();
  }
  if(digitalRead(CH16)==HIGH && (micros()-tBTN) > 500)
  {
    statusCH16 = !statusCH16;
    Serial.println("ch15");
    tBTN = millis();
  }

  if (millis() - tRead > 500)
  {
    potValue = analogRead(CH1);
    data.ch[0] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH2);
    data.ch[1] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH3);
    data.ch[2] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH4);
    data.ch[3] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH5);
    data.ch[4] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH6);
    data.ch[5] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH7);
    data.ch[6] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH8);
    data.ch[7] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH9);
    data.ch[8] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH10);
    data.ch[9] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH11);
    data.ch[10] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH12);
    data.ch[11] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH13);
    data.ch[12] = map(potValue, 0,4095, 1100, 1900);
    potValue = analogRead(CH14);
    data.ch[13] = map(potValue, 0,4095, 1100, 1900);
    data.ch[14] = statusCH15? 1100:1900;
    data.ch[15] = statusCH16? 1100:1900;
    /* Set the SBUS TX data to the received data */
    sbus_tx.data(data);
    /* Write the data to the servos */
    sbus_tx.Write();
    Serial.print("ch:");Serial.print(data.ch[0]);
    Serial.print("-");Serial.print(data.ch[1]);
    Serial.print("-");Serial.print(data.ch[2]);
    Serial.print("-");Serial.print(data.ch[3]);
    Serial.print("-");Serial.print(data.ch[4]);
    Serial.print("-");Serial.print(data.ch[5]);
    Serial.print("-");Serial.print(data.ch[6]);
    Serial.print("-");Serial.print(data.ch[7]);
    Serial.print("-");Serial.print(data.ch[8]);
    Serial.print("-");Serial.print(data.ch[9]);
    Serial.print("-");Serial.print(data.ch[10]);
    Serial.print("-");Serial.print(data.ch[11]);
    Serial.print("-");Serial.print(data.ch[12]);
    Serial.print("-");Serial.print(data.ch[13]);
    Serial.print("-");Serial.print(data.ch[14]);
    Serial.print("-");Serial.println(data.ch[15]);

    Serial.print("lll"); Serial.println(data.ch[15]-data.ch[14]);
    tRead = millis();
  }
  
  if(millis() - tShow > tShowDisplay)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    for(int i = 0; i<4; i++)
    {
      display.setCursor(0,i*16); 
      display.print("CH");
      display.print(i+CHShow+1);
      display.print(":");
      display.setCursor(64,i*16); 
      display.print(data.ch[i+CHShow]);
    }
    display.display();
    CHShow += 4;
    if (CHShow > 13) CHShow = 0;
    tShow = millis();
  }  
}