/*
 * 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"

#define HALLSTICK_BUFF_SIZE             ( 512 )
#define FLYSKY_HALL_BAUDRATE            ( 921600 )
#define FLYSKY_HALL_CHANNEL_COUNT       ( 4 )

#define MAX_ADC_CHANNEL_VALUE           ( 4095 )
#define MIN_ADC_CHANNLE_VALUE           ( 0 )
#define MIDDLE_ADC_CHANNLE_VALUE        ( 2047 )

#define FLYSKY_HALL_PROTOLO_HEAD        0x55
#define FLYSKY_HALL_RESP_TYPE_CALIB     0x0e
#define FLYSKY_HALL_RESP_TYPE_VALUES    0x0c
#define FLYSKY_HALL_ERROR_OFFSET        10

typedef  struct
{
  signed short min;
  signed short mid;
  signed short max;
} STRUCT_STICK_CALIBRATION;

typedef  struct
{
  STRUCT_STICK_CALIBRATION sticksCalibration[4];
  unsigned char reststate;
  unsigned short CRC16;
} STRUCT_STICK_CALIBRATION_PACK;

typedef  struct
{
  signed short channel[4];
  unsigned char  stickState;
  unsigned short CRC16;
} STRUCT_CHANNEL_PACK;

typedef  union
{
  STRUCT_STICK_CALIBRATION_PACK channelPack;
  STRUCT_CHANNEL_PACK sticksCalibrationPack;
} UNION_DATA;

typedef  struct
{
  unsigned char start;
  unsigned char senderID:2;
  unsigned char receiverID:2;
  unsigned char packetID:4;
  unsigned char length;
  UNION_DATA    payload;
} STRUCT_HALLDATA;

typedef  struct
{
  unsigned char senderID:2;
  unsigned char receiverID:2;
  unsigned char packetID:4;
} STRUCT_HALLID;

typedef union
{
  STRUCT_HALLID hall_Id;
  unsigned char ID;
} STRUCT_ID;


typedef union
{
  STRUCT_HALLDATA halldat;
  unsigned char buffer[30];
} UNION_HALLDATA;


typedef  struct
{
  unsigned char head;
  STRUCT_ID hallID;
  unsigned char length;
  unsigned char data[HALLSTICK_BUFF_SIZE];
  unsigned char reserved[15];
  unsigned short checkSum;
  unsigned char stickState;
  unsigned char startIndex;
  unsigned char endIndex;
  unsigned char index;
  unsigned char dataIndex;
  unsigned char deindex;
  unsigned char completeFlg;
  unsigned char status;
  unsigned char recevied;
  unsigned char msg_OK;
} STRUCT_HALL;

enum
{
  GET_START = 0,
  GET_ID,
  GET_LENGTH,
  GET_DATA,
  GET_STATE,
  GET_CHECKSUM,
  CHECKSUM,
};

enum HALLSTICK_SEND_STATE_E {
  HALLSTICK_SEND_STATE_IDLE,
  HALLSTICK_STATE_SEND_RESET,
  HALLSTICK_STATE_GET_CONFIG,
  HALLSTICK_STATE_GET_FIRMWARE,
  HALLSTICK_STATE_UPDATE_FW
};

enum TRANSFER_DIR_E {
  TRANSFER_DIR_HALLSTICK,
  TRANSFER_DIR_TXMCU,
  TRANSFER_DIR_HOSTPC,
  TRANSFER_DIR_RFMODULE,
};


DMAFifo<HALLSTICK_BUFF_SIZE> hallDMAFifo __DMA (FLYSKY_HALL_DMA_Stream_RX);
Fifo<uint8_t, HALLSTICK_BUFF_SIZE> hallStickTxFifo;
unsigned char HallCmd[264] __DMA;

STRUCT_HALL HallProtocol = { 0 };
STRUCT_HALL HallProtocolTx = { 0 };
signed short hall_raw_values[FLYSKY_HALL_CHANNEL_COUNT];
STRUCT_STICK_CALIBRATION hall_calibration[FLYSKY_HALL_CHANNEL_COUNT] = { {0, 0, 0} };
unsigned short hall_adc_values[FLYSKY_HALL_CHANNEL_COUNT];

/* crc16 implementation according to CCITT standards */
const unsigned short CRC16Table[256]= {
  0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
  0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,
  0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,
  0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de,
  0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,
  0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d,
  0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,
  0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc,
  0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,
  0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b,
  0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,
  0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a,
  0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,
  0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49,
  0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,
  0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78,
  0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,
  0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067,
  0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,
  0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256,
  0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,
  0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405,
  0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,
  0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634,
  0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,
  0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3,
  0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,
  0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92,
  0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,
  0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1,
  0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,
  0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0
};

