/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{
 * @addtogroup OSDModule OSD Module
 * @brief On screen display support
 * @{
 *
 * @file       OsdEtStd.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Interfacing with EagleTree OSD Std module
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * 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.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */


#include "openpilot.h"

#include "flightbatterystate.h"
#include "gpsposition.h"
#include "attitudeactual.h"
#include "baroaltitude.h"

//
// Configuration
//
#define DEBUG_PORT PIOS_COM_GPS
// #define ENABLE_DEBUG_MSG
// #define PIOS_ENABLE_DEBUG_PINS
// #define DUMP_CONFIG           // Enable this do read and dump the OSD config

//
// Private constants
//

#ifdef ENABLE_DEBUG_MSG
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__)
#else
#define DEBUG_MSG(format, ...)
#endif

#define CONFIG_LENGTH             6726
#define MIN(a, b) ((a) < (b) ? (a) : (b))

#define SUPPORTED_VERSION         115


#define OSD_ADDRESS               0x30

#define OSDMSG_V_LS_IDX           10
#define OSDMSG_BALT_IDX1          11
#define OSDMSG_BALT_IDX2          12
#define OSDMSG_A_LS_IDX           17
#define OSDMSG_VA_MS_IDX          18
#define OSDMSG_LAT_IDX            33
#define OSDMSG_LON_IDX            37
#define OSDMSG_HOME_IDX           47
#define OSDMSG_ALT_IDX            49
#define OSDMSG_NB_SATS            58
#define OSDMSG_GPS_STAT           59

#define OSDMSG_GPS_STAT_NOFIX     0x03
#define OSDMSG_GPS_STAT_FIX       0x2B
#define OSDMSG_GPS_STAT_HB_FLAG   0x10

#ifdef PIOS_ENABLE_DEBUG_PINS
        #define DEBUG_PIN_RUNNING 0
        #define DEBUG_PIN_I2C     1
        #define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
        #define DebugPinLow(x)  PIOS_DEBUG_PinLow(x)
#else
        #define DebugPinHigh(x)
        #define DebugPinLow(x)
#endif


static const char *UpdateConfFilePath = "/etosd/update.ocf";
#ifdef DUMP_CONFIG
static const char *DumpConfFilePath   = "/etosd/dump.ocf";
#endif

//
// Private types
//

//
// Private variables
//

// | Header / cmd?|                                  | V  |      E3                          |  V |                                                                     | LAT: 37 57.0000   | LONG: 24 00.4590  |                             | Hom<-179| Alt 545.4 m       |                        |#sat|stat|
// 00   01   02   03   04   05   06   07   08   09   10   11   12   13   14   15   16   17   18   19   20   21   22   23   24   25   26   27   28   29   30   31   32   33   34   35   36   37   38   39   40   41   42   43   44   45   46   47   48   49   50   51   52   53   54   55   56   57   58   59   60   61   62
uint8_t msg[63] =
{ 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90,0x00,
  0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A };
static volatile bool newPosData  = FALSE;
static volatile bool newBattData = FALSE;
static volatile bool newBaroData = FALSE;

static enum {
    STATE_DETECT,
    STATE_UPDATE_CONF,
    STATE_DUMP_CONF,
    STTE_RUNNING
} state;

static UAVObjEvent ev;
static uint32_t version = 0;


//
// Private functions
//
static void WriteToMsg8(uint8_t index, uint8_t value)
{
    if (value > 100) {
        value = 100;
    }

    msg[index] = ((value / 10) << 4) + (value % 10);
}

static void WriteToMsg16(uint8_t index, uint16_t value)
{
    WriteToMsg8(index, value % 100);
    WriteToMsg8(index + 1, value / 100);
}

static void WriteToMsg24(uint8_t index, uint32_t value)
{
    WriteToMsg16(index, value % 10000);
    WriteToMsg8(index + 2, value / 10000);
}

static void WriteToMsg32(uint8_t index, uint32_t value)
{
    WriteToMsg16(index, value % 10000);
    WriteToMsg16(index + 2, value / 10000);
}

static void SetCoord(uint8_t index, uint32_t coord)
{
#define E7 10000000
    uint8_t deg = coord / E7;
    float sec   = (float)(coord - deg * E7) / ((float)E7 / (60.0 * 10000));

    WriteToMsg8(index + 3, deg);
    WriteToMsg24(index, sec);
}

static void SetCourse(uint16_t dir)
{
    WriteToMsg16(OSDMSG_HOME_IDX, dir);
}

static void SetBaroAltitude(int16_t altitudeMeter)
{
    // calculated formula
    // ET OSD uses first update as zeropoint and then +- from that
    altitudeMeter = (4571 - altitudeMeter) / 0.37;
    msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter & 0x00FF);
    msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8) & 0x3F;
}


