/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */

#include <string.h>

#include "os/mynewt.h"
#include <sys/queue.h>
#include <battery/battery.h>
#include <battery/battery_prop.h>
#include <battery/battery_drv.h>

#define BATTERY_MAX_COUNT 1

#define GET_BIT(arr, bit)   ((arr)[(bit) / 32] & (1 << ((bit) % 32)))

/*
 * Structure holds information about listener and what is it listening for.
 */
struct listener_data
{
    /* The properties to monitor for change */
    uint32_t ld_prop_change_mask[BATTERY_PROPERTY_MASK_SIZE];
    /* The properties to monitor for periodic read */
    uint32_t ld_prop_read_mask[BATTERY_PROPERTY_MASK_SIZE];
    /* Pointer to listener provided by the application. */
    struct battery_prop_listener *ld_listener;
};

struct battery_manager
{
    /* The lock for battery object */
    struct os_mutex bm_lock;

    struct battery *bm_batteries[BATTERY_MAX_COUNT];

    /** Event queue */
    struct os_eventq *bm_eventq;

    /** Manages poll rates of the batteries */
    struct os_callout bm_poll_callout;

} battery_manager;

struct os_eventq *
battery_mgr_evq_get(void)
{
    return (battery_manager.bm_eventq);
}

static void
battery_mgr_evq_set(struct os_eventq *evq)
{
    assert(evq != NULL);
    battery_manager.bm_eventq = evq;
}

static void battery_mgr_poll_battery(struct battery *battery);

static void
battery_poll_event_cb(struct os_event *ev)
{
    int i;
    int pflag;
    os_time_t next_poll = 0;
    os_time_t now = os_time_get();
    struct battery *bat;
    os_stime_t ticks;

    pflag = 0;
    for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
        bat = battery_manager.bm_batteries[i];
        if (bat) {
            if (OS_TIME_TICK_GEQ(now, bat->b_next_run)) {
                bat->b_last_read_time = now;
                battery_mgr_poll_battery(battery_manager.bm_batteries[i]);
                bat->b_next_run = now + os_time_ms_to_ticks32(bat->b_poll_rate);
            }
            if ((pflag == 0) || OS_TIME_TICK_LT(bat->b_next_run, next_poll)) {
                pflag = 1;
                next_poll = bat->b_next_run;
            }
        }
    }

    if (pflag) {
        ticks = (os_stime_t)(next_poll - os_time_get());
        if (ticks < 0) {
            ticks = 1;
        }
        os_callout_reset(&battery_manager.bm_poll_callout, (os_time_t)ticks);
    }
}

void
battery_mgr_process_event(struct os_event *event)
{
    os_eventq_put(battery_manager.bm_eventq, event);
}

static void
battery_mgr_init(void)
{
#ifdef MYNEWT_VAL_BATTERY_MGR_EVQ
    battery_mgr_evq_set(MYNEWT_VAL(BATTERY_MGR_EVQ));
#else
    battery_mgr_evq_set(os_eventq_dflt_get());
#endif

    /**
     * Initialize battery manager polling callout.
     */
    os_callout_init(&battery_manager.bm_poll_callout,
            battery_mgr_evq_get(), battery_poll_event_cb,
            NULL);

    os_mutex_init(&battery_manager.bm_lock);
}

/* =================================================================
 * ====================== PKG ======================================
 * =================================================================
 */

void
battery_pkg_init(void)
{
    /* Ensure this function only gets called by sysinit. */
    SYSINIT_ASSERT_ACTIVE();

    battery_mgr_init();

#if MYNEWT_VAL(BATTERY_SHELL)
    battery_shell_register();
#endif
}

int
battery_mgr_register(struct battery *battery)
{
    int i;

    os_mutex_pend(&battery_manager.bm_lock, OS_WAIT_FOREVER);

    for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
        if (battery_manager.bm_batteries[i] == NULL) {
            battery_manager.bm_batteries[i] = battery;
        }
    }
    os_mutex_release(&battery_manager.bm_lock);

    return 0;
}

