/* WiFi station Example

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <sys/param.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "nvs_flash.h"

#include "lwip/err.h"
#include "lwip/sys.h"
#include "lwip/sockets.h"
#include <lwip/netdb.h>
#include "vban/vban.h"
#include <math.h>

#include <inttypes.h>
#include "http_control.h"
#include "esp_dsp.h"

#include "filter/butterworthdesign.h"
//#include "filter/signalpath.h"
#define DMA_BUFF_COUNT 16


static filterDesign *lpfFilter;

/* The examples use WiFi configuration that you can set via project configuration menu

   If you'd rather not, just change the below entries to strings with
   the config you want - ie #define EXAMPLE_WIFI_SSID "mywifissid"
*/
#define EXAMPLE_ESP_WIFI_SSID "Toetersnoet_Audio"
#define EXAMPLE_ESP_WIFI_PASS "nybui9823rkjcsdv"
#define EXAMPLE_ESP_MAXIMUM_RETRY 3
#define PORT 6980

#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WPA2_PSK

/* FreeRTOS event group to signal when we are connected*/
static EventGroupHandle_t s_wifi_event_group;

/* The event group allows multiple bits for each event, but we only care about two events:
 * - we are connected to the AP with an IP
 * - we failed to connect after the maximum amount of retries */
#define WIFI_CONNECTED_BIT BIT0
#define WIFI_FAIL_BIT BIT1

static const char *TAG = "wifi station";

static int s_retry_num = 0;

static unsigned char s_noChannels = 2;
static VBAN_SAMPLERATE s_sampleRate = SR_44100;
static VBAN_CODEC s_codec = CODEC_PCM;
static VBAN_DATATYPE s_datatype = DATATYPE_INT16;
static unsigned char s_samplesPerPacket = 0;
static char s_streamname[17] = "Stream1";
static unsigned int s_framecounter = 0;

static unsigned int s_validPacketCounter = 0;
static unsigned int s_droppedPacketCounter = 0;

static float coeffs_lpf[5];
static float w_lpf[5] = {0, 0};

#include <driver/i2s.h>

i2s_port_t i2s_num = I2S_NUM_0; // i2s port number (Built-in DAC functions are only supported on I2S0 for current ESP32 chip, purportedly)
i2s_config_t i2s_config;
unsigned char i2s_running = 0;
static unsigned char audioBufferFilled=0;

void configure_i2s(int newSampleRate, uint16_t newDmaBufLen)
{

    ESP_LOGE(TAG, "Setup I2S hardware");
    ESP_LOGE(TAG, "Sample rate: %d", newSampleRate);
    ESP_LOGE(TAG, "noSamples per packet %d", newDmaBufLen);
    static const i2s_pin_config_t pin_config = {
        .bck_io_num = 15,
        .ws_io_num = 13,
        .data_out_num = 2,
        .mck_io_num = I2S_PIN_NO_CHANGE,
        .data_in_num = I2S_PIN_NO_CHANGE};

    i2s_config.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
    i2s_config.sample_rate = newSampleRate;                 // This must match VBAN Outgoing Stream SampleRate parameter
    i2s_config.bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT; // apparently built-in DAC only enabled on I2S0 which only allows 16bit?
    i2s_config.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT;
    i2s_config.communication_format = (i2s_comm_format_t)(I2S_COMM_FORMAT_STAND_MSB);
    i2s_config.intr_alloc_flags = ESP_INTR_FLAG_LEVEL1; // default interrupt priority
    i2s_config.dma_buf_count = DMA_BUFF_COUNT;
    i2s_config.dma_buf_len = newDmaBufLen;
    i2s_config.use_apll = false;
    // i2s_config.tx_desc_auto_clear = true;

    if (i2s_running == 255)
    {
        ESP_ERROR_CHECK(i2s_stop(i2s_num));
        ESP_ERROR_CHECK(i2s_driver_uninstall(i2s_num));
    }

    ESP_ERROR_CHECK(i2s_driver_install(i2s_num, &i2s_config, 0, NULL)); // install and start i2s driver
    ESP_ERROR_CHECK(i2s_set_pin(i2s_num, &pin_config));
    // ESP_ERROR_CHECK( i2s_set_dac_mode(I2S_DAC_CHANNEL_LEFT_EN)  );  // RIGHT=GPIO25, LEFT=GPIO26
    ESP_ERROR_CHECK(i2s_zero_dma_buffer(i2s_num));
    i2s_set_clk(i2s_num, newSampleRate, 32, I2S_CHANNEL_STEREO);
    i2s_running = 255;
    audioBufferFilled = 0;
}



