/*
 * This file is part of Cleanflight.
 *
 * Cleanflight 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.
 *
 * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdint.h>
#include <string.h>
#include <math.h>

extern "C" {
    #include <platform.h>
    #include "build_config.h"
    #include "version.h"
    #include "debug.h"
    #include "common/axis.h"
    #include "common/color.h"
    #include "common/maths.h"

    #include "drivers/system.h"
    #include "drivers/sensor.h"
    #include "drivers/accgyro.h"
    #include "drivers/compass.h"
    #include "drivers/serial.h"
    #include "drivers/bus_i2c.h"
    #include "drivers/timer.h"
    #include "drivers/pwm_rx.h"
    #include "drivers/buf_writer.h"

    #include "rx/rx.h"

    #include "io/rc_controls.h"
    #include "io/gps.h"
    #include "io/ledstrip.h"
    #include "io/serial_msp.h"
    #include "io/escservo.h"

    #include "telemetry/telemetry.h"

    #include "sensors/sensors.h"
    #include "sensors/boardalignment.h"
    #include "sensors/sensors.h"
    #include "sensors/battery.h"
    #include "sensors/sonar.h"
    #include "sensors/acceleration.h"
    #include "sensors/barometer.h"
    #include "sensors/compass.h"
    #include "sensors/gyro.h"

    #include "flight/mixer.h"
    #include "flight/pid.h"
    #include "flight/navigation.h"
    #include "flight/imu.h"
    #include "flight/failsafe.h"

    #include "config/runtime_config.h"
    #include "config/config.h"
    #include "config/config_profile.h"
    #include "config/config_master.h"
}

#include "unittest_macros.h"
#include "gtest/gtest.h"


extern "C" {
    void setCurrentPort(mspPort_t *port);
    void mspProcessReceivedCommand();
    extern mspPort_t *currentPort;
    extern bufWriter_t *writer;
    extern mspPort_t mspPorts[];
    profile_t *currentProfile;
}

profile_t profile;

typedef struct mspHeader_s {
    uint8_t dollar;
    uint8_t m;
    uint8_t direction;
    uint8_t size;
    uint8_t type;
} mspHeader_t;

typedef struct mspResonse_s {
    mspHeader_t header;
    uint8_t payload[];
} mspResponse_t;

#define SERIAL_BUFFER_SIZE 256
typedef union mspBuffer_u {
    mspResponse_t mspResponse;
    uint8_t buf[SERIAL_BUFFER_SIZE];
} mspBuffer_t;

static mspBuffer_t serialBuffer;

static int serialWritePos = 0;
static int serialReadPos = 0;

uint8_t buf[sizeof(bufWriter_t) + SERIAL_BUFFER_SIZE];

void serialWrite(serialPort_t *instance, uint8_t ch)
{
    UNUSED(instance);
    serialBuffer.buf[serialWritePos] = ch;
    ++serialWritePos;
}

void serialWriteBufShim(void *instance, uint8_t *data, int count)
{
    for (uint8_t *p = data; count > 0; count--, p++) {
        serialWrite((serialPort_t *)instance, *p);
    }
}

void serialBeginWrite(serialPort_t *instance)
{
    UNUSED(instance);
}

void serialEndWrite(serialPort_t *instance)
{
    UNUSED(instance);
}

uint8_t serialRxBytesWaiting(serialPort_t *instance)
{
    UNUSED(instance);
    if (serialWritePos > serialReadPos) {
        return serialWritePos - serialReadPos;
    } else {
        return 0;
    }
}

uint8_t serialRead(serialPort_t *instance)
{
    UNUSED(instance);
    const uint8_t ch = serialBuffer.buf[serialReadPos];
    ++serialReadPos;
    if (currentPort->indRX == MSP_PORT_INBUF_SIZE) {
        currentPort->indRX = 0;
    }
    currentPort->inBuf[currentPort->indRX] = ch;
    ++currentPort->indRX;
    return ch;
}

bool isSerialTransmitBufferEmpty(serialPort_t *instance)
{
    UNUSED(instance);
    return true;
}

class SerialMspUnitTest : public ::testing::Test {
protected:
    virtual void SetUp() {
        memset(serialBuffer.buf, 0, sizeof(serialBuffer));
        setCurrentPort(&mspPorts[0]);
        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, &mspPorts[0]);
    }
};


TEST_F(SerialMspUnitTest, TestMspProcessReceivedCommand)
{
    // check the MSP_API_VERSION is written out correctly
    serialWritePos = 0;
    serialReadPos = 0;
    currentPort->cmdMSP = MSP_API_VERSION;
    mspProcessReceivedCommand();

    EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
    EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
    EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
    EXPECT_EQ(3, serialBuffer.mspResponse.header.size);
    EXPECT_EQ(MSP_API_VERSION, serialBuffer.mspResponse.header.type);
    EXPECT_EQ(MSP_PROTOCOL_VERSION, serialBuffer.mspResponse.payload[0]);
    EXPECT_EQ(API_VERSION_MAJOR, serialBuffer.mspResponse.payload[1]);
    EXPECT_EQ(API_VERSION_MINOR, serialBuffer.mspResponse.payload[2]);
    int checksum = 3 ^ MSP_API_VERSION;
    checksum ^= MSP_PROTOCOL_VERSION ^ API_VERSION_MAJOR ^ API_VERSION_MINOR;
    EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);// checksum

    // check the MSP_FC_VARIANT is written out correctly
    serialWritePos = 0;
    serialReadPos = 0;
    currentPort->cmdMSP = MSP_FC_VARIANT;
    mspProcessReceivedCommand();

    EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
    EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
    EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
    EXPECT_EQ(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH, serialBuffer.mspResponse.header.size);
    EXPECT_EQ(MSP_FC_VARIANT, serialBuffer.mspResponse.header.type);
    EXPECT_EQ('C', serialBuffer.mspResponse.payload[0]);
    EXPECT_EQ('L', serialBuffer.mspResponse.payload[1]);
    EXPECT_EQ('F', serialBuffer.mspResponse.payload[2]);
    EXPECT_EQ('L', serialBuffer.mspResponse.payload[3]);
    checksum = FLIGHT_CONTROLLER_IDENTIFIER_LENGTH ^ MSP_FC_VARIANT;
    checksum ^= 'C'^ 'L' ^ 'F' ^ 'L';
    EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[4]);

    // check the MSP_FC_VERSION is written out correctly
    serialWritePos = 0;
    serialReadPos = 0;
    currentPort->cmdMSP = MSP_FC_VERSION;
    mspProcessReceivedCommand();
    EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
    EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
    EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
    EXPECT_EQ(FLIGHT_CONTROLLER_VERSION_LENGTH, serialBuffer.mspResponse.header.size);
    EXPECT_EQ(MSP_FC_VERSION, serialBuffer.mspResponse.header.type);
    EXPECT_EQ(FC_VERSION_MAJOR, serialBuffer.mspResponse.payload[0]);
    EXPECT_EQ(FC_VERSION_MINOR, serialBuffer.mspResponse.payload[1]);
    EXPECT_EQ(FC_VERSION_PATCH_LEVEL, serialBuffer.mspResponse.payload[2]);
    checksum = FLIGHT_CONTROLLER_VERSION_LENGTH ^ MSP_FC_VERSION;
    checksum ^= FC_VERSION_MAJOR ^ FC_VERSION_MINOR ^ FC_VERSION_PATCH_LEVEL;
    EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);
}


// STUBS
extern "C" {
// from acceleration.c
uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) {UNUSED(calibrationCyclesRequired);}
// from altitudehold.c
int32_t AltHold;
int32_t vario = 0;                      // variometer in cm/s
int32_t altitudeHoldGetEstimatedAltitude(void) {return 0;}
// from battery.c
uint16_t vbat = 0;                   // battery voltage in 0.1V steps (filtered)
int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
// from compass.c
int32_t magADC[XYZ_AXIS_COUNT];
// from config.c
master_t masterConfig;
controlRateConfig_t *currentControlRateProfile;
void resetPidProfile(pidProfile_t *pidProfile) {UNUSED(pidProfile);}
void handleOneshotFeatureChangeOnRestart(void) {}
void readEEPROM(void) {}
void resetEEPROM(void) {}
void writeEEPROM(void) {}
bool feature(uint32_t mask) {UNUSED(mask);return false;}
void featureSet(uint32_t mask) {UNUSED(mask);}
void featureClearAll() {}
uint32_t featureMask(void) {return 0;}
// from debug.c
int32_t debug[DEBUG32_VALUE_COUNT];
// from gps.c
#define GPS_SV_MAXSATS   16
int32_t GPS_coord[2];               // LAT/LON
uint8_t GPS_numSat;
uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
uint16_t GPS_altitude;              // altitude in 0.1m
uint16_t GPS_speed;                 // speed in 0.1m/s
uint16_t GPS_ground_course = 0;     // degrees * 10
uint8_t GPS_numCh;                          // Number of channels
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
// from gyro.c
int32_t gyroADC[XYZ_AXIS_COUNT];
// form imu.c
attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
int16_t accSmooth[XYZ_AXIS_COUNT];
// from ledstrip.c
void reevalulateLedConfig(void) {}
// from mixer.c
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS];
void stopMotors(void) {}
void loadCustomServoMixer(void) {}
// from msp.c
void rxMspFrameReceive(uint16_t *frame, int channelCount) {UNUSED(frame);UNUSED(channelCount);}
// from mw.c
uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
// from navigation.c
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint32_t GPS_distanceToHome;        // distance to home point in meters
int16_t GPS_directionToHome;        // direction to home or hol point in degrees
navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
void GPS_set_next_wp(int32_t *lat, int32_t *lon) {UNUSED(lat);UNUSED(lon);}
// from pid.c
void pidSetController(pidControllerType_e type) {UNUSED(type);}
// from rc_controls.c
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
void useRcControlsConfig(void *modeActivationConditions, void *escAndServoConfigToUse, void *pidProfileToUse) {
    UNUSED(modeActivationConditions);UNUSED(escAndServoConfigToUse);UNUSED(pidProfileToUse);}
// from runtime_config.c
uint8_t armingFlags = 0;
uint8_t stateFlags = 0;
uint16_t flightModeFlags = 0;
uint16_t disableFlightMode(flightModeFlags_e mask) {UNUSED(mask);return 0;}
bool sensors(uint32_t mask) {UNUSED(mask);return 0;}
// from rx.c
uint16_t rssi = 0;                  // range: [0;1023]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
rxRuntimeConfig_t rxRuntimeConfig;
// from system.c
void delay(uint32_t ms) {UNUSED(ms);}
// from system_stm32fN0x.c
void systemReset(void) {}
void systemResetToBootloader(void) {}
// from scheduler.c
uint16_t averageSystemLoadPercent = 0;
// from transponder_ir.c
void transponderUpdateData(uint8_t*) {}
}