int
battery_mgr_get_battery_count(void)
{
    int i;

    for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
        if (battery_manager.bm_batteries[i] == NULL) {
            break;
        }
    }

    return i;
}

struct os_dev *
battery_get_battery(int bat_num)
{
    assert(bat_num < BATTERY_MAX_COUNT);
    assert(battery_manager.bm_batteries[bat_num]);

    return &battery_manager.bm_batteries[bat_num]->b_dev;
}

static int
battery_get_num(struct battery *battery)
{
    int i;

    /* For one battery, there is no need to specify battery at all */
    if (battery == NULL) {
        return 0;
    }
    for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
        if (battery_manager.bm_batteries[i] == battery) {
            return i;
        }
    }
    assert(0);
    return 0;
}

struct battery_driver *
battery_get_driver(struct os_dev *battery,
        const char *dev_name)
{
    int i;
    assert(battery);
    struct battery *bat = (struct battery *)battery;
    for (i = 0; i < BATTERY_DRIVERS_MAX && bat->b_drivers[i]; ++i) {
        if (strcmp(bat->b_drivers[i]->dev.od_name, dev_name) == 0) {
            return bat->b_drivers[i];
        }
    }
    /* No device found */
    assert(0);
    return NULL;
}

static void
set_bit(uint32_t *mask, int bit)
{
    mask[bit / (sizeof(*mask) * 8)] |=
        1 << (bit & ((sizeof(*mask) * 8) - 1));
}

static void
clear_bit(uint32_t *mask, int bit)
{
    mask[bit / (sizeof(*mask) * 8)] &=
        ~(1 << (bit & ((sizeof(*mask) * 8) - 1)));
}

static struct battery_property *
find_driver_property(struct battery *bat, struct battery_driver *driver,
        battery_property_type_t type, battery_property_flags_t flags)
{
    struct battery_property *prop = bat->b_properties + driver->bd_first_property;
    int i;
    assert(driver);

    for (i = 0; driver->bd_property_count; ++i, ++prop) {
        if (prop->bp_type == type && prop->bp_flags == flags)
            return prop;
    }
    return NULL;
}

static struct battery_property *
find_hardware_property(struct battery *battery, struct battery_driver *driver,
        battery_property_type_t type, battery_property_flags_t flags)
{
    struct battery_property *res = NULL;
    int i;

    /* Search driver properties first */
    if (driver) {
        res = find_driver_property(battery, driver, type, flags);
    } else {
        for (i = 0; res == NULL && i < BATTERY_DRIVERS_MAX; ++i) {
            res = find_driver_property(battery, battery->b_drivers[i],
                                       type, flags);
        }
    }
    return res;
}

struct battery_property *
battery_find_property(struct os_dev *battery,
        battery_property_type_t type, battery_property_flags_t flags,
        const char *dev_name)
{
    struct battery_property *res = NULL;
    struct battery_property *prop;
    struct battery *bat = (struct battery *)battery;
    int i;
    struct battery_driver *driver = NULL;

    if (dev_name) {
        driver = battery_get_driver(battery, dev_name);
        assert(driver);
    }
    /* Search hardware properties first */
    res = find_hardware_property(bat, driver, type, flags);
    if (!res) {
        /* Search battery manager created properties */
        for (i = 0; i < bat->b_all_property_count; ++i) {
            prop =  &bat->b_properties[i];
            if (prop->bp_type == type &&
                prop->bp_flags == flags &&
                (driver == NULL ||
                 bat->b_drivers[prop->bp_drv_num] == driver)) {
                res = prop;
                break;
            }
        }
    }
    if (!res) {
        /* Create software threshold */
        if ((flags & BATTERY_PROPERTY_FLAGS_CREATE) != 0 &&
            (flags & BATTERY_PROPERTY_FLAGS_ALARM_THREASH) != 0) {
            /* Find base property that threshold is for */
            prop = find_hardware_property(bat, driver, type,
                    BATTERY_PROPERTY_FLAGS_NONE);
            /* Base property found, create derived one */
            if (prop) {
                struct battery_property *p =
                    calloc(1, sizeof(struct battery_property));
                if (prop->bp_prop_num == 0) {
                    prop->bp_prop_num = ++bat->b_all_property_count;
                    assert(bat->b_all_property_count <= BATTERY_MAX_PROPERTY_COUNT);
                }
                /* Base property is marked as base for others
                 */
                prop->bp_base = 1;
                p->bp_type = type;
                p->bp_flags = flags;
                p->bp_bat_num = prop->bp_bat_num;
                p->bp_drv_num = prop->bp_drv_num;
                p->bp_prop_num = ++bat->b_all_property_count;
                assert(bat->b_all_property_count <= BATTERY_MAX_PROPERTY_COUNT);
                res = p;
            }
        }
    }
    return res;
}