static void IRAM_ATTR  vban_decode(char *buffer, int buffersize)
{
    static uint32_t outBuf[512];
    static float leftOut[256];
    static float rightOut[256];

    VBAN_HEADER *header  = (VBAN_HEADER *)buffer;

    if (header->vban != VBAN_MARKER)
    {
        ESP_LOGE(TAG, "Invalid vban packet received");
        return;
    }

    // ESP_LOGE(TAG, "Valid vban packet received");

    s_noChannels = header->format_nbc + 1;
    memcpy(s_streamname, header->streamname, 16);
    s_sampleRate = (VBAN_SAMPLERATE)header->format_SR;
    s_datatype = (VBAN_DATATYPE)(header->format_bit & 0xf);
    s_framecounter = header->noFrame;
    int noSamples = header->format_nbs + 1;

    int16_t *data = (buffer + sizeof(VBAN_HEADER));

    if (i2s_running != 255)
    {
        ESP_LOGE(TAG, "Stream: %s", s_streamname);
        configure_i2s(VBAN_SRList[s_sampleRate], noSamples);
    }

    //filter needs floating point and separated channels
    for (int i = 0; i < noSamples; i++)
    {
        leftOut[i] = (float)(data[i*2]/(float)0x7fff);
        rightOut[i] = (float)(data[i*2+1]/(float)0x7fff);
    }

    //ESP_LOGE("DSP","%d %f",data[0],leftOut[0]);
    runFilter(rightOut,leftOut,noSamples,lpfFilter);
    //ESP_ERROR_CHECK(dsps_biquad_f32_ae32(leftOut,rightOut,noSamples,coeffs_lpf,w_lpf));

    for (int i = 0; i < noSamples; i++)
    {
        outBuf[i*2 +1] = (leftOut[i]+1.0)*0x7fffffff;
        outBuf[i*2] = (rightOut[i]+1.0)*0x7fffffff;
    }

    size_t bytesWritten;

    if(audioBufferFilled > DMA_BUFF_COUNT/4)
        ESP_ERROR_CHECK(i2s_write(i2s_num, outBuf, noSamples * 4 * s_noChannels, &bytesWritten, portMAX_DELAY));
    else
        audioBufferFilled++;

}

static void udp_server_task(void *pvParameters)
{
    char rx_buffer[1600];
    char addr_str[128];
    int addr_family = (int)pvParameters;
    int ip_protocol = 0;
    struct sockaddr_in6 dest_addr;

    while (1)
    {

        struct sockaddr_in *dest_addr_ip4 = (struct sockaddr_in *)&dest_addr;
        dest_addr_ip4->sin_addr.s_addr = htonl(INADDR_ANY);
        dest_addr_ip4->sin_family = AF_INET;
        dest_addr_ip4->sin_port = htons(PORT);
        ip_protocol = IPPROTO_IP;

        int sock = socket(addr_family, SOCK_DGRAM, ip_protocol);
        if (sock < 0)
        {
            ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
            break;
        }
        ESP_LOGI(TAG, "Socket created");

        // Set timeout
        struct timeval timeout;
        timeout.tv_sec = 5;
        timeout.tv_usec = 0;
        setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof timeout);

        int err = bind(sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
        if (err < 0)
        {
            ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno);
        }
        ESP_LOGI(TAG, "Socket bound, port %d", PORT);

        struct sockaddr_storage source_addr; // Large enough for both IPv4 or IPv6
        socklen_t socklen = sizeof(source_addr);

        uint32_t packetCounter = 0;
        ESP_LOGI(TAG, "Waiting for data");
        while (1)
        {

            int len = recvfrom(sock, rx_buffer, sizeof(rx_buffer) - 1, 0, (struct sockaddr *)&source_addr, &socklen);

            // Error occurred during receiving
            if (len < 0)
            {
                ESP_LOGE(TAG, "recvfrom failed: errno %d", errno);
                ESP_LOGE(TAG, "Received %x packets", packetCounter);
                //stop i2s
                if(i2s_running)
                    i2s_stop(i2s_num);
                i2s_running = 0;
                audioBufferFilled = 0;
                packetCounter = 0;
                break;
            }
            // Data received
            else
            {
                packetCounter++;
                // Get the sender's ip address as string
                vban_decode(rx_buffer, len);
                // inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr, addr_str, sizeof(addr_str) - 1);
            }
        }

        if (sock != -1)
        {
            ESP_LOGE(TAG, "Shutting down socket and restarting...");
            shutdown(sock, 0);
            close(sock);
        }
    }
    vTaskDelete(NULL);
}

