#include <stdint.h>
#include "user_interface.h"
#include "ets_sys.h"
#include "osapi.h"
#include "mem.h"
#include "gpio.h"
#include "espconn.h"
#include "os_type.h"
#include "mqtt.h"

#define DEBUG 1
/* Functions we will need to implement:
 * Send -- will handle all sending of all packets
 * Connect -- set up TCP connection and parameters
 * Publish -- send message to server
 * Subscribe -- we probably won't need this
 * We just want to connect, and publish info. We don't care about
 * security or QoS in this basic implementation.
 */

static os_timer_t oneTimer;
static os_timer_t testTimer;

LOCAL void ICACHE_FLASH_ATTR data_sent_callback(void *arg) {
  os_printf("Data sent!\n");
}

LOCAL void ICACHE_FLASH_ATTR data_recv_callback(void *arg, char *pdata, unsigned short len) {
  // deal with received data
  os_printf("Received data of length %d -- %s \r\n", len, pdata);
}

LOCAL void ICACHE_FLASH_ATTR connected_callback(void *arg) {
  struct espconn *pConn = arg;
  os_printf("Connected callback\n");
  espconn_regist_recvcb(pConn, (espconn_recv_callback)data_recv_callback);
  espconn_regist_sentcb(pConn, (espconn_sent_callback)data_sent_callback);
  // enable keepalive
  espconn_set_opt(pConn, ESPCONN_KEEPALIVE);
}

LOCAL void ICACHE_FLASH_ATTR reconnected_callback(void *arg, sint8 err) {
  os_printf("Reconnected?\n");
  os_printf("Error code: %d\n", err);
}

LOCAL void ICACHE_FLASH_ATTR disconnected_callback(void *arg) {
  os_printf("Disconnected\n");
}

LOCAL uint8_t ICACHE_FLASH_ATTR tcpConnect(void *arg) {
  mqtt_session_t *session = arg;
  struct ip_info ipConfig;
  LOCAL struct espconn conn;
  LOCAL struct _esp_tcp tcp_s;  
  if (wifi_station_get_connect_status() == STATION_GOT_IP && ipConfig.ip.addr != 0) {
    os_printf("Everything looks good!\n");
  }
  os_printf("Entered tcpConnect\n");
  wifi_get_ip_info(STATION_IF, &ipConfig);
  // set up basic TCP connection parameters
  os_printf("about to set up TCP params\n");
  conn.proto.tcp = &tcp_s;
  conn.type = ESPCONN_TCP;
  conn.proto.tcp->local_port = espconn_port();
  conn.proto.tcp->remote_port = session->port;
  conn.state = ESPCONN_NONE;
  os_memcpy(conn.proto.tcp->remote_ip, session->ip, 4);
  
  os_printf("About to register callbacks\n");
  // register callbacks
  espconn_regist_connectcb(&conn, (espconn_connect_callback)connected_callback);
  espconn_regist_reconcb(&conn, (espconn_reconnect_callback)reconnected_callback);
  espconn_regist_disconcb(&conn, (espconn_connect_callback)disconnected_callback);
  os_printf("About to connect\n");
  //make the connection
  if(espconn_connect(&conn) == 0) {
    os_printf("Connection successful\n");
  } else {
    os_printf("Connection error\n");
  }
  session->activeConnection = &conn;
  os_printf("About to return\n");
  return 0;
}

LOCAL uint8_t ICACHE_FLASH_ATTR mqtt_connect(mqtt_session_t *session, uint8_t *username, uint8_t *password) {
    
}

/************************************************************************************************/
/* Function: encodeLength								        */
/* Parameters: the length in bytes							        */
/* Returns: encoded multi-byte number per MQTT standards. The first byte this pointer	        */
/* returns is the number of bytes to follow.						        */
/* The MQTT standard uses a very strange method of encoding the lengths of the various sections */
/* of the packets. This makes it simpler to implement in code.				        */
/************************************************************************************************/


LOCAL uint8_t ICACHE_FLASH_ATTR *encodeLength(uint32_t trueLength) {
  uint8_t *encodedByte = os_zalloc(sizeof(uint8_t) * 5); // can't be more than 5 bytes
  uint8_t numBytes = 1;
  do {
    encodedByte[numBytes] = trueLength % 128;
    trueLength /= 128;
    if(trueLength > 0) {
      encodedByte[numBytes] |= 128;
    }
    numBytes++;
  } while(trueLength > 0);
  encodedByte[0] = numBytes - 1;
  
  return encodedByte;

}