int
battery_get_property_count(struct os_dev *battery,
        struct battery_driver *driver)
{
    struct battery *bat = (struct battery *)battery;
    int i;

    if (driver == NULL) {
        return bat->b_all_property_count;
    } else {
        for (i = 0; i < BATTERY_DRIVERS_MAX && bat->b_drivers[i]; ++i) {
            if (bat->b_drivers[i] == driver) {
                return bat->b_drivers[i]->bd_property_count;
            }
        }
    }

    return 0;
}

struct battery_property *
battery_enum_property(struct os_dev *battery, struct battery_driver *driver,
        uint8_t prop_num)
{
    struct battery *bat = (struct battery *)battery;
    struct battery_property *prop = NULL;
    int i;

    if (driver == NULL) {
        if (prop_num < bat->b_all_property_count) {
            prop = &bat->b_properties[prop_num];
        }
    } else {
        for (i = 0; i < BATTERY_DRIVERS_MAX && bat->b_drivers[i]; ++i) {
            if (bat->b_drivers[i] == driver) {
                if (prop_num < driver->bd_property_count) {
                    prop = &bat->b_properties[prop_num + driver->bd_first_property];
                }
            }
        }
    }

    return prop;
}

char *
battery_prop_get_name(const struct battery_property *prop, char *buf,
        size_t buf_size)
{
    struct battery *bat = battery_manager.bm_batteries[prop->bp_bat_num];
    struct battery_driver *driver = bat->b_drivers[prop->bp_drv_num];
    const struct battery_driver_property *driver_prop =
            &driver->bd_driver_properties[prop->bp_prop_num - driver->bd_first_property];

    strncpy(buf, driver_prop->bdp_name, buf_size);

    return buf;
}

struct battery_property *
battery_find_property_by_name(struct os_dev *battery, const char *name)
{
    struct battery *bat = (struct battery *)battery;
    char buf[20];
    int i;

    for (i = 0; i < bat->b_all_property_count; ++i) {
        battery_prop_get_name(&bat->b_properties[i], buf, 20);
        if (strcmp(buf, name) == 0) {
            return &bat->b_properties[i];
        }
    }
    return NULL;
}

static void
battery_mgr_poll_battery_driver(struct battery *bat,
        struct battery_driver *drv, uint32_t changed[], uint32_t queried[])
{
    int i;
    battery_property_value_t old_val;
    struct battery_property *prop = &bat->b_properties[drv->bd_first_property];

    /* Read requested properties */
    for (i = 0; i < drv->bd_property_count; ++i, ++prop) {
        if (!GET_BIT(bat->b_polled_properties, i)) {
            continue;
        }

        if (!driver_property(prop)) {
            continue;
        }

        old_val = prop->bp_value;
        if (drv->bd_funcs->bdf_property_get(drv, prop, 100)) {
            prop->bp_valid = 0;
        } else {
            prop->bp_valid = 1;
            if (memcmp(&old_val, &prop->bp_value, sizeof(old_val))) {
                set_bit(changed, prop->bp_prop_num);
            }
            set_bit(queried, prop->bp_prop_num);
        }
    }
}