static void event_handler(void *arg, esp_event_base_t event_base,
                          int32_t event_id, void *event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START)
    {
        esp_wifi_connect();
    }
    else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED)
    {
        if (s_retry_num < EXAMPLE_ESP_MAXIMUM_RETRY)
        {
            esp_wifi_connect();
            s_retry_num++;
            ESP_LOGI(TAG, "retry to connect to the AP");
        }
        else
        {
            xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
        }
        ESP_LOGI(TAG, "connect to the AP fail");
    }
    else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP)
    {
        ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
        ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip));
        s_retry_num = 0;
        xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
    }
}

void wifi_init_sta(void)
{
    s_wifi_event_group = xEventGroupCreate();

    ESP_ERROR_CHECK(esp_netif_init());

    esp_netif_t *esp_netif = NULL;
    esp_netif = esp_netif_next(esp_netif);
    esp_netif_set_hostname(esp_netif, "Speaker1");

    ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));

    esp_event_handler_instance_t instance_any_id;
    esp_event_handler_instance_t instance_got_ip;
    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT,
                                                        ESP_EVENT_ANY_ID,
                                                        &event_handler,
                                                        NULL,
                                                        &instance_any_id));
    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT,
                                                        IP_EVENT_STA_GOT_IP,
                                                        &event_handler,
                                                        NULL,
                                                        &instance_got_ip));

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = EXAMPLE_ESP_WIFI_SSID,
            .password = EXAMPLE_ESP_WIFI_PASS,
            /* Setting a password implies station will connect to all security modes including WEP/WPA.
             * However these modes are deprecated and not advisable to be used. Incase your Access point
             * doesn't support WPA2, these mode can be enabled by commenting below line */
            .threshold.authmode = ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD,
            .sae_pwe_h2e = WPA3_SAE_PWE_BOTH,
        },
    };
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
    ESP_ERROR_CHECK(esp_wifi_start());

    ESP_LOGI(TAG, "wifi_init_sta finished.");

    /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum
     * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */
    EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
                                           WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
                                           pdFALSE,
                                           pdFALSE,
                                           portMAX_DELAY);

    /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually
     * happened. */
    if (bits & WIFI_CONNECTED_BIT)
    {
        ESP_LOGI(TAG, "connected to ap SSID:%s password:%s",
                 EXAMPLE_ESP_WIFI_SSID, EXAMPLE_ESP_WIFI_PASS);
    }
    else if (bits & WIFI_FAIL_BIT)
    {
        ESP_LOGI(TAG, "Failed to connect to SSID:%s, password:%s",
                 EXAMPLE_ESP_WIFI_SSID, EXAMPLE_ESP_WIFI_PASS);
    }
    else
    {
        ESP_LOGE(TAG, "UNEXPECTED EVENT");
    }

    /* The event will not be processed after unregister */
    ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip));
    ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
    vEventGroupDelete(s_wifi_event_group);
}