static void SetAltitude(uint32_t altitudeMeter)
{
    WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10);
}

static void SetVoltage(uint32_t milliVolt)
{
    msg[OSDMSG_VA_MS_IDX] &= 0x0F;
    msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4;
    msg[OSDMSG_V_LS_IDX]   = (milliVolt % 6444) * 256 / 6444;
}

static void SetCurrent(uint32_t milliAmp)
{
    uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA;

    msg[OSDMSG_VA_MS_IDX] &= 0xF0;
    msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F);
    msg[OSDMSG_A_LS_IDX]   = (value & 0xFF);
}

static void SetNbSats(uint8_t nb)
{
    msg[OSDMSG_NB_SATS] = nb;
}

static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
    newBattData = TRUE;
}

static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
    newPosData = TRUE;
}

static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
    newBaroData = TRUE;
}

static bool Read(uint32_t start, uint8_t length, uint8_t *buffer)
{
    uint8_t cmd[5];

    const struct pios_i2c_txn txn_list[] = {
        {
            .addr = OSD_ADDRESS,
            .rw   = PIOS_I2C_TXN_WRITE,
            .len  = sizeof(cmd),
            .buf  = cmd,
        }
        ,
        {
            .addr = OSD_ADDRESS,
            .rw   = PIOS_I2C_TXN_READ,
            .len  = length,
            .buf  = buffer,
        }
        ,
    };

    cmd[0] = 0x02;
    cmd[1] = 0x05;
    cmd[2] = (uint8_t)(start & 0xFF);
    cmd[3] = (uint8_t)(start >> 8);
    cmd[4] = length;

    return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0;
}

static bool Write(uint32_t start, uint8_t length, const uint8_t *buffer)
{
    uint8_t cmd[125];
    uint8_t ack[3];

    const struct pios_i2c_txn txn_list1[] = {
        {
            .addr = OSD_ADDRESS,
            .rw   = PIOS_I2C_TXN_WRITE,
            .len  = sizeof(cmd),
            .buf  = cmd,
        }
        ,
        {
            .addr = OSD_ADDRESS,
            .rw   = PIOS_I2C_TXN_READ,
            .len  = sizeof(ack),
            .buf  = ack,
        }
        ,
    };

    if (length + 5 > sizeof(cmd)) {
        // Too big
        return FALSE;
    }

    cmd[0] = 0x01;
    cmd[1] = 0x7D;
    cmd[2] = (uint8_t)(start & 0xFF);
    cmd[3] = (uint8_t)(start >> 8);
    cmd[4] = length;
    memcpy(&cmd[5], buffer, length);

    ack[0] = 0;

    //
    // FIXME: See OP-305, the driver seems to return FALSE while all is OK
    //
    PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1));
// if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) {
    // DEBUG_MSG("ACK=%d ", ack[0]);
    if (ack[0] == 49) {
        return TRUE;
    }
// }

    return FALSE;
}

static uint32_t ReadSwVersion(void)
{
    uint8_t buf[4];
    uint32_t version;

    if (Read(0, 4, buf)) {
        version  = (buf[0] - '0') * 100;
        version += (buf[2] - '0') * 10;
        version += (buf[3] - '0');
    } else {
        version = 0;
    }

    return version;
}

static void UpdateConfig(void)
{
    uint8_t buf[120];
    uint32_t addr = 0;
    uint32_t n;
    FILEINFO file;
    uint32_t res;

    // Try to open the file that contains a new config
    res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file);
    if (res == DFS_OK) {
        uint32_t bytesRead;
        bool ok = TRUE;

        DEBUG_MSG("Updating Config ");

        // Write the config-data in blocks to OSD
        while (addr < CONFIG_LENGTH && ok) {
            n   = MIN(CONFIG_LENGTH - addr, sizeof(buf));
            res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n);
            if (res == DFS_OK && bytesRead == n) {
                ok = Write(addr, n, buf);
                if (ok) {
                    // DEBUG_MSG(" %d %d\n", addr, n);
                    DEBUG_MSG(".");
                    addr += n;
                }
            } else {
                DEBUG_MSG(" FILEREAD FAILED ");
            }
        }

        DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");

        // If writing is OK, read the data back and verify
        if (ok) {
            DEBUG_MSG("Verify Config ");
            DFS_Seek(&file, 0, PIOS_SDCARD_Sector);

            addr = 0;
            while (addr < CONFIG_LENGTH && ok) {
                // First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file
                n  = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2);
                ok = Read(addr, n, buf);
                if (ok) {
                    uint32_t bytesRead;
                    res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n);
                    if (res == DFS_OK && bytesRead == n) {
                        DEBUG_MSG(".");
                        addr += n;

                        if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) {
                            DEBUG_MSG(" MISMATCH ");
                            ok = FALSE;
                        }
                    }
                }
            }
            DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
        }

        DFS_Close(&file);

        // When the config was updated correctly, remove the config-file
        if (ok) {
            DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, PIOS_SDCARD_Sector);
        }
    }
}

