#include <EasyTransfer.h>
#include <TroykaButton.h>
EasyTransfer ET;

#define THROTTLE_AXIS A0
#define PITCH_AXIS A3
#define YAW_AXIS A1
#define CAM_PITCH_AXIS A2
#define CAM_ROLL_AXIS A4
#define CAM_RESET_POSITION A5
#define RED_LED 13
#define GREEN_LED 8
#define RF_MODULE_SET_PIN 2
#define RF_MODULE_UART_SPEED 115200

struct SEND_DATA_STRUCTURE {
  int throttle = 0;
  int pitch = 0;
  int yaw = 0;
  int camPitch = 0;
  int camRoll = 0;
};
int Px = 90;
int Py = 90;
long lastTimeConnect = 0;
bool connectFlag = false;
long AsixTime = 0;
long lastTimeSendData = 0;
SEND_DATA_STRUCTURE mydata;
TroykaButton resetCamPosition(CAM_RESET_POSITION);
void setup() {
  Serial.begin(115200);
  Serial1.begin(RF_MODULE_UART_SPEED);
  ET.begin(details(mydata), &Serial1);
  pinMode(RF_MODULE_SET_PIN, OUTPUT);
  digitalWrite(RF_MODULE_SET_PIN, HIGH);
  pinMode(RED_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
}

void loop() {
  resetCamPosition.read();
  while (Serial1.available() > 0) {
    char b = Serial1.read();
    if (b == 'G') {
      connectFlag = true;
      lastTimeConnect = millis();
    }
  }
  if (millis() - lastTimeConnect > 3000) {
    connectFlag = false;
  }
  if (connectFlag) {
    digitalWrite(GREEN_LED, HIGH);
    digitalWrite(RED_LED, LOW);
  } else {
    digitalWrite(RED_LED, HIGH);
    digitalWrite(GREEN_LED, LOW);
  }
  if (resetCamPosition.isClick()) {
    Px = 90;
    Py = 90;
  } else {
    int Vx = map(analogRead(CAM_PITCH_AXIS), 0, 1023, -4, 4);
    int Vy = map(analogRead(CAM_ROLL_AXIS), 0, 1023, -4, 4);
    if (millis() - AsixTime > 50) {
      if (Vx < -1 || Vx > 1) {
        Px += Vx;
      }
      if (Vy < -1 || Vy > 1) {
        Py += Vy;
      }
      AsixTime = millis();
    }
  }
  mydata.camPitch = Px;
  mydata.camRoll = Py;
  int temp = constrain(map(analogRead(THROTTLE_AXIS), 600, 1023, 0, 255), 0, 255);
  if (temp < 20) {
    mydata.throttle =  0;
  } else {
    mydata.throttle =  temp;
  }
  mydata.pitch = constrain(map(analogRead(PITCH_AXIS), 0, 1023, 0, 180), 0, 180);
  temp = constrain(map(analogRead(YAW_AXIS), 0, 1023, -255, 255), -255, 255);
  if ((temp > -20) && (temp < 20)) {
    mydata.yaw = 0;
  } else {
    mydata.yaw = temp;
  }
  if (millis() - lastTimeSendData > 100) {
    ET.sendData();
    lastTimeSendData = millis();
    Serial.println("Throttle = " + String(constrain(map(analogRead(THROTTLE_AXIS), 600, 1023, 0, 255), 0, 255)) + " Yaw = " + String(constrain(map(analogRead(YAW_AXIS), 0, 1023, -255, 255), -255, 255)));
  }
}