// some .ino file must include the SPI library, otherwise it's not available inside a7105.cpp
#include <SPI.h>
#include "common.h"
#include "manager.h"
#include "diagnostics.h"

#define BLYNK_PRINT Serial
#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>

#define AUTH_TOKEN "Your Auth Token"
#define WIFI_SSID  "Your WiFi name"
#define WIFI_PASS  "Your WiFi password"




void setup()
{
  Serial.begin(BAUDRATE);

  DEBUG_MSG("INIT...");
  manager_init();
  DEBUG_MSG("INIT OK");

  Blynk.begin(AUTH_TOKEN, WIFI_SSID, WIFI_PASS);
}

int hex2int(byte *arr, int startpos, int endpos)
{
    int base = 1;
    int ret = 0;
    for(int i = endpos; i >= startpos; i--) {
        int val = 0;
        if (arr[i] >= 'A' && arr[i] <= 'F')
           val = 10 + (arr[i] - 'A');
        else if (arr[i] >= 'a' && arr[i] <= 'f')
           val = 10 + (arr[i] - 'a');
        else if (arr[i] >= '0' && arr[i] <= '9')
           val = arr[i] - '0';
           
        ret += val * base;
        base *= 16;
    }
    return ret;
}

void send_response(int result_code, int* response)
{
    #ifdef SERIAL_ASCII
        Serial.print("command result ");
        Serial.print(String(result_code, HEX));
        for (int i=1; i <= response[0]; i++) {
            Serial.print(" "+String(response[i], HEX));
        }
        Serial.println();
    #else
        int sum = result_code;
        Serial.write(result_code);
        for (int i=1; i <= response[0]; i++) {
            Serial.write(response[i]);
            sum += response[i];
        }
        Serial.write((256 - (sum % 256)) & 0xFF);
    #endif
}

int response[7] = {0};
int copterid = 1;

//COPTER_BIND   0x01  copter type (see below)   Initiate binding process. Timing is very important here, so during this time (max. 500ms) the processor is blocked. Return values the copterid
BLYNK_WRITE(1) {
  if (param.asInt()) {
    int cmd = COPTER_BIND;
    int value = 1;
    int checksum = calculate_checksum(copterid, cmd, value);
    if (PROTOCOL_OK == manager_processcommand(copterid, cmd, value, checksum, response)) {
      copterid = response[1];
      BLYNK_LOG("Copter ID: %d", copterid);
    }
  }
}
//COPTER_THROTTLE   0x02  throttle value (range 0x00 - 0xFF)  set throttle (top/down) value
BLYNK_WRITE(2) {
  int cmd = COPTER_THROTTLE;
  int value = map(param.asInt(), 0, 1000, 0x00, 0xFF);
  int checksum = calculate_checksum(copterid, cmd, value);
  manager_processcommand(copterid, cmd, value, checksum, response);
}
//COPTER_RUDDER   0x03  rudder value (range 0x34 - 0xCC)  set rudder (rotate left/right) value
BLYNK_WRITE(3) {
  int cmd = COPTER_RUDDER;
  int value = map(param.asInt(), -100, 100, 0x34, 0xCC);
  int checksum = calculate_checksum(copterid, cmd, value);
  manager_processcommand(copterid, cmd, value, checksum, response);
}
//COPTER_AILERON  0x04  aileron value (range 0x45 - 0xC3)   set aileron (drift left/right) value
BLYNK_WRITE(4) {
  int cmd = COPTER_AILERON;
  int value = map(param.asInt(), -100, 100, 0x45, 0xC3);
  int checksum = calculate_checksum(copterid, cmd, value);
  manager_processcommand(copterid, cmd, value, checksum, response);
}
//COPTER_ELEVATOR   0x05  elevator value (range 0x3E - 0xBC)  set elevator (forward/backward) value
BLYNK_WRITE(5) {
  int cmd = COPTER_ELEVATOR;
  int value = map(param.asInt(), -100, 100, 0x3E, 0xBC);
  int checksum = calculate_checksum(copterid, cmd, value);
  manager_processcommand(copterid, cmd, value, checksum, response);
}
//COPTER_LED  0x06  enable or disable (1 or 0)  enable/disable leds
BLYNK_WRITE(6) {
  int cmd = COPTER_LED;
  int value = param.asInt();
  int checksum = calculate_checksum(copterid, cmd, value);
  manager_processcommand(copterid, cmd, value, checksum, response);
}

void loop()
{
    Blynk.run();    
    manager_loop();
}