const uint8_t sticks_mapping[4] = { 0 /*STICK1*/,  1/*STICK2*/, 2/*STICK3*/, 3 /*STICK4*/};

unsigned short calc_crc16(void *pBuffer,unsigned char BufferSize)
{
  unsigned short crc16;
  crc16 = 0xffff;
  while (BufferSize)
  {
    crc16 = (crc16 << 8) ^ CRC16Table[((crc16>>8) ^ (*(unsigned char *)pBuffer)) & 0x00ff];
    pBuffer = (void *)((unsigned char *)pBuffer + 1);
    BufferSize--;
  }
  return crc16;
}

uint16_t get_flysky_hall_adc_value(uint8_t ch)
{
  if (ch >= FLYSKY_HALL_CHANNEL_COUNT)
  {
    return 0;
  }

#if defined(FLYSKY_HALL_STICKS_REVERSE)
  ch = sticks_mapping[ch];

  return MAX_ADC_CHANNEL_VALUE - hall_adc_values[ch];
#else
  if (ch < 2)
  {
    return MAX_ADC_CHANNEL_VALUE - hall_adc_values[ch];
  }

  return hall_adc_values[ch];
#endif
}

void HallSendBuffer(uint8_t * buffer, uint32_t count)
{
  for(uint32_t idx = 0; buffer != HallCmd && idx < count; idx++)
  {
    HallCmd[idx] = buffer[idx];
  }
  DMA_InitTypeDef DMA_InitStructure;
  DMA_DeInit(FLYSKY_HALL_DMA_Stream_TX);
  DMA_InitStructure.DMA_Channel = FLYSKY_HALL_DMA_Channel;
  DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&FLYSKY_HALL_SERIAL_USART->DR);
  DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
  DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(HallCmd);
  DMA_InitStructure.DMA_BufferSize = count;
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
  DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
  DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
  DMA_Init(FLYSKY_HALL_DMA_Stream_TX, &DMA_InitStructure);
  DMA_Cmd(FLYSKY_HALL_DMA_Stream_TX, ENABLE);
  USART_DMACmd(FLYSKY_HALL_SERIAL_USART, USART_DMAReq_Tx, ENABLE);
  DMA_ITConfig(FLYSKY_HALL_DMA_Stream_TX, DMA_IT_TC, ENABLE);

  /* enable interrupt and set it's priority */
  NVIC_EnableIRQ(FLYSKY_HALL_SERIAL_TX_DMA_Stream_IRQn);
  NVIC_SetPriority(FLYSKY_HALL_SERIAL_TX_DMA_Stream_IRQn, 7);
}

extern "C" void FLYSKY_HALL_TX_DMA_Stream_IRQHandler(void)
{
  DEBUG_INTERRUPT(INT_TELEM_DMA);

  if (DMA_GetITStatus(FLYSKY_HALL_DMA_Stream_TX, FLYSKY_HALL_DMA_TX_FLAG_TC))
  {
    DMA_ClearITPendingBit(FLYSKY_HALL_DMA_Stream_TX, FLYSKY_HALL_DMA_TX_FLAG_TC);
    FLYSKY_HALL_SERIAL_USART->CR1 |= USART_CR1_TCIE;
  }
}

uint8_t HallGetByte(uint8_t * byte)
{
  return hallDMAFifo.pop(*byte);
}


void reset_hall_stick( void )
{
  unsigned short crc16 = 0xffff;

  HallCmd[0] = FLYSKY_HALL_PROTOLO_HEAD;
  HallCmd[1] = 0xD1;
  HallCmd[2] = 0x01;
  HallCmd[3] = 0x01;

  crc16 = calc_crc16(HallCmd, 4);

  HallCmd[4] = crc16 & 0xff;
  HallCmd[5] = crc16 >>8 & 0xff;

  HallSendBuffer( HallCmd, 6);
}