static void
battery_mgr_poll_battery(struct battery *battery)
{
    uint32_t changed[BATTERY_PROPERTY_MASK_SIZE] = {0};
    uint32_t queried[BATTERY_PROPERTY_MASK_SIZE] = {0};
    uint32_t masked;
    int first_one;
    struct listener_data *ld;
    struct battery_driver *driver;
    int i;
    int j;

    /* Poll battery drivers */
    for(i = 0; i < BATTERY_DRIVERS_MAX; ++i) {
        driver = battery->b_drivers[i];
        if (driver) {
            battery_mgr_poll_battery_driver(battery, driver, changed, queried);
        }
    }

    /* Notify listeners about property changes */
    for (i = 0; i < battery->b_listener_count; ++i) {
        ld = &battery->b_listeners[i];
        for (j = 0; j < BATTERY_PROPERTY_MASK_SIZE; ++j) {
            masked = changed[j] & ld->ld_prop_change_mask[j];
            while (masked) {
                /* Find first set bit */
                first_one = __builtin_ffs(masked) - 1;
                /* Clear it for next loop */
                masked ^= (1 << first_one);
                driver = battery->b_drivers[i];
                /* Notify listener */
                ld->ld_listener->bpl_prop_changed(ld->ld_listener,
                                                  battery->b_properties +
                                                  first_one + j * 32);
            }
        }
    }

    /* Notify listeners about periodic reads */
    for (i = 0; i < battery->b_listener_count; ++i) {
        ld = &battery->b_listeners[i];
        for (j = 0; j < BATTERY_PROPERTY_MASK_SIZE; ++j) {
            masked = queried[j] & ld->ld_prop_read_mask[j];
            while (masked) {
                /* Find first set bit */
                first_one = __builtin_ffs(masked) - 1;
                /* Clear it for next loop */
                masked ^= (1 << first_one);
                driver = battery->b_drivers[i];
                /* Notify listener */
                ld->ld_listener->bpl_prop_read(ld->ld_listener,
                                          battery->b_properties +
                                          first_one + j * 32);
            }
        }
    }
}

int
battery_set_poll_rate_ms(struct os_dev *battery, uint32_t poll_rate)
{
    struct battery *bat = (struct battery *)battery;

    if ((poll_rate == 0) || (bat == NULL)) {
        return -1;
    }

    bat->b_poll_rate = poll_rate;
    bat->b_next_run = os_time_get();
    os_callout_reset(&battery_manager.bm_poll_callout, 0);

    return 0;
}

int
battery_add_driver(struct os_dev *battery, struct battery_driver *driver)
{
    struct battery *bat = (struct battery *)battery;
    const struct battery_driver_property *drv_prop;
    struct battery_property *prop;
    int bat_num;
    int drv_num;
    int i;
    int j;

    assert(battery);
    assert(driver);

    bat_num = battery_get_num(bat);
    /* Find first slot */
    for (i = 0; i < BATTERY_DRIVERS_MAX; ++i) {
        /* Just make sure that driver is not added twice */
        assert(bat->b_drivers[i] != driver);
        if (bat->b_drivers[i] == NULL) {
            break;
        }
    }
    assert(i < BATTERY_DRIVERS_MAX);
    drv_num = i;
    bat->b_drivers[i] = driver;

    drv_prop = driver->bd_driver_properties;
    for (i = 0; drv_prop->bdp_type; ++i) {
        ++drv_prop;
    }
    driver->bd_property_count = i;
    bat->b_properties = realloc(bat->b_properties,
                                ((i + bat->b_all_property_count) *
                                 sizeof(struct battery_property)));

    j = bat->b_all_property_count;
    driver->bd_first_property = (uint8_t)j;
    /* Initialize driver properties with battery manager data */
    for (i = 0; i < driver->bd_property_count; ++i, ++j) {
        prop = bat->b_properties + j;
        prop->bp_type = driver->bd_driver_properties[i].bdp_type;
        prop->bp_flags = driver->bd_driver_properties[i].bdp_flags;
        prop->bp_valid = 0;
        prop->bp_value.bpv_i32 = 0;
        prop->bp_drv_prop_num = (uint8_t)i;
        prop->bp_drv_num = drv_num;
        prop->bp_bat_num = bat_num;
        prop->bp_prop_num = j;
    }
    bat->b_all_property_count = j;

    return 0;
}

