#include <nwk.h>

#include <string.h>

#include <util/delay.h>

#include "config.h"

#include "debug.h"

#include "node.h"

#include "nv.h"

#include "sysclock.h"

#include "ota.h"

#include "scheduler.h"

#include "tidsense.h"

#include "power.h"

#include "assoc.h"

#include "ota.h"

#include "i2cmaster.h"

#include "command.h"

uint32_t cmd_next_id = 0;

sched_task_t *status_task;

sched_task_t *gpio_task;

i2c_bus_t *cmd_i2c_bus = NULL;

void command_data_confirm(NWK_DataReq_t *req) {

    switch(req->status) {

        case NWK_SUCCESS_STATUS:

            app_handle_ack(req->control);

            //debugf("cmd: ok\n");

            break;

        case NWK_ERROR_STATUS:

        case NWK_NO_ACK_STATUS:

        case NWK_PHY_NO_ACK_STATUS:

            debugf("cmd: no reply (%d)\n", req->status);

            break;

        case NWK_NO_ROUTE_STATUS:

            debugf("cmd: no route\n");

            break;

        default:

            debugf("cmd: unhandled status sending response (%d)\n", req->status);

            break;

    }

    network_free_request(req);

}

void command_set_i2c_bus(i2c_bus_t *bus) {

    cmd_i2c_bus = bus;

}

void cmd_setup_resp(command_id_t cmd, NWK_DataReq_t *req, void *hdr, NWK_DataInd_t *ind) {

    req->dstAddr = ind->srcAddr;

    req->dstEndpoint = CONTROL_ENDPOINT;

    req->srcEndpoint = CONTROL_ENDPOINT;

    req->options = NWK_OPT_ACK_REQUEST;

    req->confirm = command_data_confirm;

    cmd_header_t *req_hdr = (cmd_header_t*)ind->data;

    cmd_header_t *resp_hdr = (cmd_header_t*)hdr;

    resp_hdr->cmd = cmd;

    resp_hdr->flags = CMD_RESP;

    resp_hdr->id = req_hdr->id;

}

void cmd_setup_req(command_id_t cmd, NWK_DataReq_t *req, uint16_t dest) {

    req->dstAddr = dest;

    req->dstEndpoint = CONTROL_ENDPOINT;

    req->srcEndpoint = CONTROL_ENDPOINT;

    req->options = NWK_OPT_ACK_REQUEST;

    req->confirm = command_data_confirm;

    cmd_header_t *req_hdr = (cmd_header_t*)req->data;

    req_hdr->cmd = cmd;

    req_hdr->flags = 0;

    req_hdr->id = cmd_next_id++;

}

void cmd_nodeinfo(NWK_DataInd_t *ind) {

    NWK_DataReq_t *resp = network_alloc_request(100);

    if(!resp) {

        return;

    }

    cmd_nodeinfo_resp_t *nodeinfo = (cmd_nodeinfo_resp_t*)&resp->data[0];

    if(ind) {

        cmd_setup_resp(CMD_NODEINFO, resp, nodeinfo, ind);

    } else {

        cmd_setup_req(CMD_NODEINFO, resp, assoc_get_master());

    }

    resp->size = sizeof(cmd_nodeinfo_resp_t);

    memcpy(&nodeinfo->version_info, &version_info, sizeof(version_info_t));

    nodeinfo->ota_state = ota_get_state();

    nodeinfo->ota_progress = ota_get_progress();

    NWK_DataReq(resp);

}

void cmd_gpio_handler(NWK_DataInd_t *ind) {

    if(nv_has_feature(FEATURE_CHARGER_RELAY)) {

        PORTA.OUTSET = PIN7_bm;

        sched_task_enable(gpio_task);

    }

}   

void gpio_task_fn(void) {

    PORTA.OUTCLR = PIN7_bm;

}

bool cmd_i2c_write(NWK_DataInd_t *ind) {

    if(!cmd_i2c_bus) return false;

    NWK_DataReq_t *resp = network_alloc_request(100);

    if(!resp) {

        return false;

    }

    cmd_i2c_t *req_cmd = (cmd_i2c_t*)&ind->data[0];

    cmd_i2c_t *resp_cmd = (cmd_i2c_t*)&resp->data[0];

    resp_cmd->addr = req_cmd->addr;

    resp_cmd->reg = req_cmd->reg;

    resp_cmd->size = 0;

    cmd_setup_resp(CMD_I2C_WRITE, resp, resp_cmd, ind);

    resp_cmd->status = i2c_write_generic(cmd_i2c_bus, req_cmd->addr, 

        req_cmd->reg, &req_cmd->data[0], req_cmd->size);

    resp->size = sizeof(cmd_i2c_t) + resp_cmd->size;

    NWK_DataReq(resp);

    return true;

}

bool cmd_i2c_read(NWK_DataInd_t *ind) {

    if(!cmd_i2c_bus) return false;

    NWK_DataReq_t *resp = network_alloc_request(100);

    if(!resp) {

        return false;

    }

    cmd_i2c_t *req_cmd = (cmd_i2c_t*)&ind->data[0];

    cmd_i2c_t *resp_cmd = (cmd_i2c_t*)&resp->data[0];

    resp_cmd->addr = req_cmd->addr;

    resp_cmd->reg = req_cmd->reg;

    resp_cmd->size = 0;

    cmd_setup_resp(CMD_I2C_READ, resp, resp_cmd, ind);

    resp_cmd->status = i2c_read_generic(cmd_i2c_bus, req_cmd->addr,

        req_cmd->reg, &resp_cmd->data[0], req_cmd->size);

    if(resp_cmd->status == I2C_SUCCESS) {

        resp_cmd->size = req_cmd->size;

    }

    resp->size = sizeof(cmd_i2c_t) + resp_cmd->size;

    NWK_DataReq(resp);

    return true;

}

static bool command_data_ind(NWK_DataInd_t *ind) {

    command_id_t cmd = ind->data[0];

    //debugf("rcmd: %d\n", cmd);

    switch(cmd) {

        case CMD_NODEINFO:

            cmd_nodeinfo(ind);

            return true;

        case CMD_DISCOVER:

            assoc_cmd_handler(ind);

            return true;

        case CMD_SETGPIO:

            cmd_gpio_handler(ind);

            return true;

        case CMD_OTASTART:

        case CMD_OTAREQUEST:

        case CMD_OTASTATUSREQUEST:

        case CMD_REBOOT:

            ota_cmd_handler(ind);

            return true;

        case CMD_I2C_WRITE:

            return cmd_i2c_write(ind);

        case CMD_I2C_READ:

            return cmd_i2c_read(ind);

        default: debugf("cmd: unknown command (%d)\n", cmd);

    }

    return false;

}

void cmd_periodic_status_task(void) {

    if(assoc_get_state() == ASSOC_STATE_ASSOCIATED) {

        power_radio_on();

        cmd_nodeinfo(NULL);

        sched_task_set_interval(status_task, 300, 0);

    }

}

void command_setup(void) {

    NWK_OpenEndpoint(CONTROL_ENDPOINT, command_data_ind);

    status_task = sched_task_create(cmd_periodic_status_task, 10, 0, SCHED_ENABLED);

    if(nv_has_feature(FEATURE_CHARGER_RELAY)) {

        PORTA.DIRSET = PIN7_bm;

        PORTA.OUTCLR = PIN7_bm;

        gpio_task = sched_task_create(gpio_task_fn, 0, 500, SCHED_ONESHOT);

    }

}

uint32_t cmd_get_next_id(void) {

    return cmd_next_id++;

}