void get_hall_config( void )
{
  unsigned short crc16 = 0xffff;

  HallCmd[0] = FLYSKY_HALL_PROTOLO_HEAD;
  HallCmd[1] = 0xD1;
  HallCmd[2] = 0x01;
  HallCmd[3] = 0x00;

  crc16 = calc_crc16(HallCmd, 4); // 2B 2C

  HallCmd[4] = crc16 & 0xff;
  HallCmd[5] = crc16 >>8 & 0xff ;

  HallSendBuffer( HallCmd, 6);
}

void flysky_hall_stick_init()
{
  NVIC_InitTypeDef NVIC_InitStructure;
  NVIC_InitStructure.NVIC_IRQChannel = FLYSKY_HALL_SERIAL_RX_DMA_Stream_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  USART_InitTypeDef USART_InitStructure;
  GPIO_InitTypeDef GPIO_InitStructure;

  GPIO_PinAFConfig(FLYSKY_HALL_SERIAL_GPIO, FLYSKY_HALL_SERIAL_RX_GPIO_PinSource, FLYSKY_HALL_SERIAL_GPIO_AF);
  GPIO_PinAFConfig(FLYSKY_HALL_SERIAL_GPIO, FLYSKY_HALL_SERIAL_TX_GPIO_PinSource, FLYSKY_HALL_SERIAL_GPIO_AF);

  GPIO_InitStructure.GPIO_Pin = FLYSKY_HALL_SERIAL_TX_GPIO_PIN | FLYSKY_HALL_SERIAL_RX_GPIO_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_Init(FLYSKY_HALL_SERIAL_GPIO, &GPIO_InitStructure);

  USART_InitStructure.USART_BaudRate = FLYSKY_HALL_BAUDRATE;
  USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  USART_InitStructure.USART_StopBits = USART_StopBits_1;
  USART_InitStructure.USART_Parity = USART_Parity_No;
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
  USART_Init(FLYSKY_HALL_SERIAL_USART, &USART_InitStructure);

  DMA_Cmd(FLYSKY_HALL_DMA_Stream_RX, DISABLE);
  USART_DMACmd(FLYSKY_HALL_SERIAL_USART, USART_DMAReq_Rx, DISABLE);
  DMA_DeInit(FLYSKY_HALL_DMA_Stream_RX);

  DMA_InitTypeDef DMA_InitStructure;
  hallDMAFifo.clear();

  USART_ITConfig(FLYSKY_HALL_SERIAL_USART, USART_IT_RXNE, DISABLE);
  USART_ITConfig(FLYSKY_HALL_SERIAL_USART, USART_IT_TXE, DISABLE);

  DMA_InitStructure.DMA_Channel = FLYSKY_HALL_DMA_Channel;
  DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&FLYSKY_HALL_SERIAL_USART->DR);
  DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(hallDMAFifo.buffer());
  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
  DMA_InitStructure.DMA_BufferSize = hallDMAFifo.size();
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
  DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
  DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
  DMA_Init(FLYSKY_HALL_DMA_Stream_RX, &DMA_InitStructure);
  USART_DMACmd(FLYSKY_HALL_SERIAL_USART, USART_DMAReq_Rx, ENABLE);
  USART_Cmd(FLYSKY_HALL_SERIAL_USART, ENABLE);
  DMA_Cmd(FLYSKY_HALL_DMA_Stream_RX, ENABLE);

  reset_hall_stick();
}