static int
get_listener_index(struct battery *battery,
        struct battery_prop_listener *listener)
{
    int i;

    assert(listener);
    assert(battery);

    /* Find existing listener */
    for (i = 0; i < battery->b_listener_count; ++i) {
        if (battery->b_listeners[i].ld_listener == listener) {
            break;
        }
    }
    /* If listener was not yet added, create space for it and fill
     * masks with zeros so they will be filled when listener is actually
     * added to properties.
     */
    if (i >= battery->b_listener_count) {
        i = battery->b_listener_count++;
        battery->b_listeners = realloc(battery->b_listeners,
                sizeof(struct listener_data) * battery->b_listener_count);
        if (battery->b_listeners == NULL) {
            return -1;
        }
        battery->b_listeners[i].ld_listener = listener;
        memset(battery->b_listeners[i].ld_prop_change_mask, 0,
               sizeof(battery->b_listeners[i].ld_prop_change_mask));
        memset(battery->b_listeners[i].ld_prop_read_mask, 0,
               sizeof(battery->b_listeners[i].ld_prop_read_mask));
    }

    return i;
}

static int
battery_update_polled_properties(struct battery *battery)
{
    struct listener_data *ld;
    int i;
    int j;

    for (i = 0; i < BATTERY_PROPERTY_MASK_SIZE; ++i) {
        battery->b_polled_properties[i] = 0;
        for (j = 0; j < battery->b_listener_count; ++j) {
            ld = &battery->b_listeners[j];
            battery->b_polled_properties[i] |= ld->ld_prop_read_mask[i];
            battery->b_polled_properties[i] |= ld->ld_prop_change_mask[i];
        }
    }

    return 0;
}

int
battery_prop_change_subscribe(struct battery_prop_listener *listener,
        struct battery_property *prop)
{
    struct battery *battery;
    int listener_index;

    assert(listener);
    assert(prop);

    battery = (struct battery *)battery_get_battery(prop->bp_bat_num);
    listener_index = get_listener_index(battery, listener);

    set_bit(battery->b_listeners[listener_index].ld_prop_change_mask,
            prop->bp_prop_num);

    battery_update_polled_properties(battery);

    return 0;
}

int
battery_prop_change_unsubscribe(struct battery_prop_listener *listener,
        struct battery_property *prop)
{
    struct battery *battery;
    int i;
    int j;

    if (prop) {
        /* Single propety supplied */
        battery = (struct battery *)battery_get_battery(prop->bp_bat_num);
        for (i = 0; i < battery->b_listener_count; ++i) {
            if (listener == battery->b_listeners[i].ld_listener) {
                break;
            }
        }
        if (i >= battery->b_listener_count) {
            return -1;
        }

        clear_bit(battery->b_listeners[i].ld_prop_change_mask,
                  prop->bp_prop_num);
    } else {
        /* No property supplied, remove listener from all subscriptions */
        for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
            battery =  battery_manager.bm_batteries[i];
            for (j = 0; j < battery->b_listener_count; ++i) {
                if (battery->b_listeners[j].ld_listener == listener) {
                    /* Reduce number of listeners */
                    battery->b_listener_count--;
                    if (j < battery->b_listener_count) {
                        /* It was not last listener,
                         * move last to emptied space */
                        battery->b_listeners[j] =
                                battery->b_listeners[battery->b_listener_count];
                    }
                    battery->b_listeners =
                        realloc(battery->b_listeners,
                                sizeof(struct listener_data) *
                                battery->b_listener_count);
                    break;
                }
            }
        }
    }

    battery_update_polled_properties(battery);

    return 0;
}