#include "driver/i2c.h"
unsigned char dacSettings[31];
static void getDACStatus()
{
    int i2c_master_port = 0;
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = 16, // select GPIO specific to your project
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = 14, // select GPIO specific to your project
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000, // select frequency specific to your project
        // .clk_flags = 0,          /*!< Optional, you can use I2C_SCLK_SRC_FLAG_* flags to choose i2c source clock here. */
    };

    ESP_ERROR_CHECK(i2c_param_config(i2c_master_port, &conf));
    ESP_ERROR_CHECK(i2c_driver_install(i2c_master_port, I2C_MODE_MASTER, 0, 0, 0));

    unsigned char addressToRead = 0x0f;
    unsigned char reply=40;
    uint8_t write_buf[2] = {0x0f, reply};
    ESP_ERROR_CHECK(i2c_master_write_to_device(i2c_master_port,0x48,write_buf,2,100));
     write_buf[0] = 0x10;
    ESP_ERROR_CHECK(i2c_master_write_to_device(i2c_master_port,0x48,write_buf,2,100));
    
    //i2c_master_write_read_device(i2c_master_port, 0x48, &addressToRead, 1, reply, 1, 100);
    for(int i = 0; i<30;i++)
    {
        addressToRead = i;
        //i2c_master_write_read_device(i2c_master_port, 0x48, &addressToRead, 1, dacSettings+i, 1, 100);
        //ESP_LOGE(TAG,"address %d-,%d",i,dacSettings[i]);
    }

    ESP_LOGE(TAG,"reply %d",reply);
}





static void createFilterbtw()
{
    lpfFilter = createFilter(filter_lpf,2,44100,1000,0);
    //lpfFilter = createFilter(filter_hpf,2,44100,1000,1);
    //ESP_ERROR_CHECK(dsps_biquad_gen_lpf_f32(coeffs_lpf,0.009,0.707));
}

static void printStats()
{
    
    while(1)
    {
        char pBuffer[2048] = {0};
     vTaskGetRunTimeStats(pBuffer);
     printf("%s\n\r",(pBuffer));
     vTaskDelay(5000/portTICK_PERIOD_MS);
    }
}

#include "button.h"

rotaryPins volume;

void cwcallback(void *arg)
{
    printf("CW\n");
}

void ccwcallback(void *arg)
{
    printf("CCW\n");
}

void buttonCallback(void *arg)
{
    printf("buttonPress\n");
}

#include "power/husb238.h"

void app_main(void)
{
    i2s_running = 0;

    ESP_LOGI(TAG, "ESP_WIFI_MODE_STA");
    ESP_ERROR_CHECK(esp_netif_init());
    

    /* This helper function configures Wi-Fi or Ethernet, as selected in menuconfig.
     * Read "Establishing Wi-Fi or Ethernet Connection" section in
     * examples/protocols/README.md for more information about this function.
     */

    wifi_init_sta();
    //getDACStatus();
    httpd_handle_t server = start_webserver();
    createFilterbtw();

    volume.CWCallback = cwcallback;
    volume.CCWCallback = ccwcallback;
    volume.buttonCallback = buttonCallback;

    volume.PIN1 = 19;
    volume.PIN2 = 21;
    volume.BUT = 22;

    registerInterrupt(&volume);

    filter_registerHTTPHandlers(&server);
    xTaskCreatePinnedToCore(udp_server_task, "udp_server", 8192, (void *)AF_INET, 10, NULL,0);
    //init_i2c();
    //husb238_init(0);
    //husb238_requestVoltage(0,PD_SRC_15V);
    //xTaskCreate(printStats,"stats",4096,NULL,0,0);

    /*
        int frequency = 220;
        int samplRate = 44100;
        int noSamples = samplRate / frequency;
        configure_i2s(44100,noSamples*2);
         //fill the buffer with initial 1000 hz sine wave
        //100 hz is sine wave in samplerate/1000 samples

        for (int i = 0; i < noSamples; i++)
        {
            unsigned int  temp =sin((3.14159*2)*(i/(float)noSamples))*0x8fffffff + 0x8fffffff;
            outBuf[i*2] = temp;
            outBuf[(i * 2) + 1] = temp;
        }
      size_t bytesWritten;
      while(1)
      {
        ESP_ERROR_CHECK(i2s_write(i2s_num,outBuf,noSamples*2*4,&bytesWritten,100 ));
        vTaskDelay(5000/portTICK_RATE_MS);
      }
      */
}