static void DumpConfig(void)
{
#ifdef DUMP_CONFIG
    uint8_t buf[100];
    uint32_t addr = 0;
    uint32_t n;
    FILEINFO file;
    uint32_t res;

    DEBUG_MSG("Dumping Config ");

    res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file);
    if (res == DFS_OK) {
        uint32_t bytesWritten;
        bool ok = TRUE;

        while (addr < CONFIG_LENGTH && ok) {
            n  = MIN(CONFIG_LENGTH - addr, sizeof(buf));
            ok = Read(addr, n, buf);
            if (ok) {
                res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n);
                if (res == DFS_OK && bytesWritten == n) {
                    // DEBUG_MSG(" %d %d\n", addr, n);
                    DEBUG_MSG(".");
                    addr += n;
                }
            }
        }

        DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");

        DFS_Close(&file);
    } else {
        DEBUG_MSG("Error Opening File %x\n", res);
    }
#endif /* ifdef DUMP_CONFIG */
}

static void Run(void)
{
    static uint32_t cnt = 0;

    if (newBattData) {
        FlightBatteryStateData flightBatteryData;

        FlightBatteryStateGet(&flightBatteryData);

        // DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000));

        SetVoltage((uint32_t)(flightBatteryData.Voltage * 1000));
        SetCurrent((uint32_t)(flightBatteryData.Current * 1000));
        newBattData = FALSE;
    }

    if (newPosData) {
        GPSPositionData positionData;
        AttitudeActualData attitudeActualData;

        GPSPositionGet(&positionData);
        AttitudeActualGet(&attitudeActualData);

        // DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt,
        // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude);

        // GPS Status
        if (positionData.Status == GPSPOSITION_STATUS_FIX3D) {
            msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX;
        } else {
            msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX;
        }
        msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG;

        // GPS info
        SetCoord(OSDMSG_LAT_IDX, positionData.Latitude);
        SetCoord(OSDMSG_LON_IDX, positionData.Longitude);
        SetAltitude(positionData.Altitude);
        SetNbSats(positionData.Satellites);
        SetCourse(attitudeActualData.Yaw);

        newPosData = FALSE;
    } else {
        msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG;
    }
    if (newBaroData) {
        BaroAltitudeData baroData;

        BaroAltitudeGet(&baroData);
        SetBaroAltitude(baroData.Altitude);

        newBaroData = FALSE;
    }

    DEBUG_MSG("SendMsg %d\n", cnt);
    {
        DebugPinHigh(DEBUG_PIN_I2C);
        const struct pios_i2c_txn txn_list[] = {
            {
                .addr = OSD_ADDRESS,
                .rw   = PIOS_I2C_TXN_WRITE,
                .len  = sizeof(msg),
                .buf  = msg,
            }
            ,
        };

        PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
        DebugPinLow(DEBUG_PIN_I2C);
    }

    cnt++;
}

static void onTimer(UAVObjEvent *ev)
{
    DebugPinHigh(DEBUG_PIN_RUNNING);

        #ifdef ENABLE_DEBUG_MSG
    PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
        #endif

    if (state == STATE_DETECT) {
        version = ReadSwVersion();
        DEBUG_MSG("SW: %d ", version);

        if (version == SUPPORTED_VERSION) {
            DEBUG_MSG("OK\n");
            state++;
        } else {
            DEBUG_MSG("INVALID\n");
        }
    } else if (state == STATE_UPDATE_CONF) {
        UpdateConfig();
        state++;
    } else if (state == STATE_DUMP_CONF) {
        DumpConfig();
        state++;
    } else if (state == STTE_RUNNING) {
        Run();
    } else {
        // should not happen..
        state = STATE_DETECT;
    }

    DebugPinLow(DEBUG_PIN_RUNNING);
}

//
// Public functions
//


/**
 * Initialise the module
 * \return -1 if initialisation failed
 * \return 0 on success
 */
int32_t OsdEtStdInitialize(void)
{
    GPSPositionConnectCallback(GPSPositionUpdatedCb);
    FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb);
    BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);

    memset(&ev, 0, sizeof(UAVObjEvent));
    EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS);

    return 0;
}