int
battery_prop_poll_subscribe(struct battery_prop_listener *listener,
        struct battery_property *prop)
{
    struct battery *battery;
    int listener_index;

    assert(listener);
    assert(prop);

    battery = (struct battery *)battery_get_battery(prop->bp_bat_num);
    listener_index = get_listener_index(battery, listener);

    set_bit(battery->b_listeners[listener_index].ld_prop_read_mask,
            prop->bp_prop_num);

    battery_update_polled_properties(battery);

    return 0;
}

int
battery_prop_poll_unsubscribe(struct battery_prop_listener *listener,
        struct battery_property *prop)
{
    struct battery *battery;
    int i;
    int j;

    if (prop) {
        battery = (struct battery *)battery_get_battery(prop->bp_bat_num);
        for (i = 0; i < battery->b_listener_count; ++i) {
            if (listener == battery->b_listeners[i].ld_listener) {
                break;
            }
        }
        if (i >= battery->b_listener_count) {
            return -1;
        }

        clear_bit(battery->b_listeners[i].ld_prop_read_mask,
                  prop->bp_prop_num);
    } else {
        for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
            battery =  battery_manager.bm_batteries[i];
            for (j = 0; j < battery->b_listener_count; ++i) {
                if (battery->b_listeners[j].ld_listener == listener) {
                    battery->b_listener_count--;
                    if (j < battery->b_listener_count) {
                        battery->b_listeners[j] =
                                battery->b_listeners[battery->b_listener_count];
                    }
                    battery->b_listeners =
                        realloc(battery->b_listeners,
                                sizeof(struct listener_data) *
                                battery->b_listener_count);
                    break;
                }
            }
        }
    }

    battery_update_polled_properties(battery);

    return 0;
}

static int
battery_open(struct os_dev *dev, uint32_t timeout, void *arg)
{
    struct battery *bat = (struct battery *)dev;
    struct os_dev *drv_dev;
    int rollback = -1;
    int i;
    int rc = 0;

    for (i = 0; i < BATTERY_DRIVERS_MAX && bat->b_drivers[i]; ++i) {
        drv_dev = os_dev_open(bat->b_drivers[i]->dev.od_name, 0, NULL);
        assert(drv_dev == &bat->b_drivers[i]->dev);
        if (drv_dev != &bat->b_drivers[i]->dev) {
            rollback = i - 1;
            rc = -1;
            break;
        }
    }
    for (i = rollback; i >= 0; --i) {
        os_dev_close(&bat->b_drivers[i]->dev);
    }
    return rc;
}

static int
battery_close(struct os_dev *dev)
{
    struct battery *bat = (struct battery *)dev;
    int i;

    for (i = 0; i < BATTERY_DRIVERS_MAX && bat->b_drivers[i]; ++i) {
        os_dev_close(&bat->b_drivers[i]->dev);
    }
    return 0;
}

int
battery_init(struct os_dev *dev, void *arg)
{
    int i;
    struct battery *bat = (struct battery *)dev;

    OS_DEV_SETHANDLERS(dev, battery_open, battery_close);

    for (i = 0; i < BATTERY_MAX_COUNT; ++i) {
        if (battery_manager.bm_batteries[i] == NULL) {
            break;
        }
    }
    assert(i < BATTERY_MAX_COUNT);
    battery_manager.bm_batteries[i] = bat;

    memset(bat->b_drivers, 0, sizeof(bat->b_drivers));

    return 0;
}