/*
 * Copyright (C) OpenTX
 *
 * Based on code named
 *   th9x - http://code.google.com/p/th9x
 *   er9x - http://code.google.com/p/er9x
 *   gruvin9x - http://code.google.com/p/gruvin9x
 *
 * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include "opentx.h"
#include "multi.h"

// for the  MULTI protocol definition
// see https://github.com/pascallanger/DIY-Multiprotocol-TX-Module
// file Multiprotocol/multiprotocol.h

#define MULTI_SEND_BIND                     (1 << 7)
#define MULTI_SEND_RANGECHECK               (1 << 5)
#define MULTI_SEND_AUTOBIND                 (1 << 6)

#define MULTI_CHANS                         16
#define MULTI_CHAN_BITS                     11

#define MULTI_NORMAL   0x00
#define MULTI_FAILSAFE 0x01
#define MULTI_DATA     0x02

static void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe);
void sendChannels(uint8_t moduleIdx);
static void sendD16BindOption(uint8_t moduleIdx);
#if defined(LUA)
static void sendSport(uint8_t moduleIdx);
static void sendHott(uint8_t moduleIdx);
static void sendDSM(uint8_t moduleIdx);
#endif

void multiPatchCustom(uint8_t moduleIdx)
{
  if (g_model.moduleData[moduleIdx].multi.customProto) {
    uint8_t type = g_model.moduleData[moduleIdx].getMultiProtocol() - 1;  // custom where starting at 1, otx list at 0
    int subtype = g_model.moduleData[moduleIdx].subType;

    g_model.moduleData[moduleIdx].multi.customProto = 0;

    if (type == 2) {  // multi PROTO_FRSKYD
      g_model.moduleData[moduleIdx].subType = 1;    // D8
      return;
    }
    else if (type == 14) { // multi PROTO_FRSKYX
      g_model.moduleData[moduleIdx].setMultiProtocol(2);
      switch (subtype) {
        case 0:       //D16-16
          g_model.moduleData[moduleIdx].subType = 0;
          break;
        case 1:       //D16-8
          g_model.moduleData[moduleIdx].subType = 2;
          break;
        case 2:       //EU-16
          g_model.moduleData[moduleIdx].subType = 4;
          break;
        case 3:       //EU-8
          g_model.moduleData[moduleIdx].subType = 5;
          break;
      }
      return;
    }
    else if (type == 24) {  // multi PROTO_FRSKYV
      g_model.moduleData[moduleIdx].setMultiProtocol(2);
      g_model.moduleData[moduleIdx].subType = 3;
      return;
    }
    if (type > 14)
      type -= 1;
    if (type > 24)
      type -= 1;
    g_model.moduleData[moduleIdx].setMultiProtocol(type);
  }
}

static void sendMulti(uint8_t moduleIdx, uint8_t b)
{
#if defined(HARDWARE_INTERNAL_MODULE)
  if (moduleIdx == INTERNAL_MODULE) {
    intmodulePulsesData.multi.sendByte(b);
  }
  else
#endif
    sendByteSbus(b);
}

static void sendFailsafeChannels(uint8_t moduleIdx)
{
  uint32_t bits = 0;
  uint8_t bitsavailable = 0;

  for (int i = 0; i < MULTI_CHANS; i++) {
    int16_t failsafeValue = g_model.failsafeChannels[i];
    int pulseValue;

    if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_HOLD || failsafeValue == FAILSAFE_CHANNEL_HOLD) {
      pulseValue = 2047;
    }
    else if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_NOPULSES || failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
      pulseValue = 0;
    }
    else {
      failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[moduleIdx].channelsStart + i) - 2 * PPM_CENTER;
      pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2046);
    }

    bits |= pulseValue << bitsavailable;
    bitsavailable += MULTI_CHAN_BITS;
    while (bitsavailable >= 8) {
      sendMulti(moduleIdx, (uint8_t) (bits & 0xff));
      bits >>= 8;
      bitsavailable -= 8;
    }
  }
}

void setupPulsesMulti(uint8_t moduleIdx)
{
  static int counter[2] = {0,0}; //TODO
  static uint8_t invert[2] = {0x00,        //internal
#if defined(PCBTARANIS) || defined(PCBHORUS)
    0x08        //external
#else
    0x00	//external
#endif
  };
  uint8_t type=MULTI_NORMAL;

  // Failsafe packets
  if (counter[moduleIdx] % 1000 == 0 && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_RECEIVER) {
    type|=MULTI_FAILSAFE;
  }

  // Invert telemetry if needed
  if (invert[moduleIdx] & 0x80 && !g_model.moduleData[moduleIdx].multi.disableTelemetry) {
    if (getMultiModuleStatus(moduleIdx).isValid()) {
      invert[moduleIdx] &= 0x08;    // Telemetry received, stop searching
    }
    else if (counter[moduleIdx] % 100 == 0) {
      invert[moduleIdx] ^= 0x08;  // Try inverting telemetry
    }
  }

  counter[moduleIdx]++;

  // Send header
  sendFrameProtocolHeader(moduleIdx, type&MULTI_FAILSAFE);

  // Send channels
  if (type & MULTI_FAILSAFE)
    sendFailsafeChannels(moduleIdx);
  else
    sendChannels(moduleIdx);

  // Multi V1.3.X.X -> Send byte 26, Protocol (bits 7 & 6), RX_Num (bits 5 & 4), invert, not used, disable telemetry, disable mapping
  if (moduleState[moduleIdx].mode == MODULE_MODE_SPECTRUM_ANALYSER) {
    sendMulti(moduleIdx, invert[moduleIdx] & 0x08);
  }
  else {
    sendMulti(moduleIdx, (uint8_t) (((g_model.moduleData[moduleIdx].getMultiProtocol() + 3) & 0xC0)
                                    | (g_model.header.modelId[moduleIdx] & 0x30)
                                    | (invert[moduleIdx] & 0x08)
                                    //| 0x04 // Future use
                                    | (g_model.moduleData[moduleIdx].multi.disableTelemetry << 1)
                                    | g_model.moduleData[moduleIdx].multi.disableMapping));
  }

  // Multi V1.3.X.X -> Send protocol additional data: max 9 bytes
  if (getMultiModuleStatus(moduleIdx).isValid()) {
    MultiModuleStatus &status = getMultiModuleStatus(moduleIdx);
    if (status.minor >= 3 && !(status.flags & 0x80)) { //Version 1.3.x.x or more and Buffer not full
      if ((IS_D16_MULTI(moduleIdx) || IS_R9_MULTI(moduleIdx)) && moduleState[moduleIdx].mode == MODULE_MODE_BIND) {
        sendD16BindOption(moduleIdx);//1 byte of additional data
      }
#if defined(LUA)
      // SPort send
      if (IS_D16_MULTI(moduleIdx) && outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT && outputTelemetryBuffer.size) {
        sendSport(moduleIdx);        //8 bytes of additional data
      }
      else if (IS_HOTT_MULTI(moduleIdx)) {
        sendHott(moduleIdx);        //1 byte of additional data
      }
      else if (IS_DSM_MULTI(moduleIdx)) {
        sendDSM(moduleIdx);         //7 bytes of additional data
      }
#endif
    }
  }
}

void setupPulsesMultiExternalModule()
{
#if defined(PPM_PIN_SERIAL)
  extmodulePulsesData.dsm2.serialByte = 0 ;
  extmodulePulsesData.dsm2.serialBitCount = 0 ;
#else
  extmodulePulsesData.dsm2.rest = getMultiSyncStatus(EXTERNAL_MODULE).getAdjustedRefreshRate();
  extmodulePulsesData.dsm2.index = 0;
#endif

  extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses;

  setupPulsesMulti(EXTERNAL_MODULE);
  putDsm2Flush();
}

#if defined(INTERNAL_MODULE_MULTI)
void setupPulsesMultiInternalModule()
{
  intmodulePulsesData.multi.initFrame();
  setupPulsesMulti(INTERNAL_MODULE);
}
#endif

void sendChannels(uint8_t moduleIdx)
{
  uint32_t bits = 0;
  uint8_t bitsavailable = 0;

  // byte 4-25, channels 0..2047
  // Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
  // Multi uses [204;1843] as [-100%;100%]
  for (int i = 0; i < MULTI_CHANS; i++) {
    int channel = g_model.moduleData[moduleIdx].channelsStart + i;
    int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;

    // Scale to 80%
    value = value * 800 / 1000 + 1024;
    value = limit(0, value, 2047);

    bits |= value << bitsavailable;
    bitsavailable += MULTI_CHAN_BITS;
    while (bitsavailable >= 8) {
      sendMulti(moduleIdx, (uint8_t) (bits & 0xff));
      bits >>= 8;
      bitsavailable -= 8;
    }
  }
}

void convertOtxProtocolToMulti(int *protocol, int *subprotocol)
{
  // Special treatment for the FrSky entry...
  if (*protocol == MODULE_SUBTYPE_MULTI_FRSKY +1) {
    if (*subprotocol == MM_RF_FRSKY_SUBTYPE_D8) {
      //D8
      *protocol = 3;
      *subprotocol = 0;
    } 
    else if (*subprotocol == MM_RF_FRSKY_SUBTYPE_D8_CLONED) {
      //D8
      *protocol = 3;
      *subprotocol = 1;
    } 
    else if (*subprotocol == MM_RF_FRSKY_SUBTYPE_V8) {
      //V8
      *protocol = 25;
      *subprotocol = 0;
    } 
    else {
      *protocol = 15;
      if (*subprotocol == MM_RF_FRSKY_SUBTYPE_D16_8CH)
        *subprotocol = 1;
      else if (*subprotocol == MM_RF_FRSKY_SUBTYPE_D16)
        *subprotocol = 0; // D16
      else if (*subprotocol == MM_RF_FRSKY_SUBTYPE_D16_LBT)
        *subprotocol = 2;
      else if (*subprotocol == MM_RF_FRSKY_SUBTYPE_D16_LBT_8CH)
        *subprotocol = 3;
      else
        *subprotocol = 4; // D16_CLONED
    }
  }
  else {
    // 15  for Multimodule is FrskyX or D16 which we map as a protocol of 3 (FrSky)
    // all protos > frskyx are therefore also off by one
    if (*protocol >= 15)
      *protocol = *protocol + 1;
    // 25 is again a FrSky *protocol (FrskyV) so shift again
    if (*protocol >= 25)
      *protocol = *protocol + 1;
  }
}

void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
{// byte 1+2, protocol information

  // Our enumeration starts at 0
  int type = g_model.moduleData[moduleIdx].getMultiProtocol() + 1;
  int subtype = g_model.moduleData[moduleIdx].subType;
  int8_t optionValue = g_model.moduleData[moduleIdx].multi.optionValue;

  uint8_t protoByte = 0;

  if (moduleState[moduleIdx].mode == MODULE_MODE_SPECTRUM_ANALYSER) {
    sendMulti(moduleIdx, (uint8_t) 0x54);  // Header byte
    sendMulti(moduleIdx, (uint8_t) 54);    // Spectrum custom protocol
    sendMulti(moduleIdx, (uint8_t) 0);
    sendMulti(moduleIdx, (uint8_t) 0);
    return;
  }

  if (moduleState[moduleIdx].mode == MODULE_MODE_BIND)
    protoByte |= MULTI_SEND_BIND;
  else if (moduleState[moduleIdx].mode ==  MODULE_MODE_RANGECHECK)
    protoByte |= MULTI_SEND_RANGECHECK;

  // rfProtocol
  if (type == MODULE_SUBTYPE_MULTI_DSM2 +1 ) {
    // Multi module in DSM mode wants the number of channels to be used as option value along with other flags
    if (optionValue & 0x01)
      optionValue = 0x80; // Max throw
    else
      optionValue = 0x00;
    if (g_model.moduleData[moduleIdx].multi.optionValue & 0x02)
      optionValue |= 0x40; // 11ms servo refresh
    optionValue |= sentModuleChannels(moduleIdx); //add number of channels
  }

  // Special treatment for the FrSky entry...
  convertOtxProtocolToMulti(&type, &subtype);

  // Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead
  // of sending Frsky D telemetry
  if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
    optionValue = optionValue | 0x80;

  // For custom protocol send unmodified type byte
  if (g_model.moduleData[moduleIdx].getMultiProtocol() == MM_RF_CUSTOM_SELECTED)
    type = g_model.moduleData[moduleIdx].getMultiProtocol();

  uint8_t headerByte = 0x55;
  // header, byte 0,  0x55 for proto 0-31, 0x54 for proto 32-63
  if (type & 0x20)
    headerByte &= 0xFE;

  if (failsafe)
    headerByte |= 0x02;

  sendMulti(moduleIdx, headerByte);

  // protocol byte
  protoByte |= (type & 0x1f);
  if (g_model.moduleData[moduleIdx].getMultiProtocol() != MODULE_SUBTYPE_MULTI_DSM2)
    protoByte |= (g_model.moduleData[moduleIdx].multi.autoBindMode << 6);

  sendMulti(moduleIdx, protoByte);

  // byte 2, subtype, powermode, model id
  sendMulti(moduleIdx, (uint8_t) ((g_model.header.modelId[moduleIdx] & 0x0f)
                           | ((subtype & 0x7) << 4)
                           | (g_model.moduleData[moduleIdx].multi.lowPowerMode << 7))
  );

  // byte 3
  sendMulti(moduleIdx, (uint8_t) optionValue);
}

void sendD16BindOption(uint8_t moduleIdx)
{
  uint8_t bind_opt = g_model.moduleData[moduleIdx].multi.receiverTelemetryOff ? 1 : 0;
  bind_opt |= g_model.moduleData[moduleIdx].multi.receiverHigherChannels ? 2 : 0;
  sendMulti(moduleIdx, bind_opt);
}

#if defined(LUA)
void sendSport(uint8_t moduleIdx)
{
  // example: B7 30 30 0C 80 00 00 00 13
  uint8_t j=0;

  // unstuff and remove crc
  for (uint8_t i = 0; i < outputTelemetryBuffer.size - 1 && j < 8; i++, j++) {
    if (outputTelemetryBuffer.data[i] == BYTE_STUFF) {
      i++;
      sendMulti(moduleIdx, outputTelemetryBuffer.data[i] ^ STUFF_MASK);
    }
    else {
      sendMulti(moduleIdx, outputTelemetryBuffer.data[i]);
    }
  }

  outputTelemetryBuffer.reset(); // empty buffer
}

void sendHott(uint8_t moduleIdx)
{
  if (Multi_Buffer && memcmp(Multi_Buffer, "HoTT", 4) == 0 && (Multi_Buffer[5] & 0x80) && (Multi_Buffer[5] & 0x0F) >= 0x07) {
    // HoTT Lua script is running
    sendMulti(moduleIdx, Multi_Buffer[5]);
  }
}

void sendDSM(uint8_t moduleIdx)
{
  // Multi_Buffer[0..2]=="DSM" -> Lua script is running
  // Multi_Buffer[3]==0x70 + len -> TX to RX data ready to be sent
  // Multi_Buffer[4..9]=6 bytes of TX to RX data
  // Multi_Buffer[10..25]=16 bytes of RX to TX data
  if (Multi_Buffer && memcmp(Multi_Buffer, "DSM", 3) == 0 && (Multi_Buffer[3] & 0xF8) == 0x70) {
    for(uint8_t i = 0; i < 7; i++) {
        sendMulti(moduleIdx, Multi_Buffer[3+i]);
    }
    Multi_Buffer[3] = 0x00;    // Data sent
  }
}
#endif