/*************************************************************************************/
/* Function: mqtt_send								     */
/* Parameters: active mqtt_session_t, pointer to data, data length, and message type */
/* This function must do a few different things:				     */
/* * It must create the packet based on the message type and data		     */
/* * It must send the packet to the server					     */
/*************************************************************************************/
LOCAL uint8_t ICACHE_FLASH_ATTR mqtt_send(mqtt_session_t *session, uint8_t *data, uint32_t len, mqtt_message_type msgType) {
  /** First thing to do is check the packet type.
   * This will inform everything else we do
   */
#ifdef DEBUG
  os_printf("Entering mqtt_send!\n");
#endif
  LOCAL mqtt_packet_t packet;
  LOCAL mqtt_packet_t *pPacket = &packet;
  uint8_t *fullPacket;
  switch(msgType) {
    case MQTT_MSG_TYPE_CONNECT: ;
    // prepare packet for connect type
      const uint8_t varDefaults[10] = { 0x00, 0x04, 0x4D, 0x51, 0x54, 0x54, 0x04, 0xC2, 0x00, 0x32 };
      pPacket->fixedHeader = (uint8_t *)os_zalloc(sizeof(uint8_t) * 5); // fixed header cannot be longer than 5
      pPacket->fixedHeader[0] = ((uint8_t)msgType << 4) & 0xF0; // make sure lower 4 are clear
      // prepare variable header
      pPacket->varHeader = os_zalloc(sizeof(uint8_t) * 10); // 10 bytes for connect
      os_memcpy(pPacket->varHeader, varDefaults, 10); // copy defaults
      pPacket->varHeader_len = 10;
      // prepare payload
      uint32_t offset = 0;
      uint32_t maxPayloadLength = session->client_id_len + session->username_len + session->password_len + 12; // length can only take a maximum of 12 bytes
      pPacket->payload = (uint8_t *)os_zalloc(sizeof(uint8_t) * maxPayloadLength);
      // copy client id to payload
      // insert a 0
      os_memset(pPacket->payload + offset, 0, 1);
      offset += 1;
      os_memcpy(pPacket->payload + offset, &session->client_id_len, 1);
      offset += 1;
      os_memcpy(pPacket->payload + offset, session->client_id, session->client_id_len);
      offset += session->client_id_len;
      // copy username to payload
      // insert a 0
      os_memset(pPacket->payload + offset, 0, 1);
      offset += 1;
      os_memcpy(pPacket->payload + offset, &session->username_len, 1);
      offset += 1;
      os_memcpy(pPacket->payload + offset, session->username, session->username_len);
      offset += session->username_len;
      // Password
      // insert a 0
      os_memset(pPacket->payload + offset, 0, 1);
      offset += 1;
      os_memcpy(pPacket->payload + offset, &session->password_len, 1);
      offset += 1;
      os_memcpy(pPacket->payload + offset, session->password, session->password_len);
      offset += session->password_len;
      pPacket->payload_len = offset;
      os_printf("Total offset: %d", offset);
      uint8_t *remaining_len_encoded = encodeLength((uint32_t)(pPacket->varHeader_len + pPacket->payload_len));
      os_memcpy(pPacket->fixedHeader + 1, remaining_len_encoded + 1, remaining_len_encoded[0]); // remaining length = length of varHeader + length of payload
      pPacket->fixedHeader_len = remaining_len_encoded[0] + 1;
      // the full length is the length of the varHeader, payload, and fixedHeader
      pPacket->length = (pPacket->varHeader_len + pPacket->payload_len + pPacket->fixedHeader_len);
      os_printf("Packet length: %d\n", pPacket->length);
      // construct packet data
      fullPacket = os_zalloc(sizeof(uint8_t) * pPacket->length);
      os_memcpy(fullPacket, pPacket->fixedHeader, pPacket->fixedHeader_len);
      os_memcpy(fullPacket + pPacket->fixedHeader_len, pPacket->varHeader, pPacket->varHeader_len);
      os_memcpy(fullPacket + pPacket->fixedHeader_len + pPacket->varHeader_len, pPacket->payload, pPacket->payload_len);
      break;
    case MQTT_MSG_TYPE_PUBLISH:
    // prepare for publish
      break;
    case MQTT_MSG_TYPE_SUBSCRIBE:
      // prepare for subscribe
      break;
    case MQTT_MSG_TYPE_UNSUBSCRIBE:
      // prepare for unsubscribe
      break;
    case MQTT_MSG_TYPE_PINGREQ:
      // prepare for ping
      break;
    case MQTT_MSG_TYPE_DISCONNECT:
      // disconnect from broker
      break;
    default:
      // something has gone wrong
      os_printf("Attempt to send incorrect packet type: %d", (uint8_t)msgType);
      return -1;
  }
#ifdef DEBUG
  os_printf("About to send MQTT connect...\n");
#endif
  espconn_send(session->activeConnection, fullPacket, pPacket->length);
  os_free(fullPacket);
  os_free(pPacket->fixedHeader);
  os_free(pPacket->varHeader);
  os_free(pPacket->payload);
}

