#include <Wire.h>
#include <SPI.h>
#include <ArduinoRS485.h>
#include <SoftwareSerial.h>
#include <Adafruit_BMP280.h>
#include <MPU6050_tockn.h>
#include <EEPROM.h>
#include <SimpleKalmanFilter.h>
///////////КОНСТАНТЫ/////////////
static const int RXPin = 12, TXPin = 11;
static const uint32_t GPSBaud = 9600;
#define BMP_SCK  (8)
#define BMP_MISO (5)
#define BMP_MOSI (7)
#define BMP_CS   (6)
String jsonString;
float tmp;
float pressure;
float temperature;
////////////////////////
int launch = 0;

///////////Инициализация устройств///////
Adafruit_BMP280 bmp(BMP_CS, BMP_MOSI, BMP_MISO,  BMP_SCK);
MPU6050 mpu6050(Wire);
SimpleKalmanFilter gyrokfx(0.1, 0.11, 0.01);SimpleKalmanFilter gyrokfy(2, 1, 0.01);SimpleKalmanFilter gyrokfz(2, 1, 0.01);
SimpleKalmanFilter bmpkf(2, 1, 0.01);
SimpleKalmanFilter fuelkf(2, 2, 0.01);
SoftwareSerial ss(RXPin, TXPin);
void setup()
{
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  digitalWrite(3, HIGH);
  digitalWrite(4, LOW);
  Serial.begin(38400);
  RS485.begin(38400);
  //ss.begin(GPSBaud);
  launch = EEPROM_int_read(0);
  //Serial.print("Launch count: ");Serial.println(launch);
  bmp.begin(); // Инициализация датчика BMP280
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  delay(100);
  launch++;
  EEPROM_int_write(0, launch);
  digitalWrite(4, HIGH);
  digitalWrite(3, LOW);
}
// чтение
float EEPROM_float_read(int addr) {
  byte raw[4];
  for(byte i = 0; i < 4; i++) raw[i] = EEPROM.read(addr+i);
  float &num = (float&)raw;
  return num;
}

// запись
void EEPROM_float_write(int addr, float num) {
  byte raw[4];
  (float&)raw = num;
  for(byte i = 0; i < 4; i++) EEPROM.write(addr+i, raw[i]);
}

// запись
void EEPROM_int_write(int addr, int num) {
  byte raw[2];
  (int&)raw = num;
  for(byte i = 0; i < 2; i++) EEPROM.write(addr+i, raw[i]);
}
// чтение
int EEPROM_int_read(int addr) {
  byte raw[2];
  for(byte i = 0; i < 2; i++) raw[i] = EEPROM.read(addr+i);
  int &num = (int&)raw;
  return num;
}

void loop()
{
  pressure = bmp.readPressure()/100;temperature = bmp.readTemperature();
  RS485.beginTransmission();
  mpu6050.update();
  RS485.print("{\"status\":\"1\", \"launches\":\"");RS485.print(launch);RS485.print("\",");RS485.print("\"gyroscope\":");
  RS485.print("{\"angleX\":\"");RS485.print(gyrokfx.updateEstimate(mpu6050.getAngleX()));RS485.print("\",");
  RS485.print("\"angleY\":\"");RS485.print(gyrokfy.updateEstimate(mpu6050.getAngleY()));RS485.print("\",");
  RS485.print("\"angleZ\":\"");RS485.print(gyrokfz.updateEstimate(mpu6050.getAngleZ()));RS485.print("\"}, \"meteo\":");
  //////////////////////////////////////////////////////////
  RS485.print("{\"pressure\":\"");RS485.print(pressure);RS485.print("\",");
  RS485.print("\"temperature\":\"");RS485.print(temperature);RS485.print("\"}");//RS485.print("\"}, \"gps\":");
  //////////////////////////////////////////////////////////
  RS485.println("}");
  //////////////////////////////////////////////////////////
  RS485.endTransmission();