static int parse_ps_state = 0;
void Parse_Character(STRUCT_HALL *hallBuffer, unsigned char ch)
{
  if (parse_ps_state != 0) return;
  parse_ps_state = 1;

  switch( hallBuffer->status )
  {
    case GET_START:
    {
      if ( FLYSKY_HALL_PROTOLO_HEAD == ch )
      {
        hallBuffer->head  = FLYSKY_HALL_PROTOLO_HEAD;
        hallBuffer->status = GET_ID;
        hallBuffer->msg_OK = 0;
      }
      break;
    }
    case GET_ID:
    {
      hallBuffer->hallID.ID = ch;
      hallBuffer->status = GET_LENGTH;
      break;
    }
    case GET_LENGTH:
    {
      hallBuffer->length = ch;
      hallBuffer->dataIndex = 0;
      hallBuffer->status = GET_DATA;
      if( 0 == hallBuffer->length )
      {
        hallBuffer->status = GET_CHECKSUM;
        hallBuffer->checkSum=0;
      }
      break;
    }
    case GET_DATA:
    {
      hallBuffer->data[hallBuffer->dataIndex++] = ch;
      if( hallBuffer->dataIndex >= hallBuffer->length)
      {
        hallBuffer->checkSum = 0;
        hallBuffer->dataIndex = 0;
        hallBuffer->status = GET_STATE;
      }
      break;
    }
    case GET_STATE:
    {
      hallBuffer->checkSum = 0;
      hallBuffer->dataIndex = 0;
      hallBuffer->status = GET_CHECKSUM;
    }
    case GET_CHECKSUM:
    {
      hallBuffer->checkSum |= ch << ((hallBuffer->dataIndex++) * 8);
      if( hallBuffer->dataIndex >= 2 )
      {
        hallBuffer->dataIndex = 0;
        hallBuffer->status = CHECKSUM;
      }
      else
      {
        break;
      }
    }
    case CHECKSUM:
    {
      if(hallBuffer->checkSum == calc_crc16( (U8*)&hallBuffer->head, hallBuffer->length + 3 ) )
      {
        hallBuffer->msg_OK = 1;
        goto Label_restart;
      }
      else
      {
        goto Label_error;
      }
    }
  }

  goto exit;

  Label_error:
  Label_restart:
    hallBuffer->status = GET_START;
  exit:
    parse_ps_state = 0;
    return;
}

void convert_hall_to_adc_value( void )
{
    uint16_t value;

    for ( uint8_t channel = 0; channel < 4; channel++ )
    {
        if (hall_raw_values[channel] < hall_calibration[channel].mid)
        {
            value = hall_calibration[channel].mid - (hall_calibration[channel].min+FLYSKY_HALL_ERROR_OFFSET);
            value = ( MIDDLE_ADC_CHANNLE_VALUE * (hall_calibration[channel].mid - hall_raw_values[channel] ) ) / ( value );

            if (value >= MIDDLE_ADC_CHANNLE_VALUE ) {
                value = MIDDLE_ADC_CHANNLE_VALUE;
            }

            hall_adc_values[channel] = MIDDLE_ADC_CHANNLE_VALUE - value;
        }
        else
        {
            value = (hall_calibration[channel].max - FLYSKY_HALL_ERROR_OFFSET) - hall_calibration[channel].mid;

            value = ( MIDDLE_ADC_CHANNLE_VALUE * (hall_raw_values[channel] - hall_calibration[channel].mid ) ) / (value );

            if (value >= MIDDLE_ADC_CHANNLE_VALUE )
            {
                value = MIDDLE_ADC_CHANNLE_VALUE;
            }

            hall_adc_values[channel] = MIDDLE_ADC_CHANNLE_VALUE + value + 1;
        }
    }
}

/* Run it in 1ms timer routine */
void flysky_hall_stick_loop(void)
{
    static tmr10ms_t lastConfigTime = get_tmr10ms();
    bool log = 0;

    uint8_t byte;

    while(HallGetByte(&byte))
    {
        HallProtocol.index++;

        Parse_Character(&HallProtocol, byte);
        if ( HallProtocol.msg_OK )
        {
            HallProtocol.msg_OK = 0;
            HallProtocol.stickState = HallProtocol.data[HallProtocol.length - 1];

            switch ( HallProtocol.hallID.hall_Id.receiverID )
            {
            case TRANSFER_DIR_TXMCU:
                if(HallProtocol.hallID.hall_Id.packetID == FLYSKY_HALL_RESP_TYPE_CALIB) {
                  memcpy(&hall_calibration, HallProtocol.data, sizeof(hall_calibration));
                }
                else if(HallProtocol.hallID.hall_Id.packetID == FLYSKY_HALL_RESP_TYPE_VALUES) {
                  memcpy(hall_raw_values, HallProtocol.data, sizeof(hall_raw_values));
                  convert_hall_to_adc_value();
                }
                break;
            }
        }
    }
    //check periodically  if calibration is correct
    if (get_tmr10ms() - lastConfigTime > 200)
    {
        //invalid calibration
        if(hall_calibration[0].max - hall_calibration[0].min < 1024) {
          TRACE("GET HALL CONFIG");
          get_hall_config();
          lastConfigTime = get_tmr10ms();
        }

        if (log)
        {
            for (int idx = 0; idx < HallProtocol.length + 5; idx++)
            {
                TRACE_NOCRLF(" %02X", *((uint8_t*)&HallProtocol + idx));
            }
            TRACE(";");
        }
    }
}