LOCAL void ICACHE_FLASH_ATTR test(void *arg) {
  os_printf("Entered test!\n");
  mqtt_session_t *pSession = (mqtt_session_t *)arg;
  mqtt_send(pSession, NULL, 0, MQTT_MSG_TYPE_CONNECT);
}

void ICACHE_FLASH_ATTR user_init() {
  uint8_t wifiStatus;
  LOCAL mqtt_session_t globalSession;
  LOCAL mqtt_session_t *pGlobalSession = &globalSession;
  char ssid[32] = "Kwangmyong";
  char passkey[64] = "vqmfg55020";
  struct station_config stationConf;
  gpio_init(); // init gpio so we can use the LED
  wifi_status_led_install(0, PERIPHS_IO_MUX_GPIO0_U, FUNC_GPIO0); // set GPIO0 as status LED
  stationConf.bssid_set = 0;
  os_memcpy(&stationConf.ssid, ssid, 32); // copy the ssid and passkey into the station_config struct
  os_memcpy(&stationConf.password, passkey, 64);
  wifi_set_opmode_current(0x01); //station mode
  wifi_station_set_config_current(&stationConf); // tell it about our config, this auto-connects us as well
  
  // prepare the TCP/MQTT connection stuff
  // test server is at 51.15.65.206
  char testUser[11] = { 0x4d, 0x72, 0x41, 0x75, 0x72, 0x65, 0x6c, 0x69, 0x75, 0x73, 0x52 };
  char *pTestUser = testUser;
  char testPass[4] = { 0x74, 0x65, 0x73, 0x74 };
  char *pTestPass = testPass;
  char testTopic[5] = "input";
  char clientID[5] = {0x46, 0x52, 0x5a, 0x4e, 0x30 };
  char *pClientID = clientID;
  pGlobalSession->port = 1883; // mqtt port
  const char esp_tcp_server_ip[4] = {51, 15, 65, 206}; // remote IP of TCP server
  os_memcpy(pGlobalSession->ip, esp_tcp_server_ip, 4);
  pGlobalSession->username_len = 11;
  pGlobalSession->username = os_zalloc(sizeof(uint8_t) * pGlobalSession->username_len);
  os_memcpy(pGlobalSession->username, testUser, pGlobalSession->username_len);
  pGlobalSession->password_len = 4;
  pGlobalSession->password = os_zalloc(sizeof(uint8_t) * pGlobalSession->password_len);
  os_memcpy(pGlobalSession->password, testPass, pGlobalSession->password_len);
  pGlobalSession->topic_name_len = 5;
  pGlobalSession->topic_name = os_zalloc(sizeof(uint8_t) * pGlobalSession->topic_name_len);
  os_memcpy(pGlobalSession->topic_name, testTopic, pGlobalSession->topic_name_len);
  pGlobalSession->client_id_len = 5;
  pGlobalSession->client_id = os_zalloc(sizeof(uint8_t) * pGlobalSession->client_id_len);
  os_memcpy(pGlobalSession->client_id, clientID, pGlobalSession->client_id_len);
  
  os_timer_setfn(&oneTimer, (os_timer_func_t *)tcpConnect, pGlobalSession);
  os_timer_arm(&oneTimer, 15000, 0);
  os_timer_setfn(&testTimer, (os_timer_func_t *)test, pGlobalSession);
  os_timer_arm(&testTimer, 17000, 0);
}