#include <AP_HAL.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_Airspeed.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_Compass.h>
#include <AP_Baro.h>
#include <AP_InertialSensor.h>
#include <DataFlash.h>

#include "LogReader.h"
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

extern const AP_HAL::HAL& hal;

LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed) :
    vehicle(VEHICLE_UNKNOWN),
    fd(-1),
    ahrs(_ahrs),
    ins(_ins),
    baro(_baro),
    compass(_compass),
    gps(_gps),
    airspeed(_airspeed),
    accel_mask(3),
    gyro_mask(3)
{}

bool LogReader::open_log(const char *logfile)
{
    fd = ::open(logfile, O_RDONLY);
    if (fd == -1) {
        return false;
    }
    return true;
}


struct PACKED log_Plane_Compass {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    int16_t mag_x;
    int16_t mag_y;
    int16_t mag_z;
    int16_t offset_x;
    int16_t offset_y;
    int16_t offset_z;
};

struct PACKED log_Copter_Compass {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    int16_t mag_x;
    int16_t mag_y;
    int16_t mag_z;
    int16_t offset_x;
    int16_t offset_y;
    int16_t offset_z;
    int16_t motor_offset_x;
    int16_t motor_offset_y;
    int16_t motor_offset_z;
};

struct PACKED log_Plane_Attitude {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    int16_t roll;
    int16_t pitch;
    uint16_t yaw;
    uint16_t error_rp;
    uint16_t error_yaw;
};

struct PACKED log_AIRSPEED {
    LOG_PACKET_HEADER;
    uint32_t timestamp;
    float   airspeed;
    float   diffpressure;
    int16_t temperature;
};

struct PACKED log_Copter_Attitude {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    int16_t control_roll;
    int16_t roll;
    int16_t control_pitch;
    int16_t pitch;
    uint16_t control_yaw;
    uint16_t yaw;
};

struct PACKED log_Copter_Nav_Tuning {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    float    desired_pos_x;
    float    desired_pos_y;
    float    pos_x;
    float    pos_y;
    float    desired_vel_x;
    float    desired_vel_y;
    float    vel_x;
    float    vel_y;
    float    desired_accel_x;
    float    desired_accel_y;
};

struct PACKED log_Rover_Attitude {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    int16_t roll;
    int16_t pitch;
    uint16_t yaw;
};

struct PACKED log_Rover_Compass {
    LOG_PACKET_HEADER;
    uint32_t time_ms;
    int16_t mag_x;
    int16_t mag_y;
    int16_t mag_z;
    int16_t offset_x;
    int16_t offset_y;
    int16_t offset_z;
    int16_t motor_offset_x;
    int16_t motor_offset_y;
    int16_t motor_offset_z;
};

void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
{
    switch (type) {
    case LOG_PLANE_COMPASS_MSG: {
        struct log_Plane_Compass msg;
        if(sizeof(msg) != length) {
            printf("Bad plane COMPASS length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
        compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
        break;
    }

    case LOG_PLANE_ATTITUDE_MSG: {
        struct log_Plane_Attitude msg;
        if(sizeof(msg) != length) {
            printf("Bad ATTITUDE length %u should be %u\n", (unsigned)length, (unsigned)sizeof(msg));
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
        break;
    }

    case LOG_PLANE_AIRSPEED_MSG: {
        struct log_AIRSPEED msg;
        if(sizeof(msg) != length) {
            printf("Bad AIRSPEED length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        airspeed.setHIL(msg.airspeed, msg.diffpressure, msg.temperature);
        break;
    }
    }
}

void LogReader::process_rover(uint8_t type, uint8_t *data, uint16_t length)
{
    switch (type) {
    case LOG_ROVER_COMPASS_MSG: {
        struct log_Rover_Compass msg;
        if(sizeof(msg) != length) {
            printf("Bad rover COMPASS length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
        compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
        break;
    }

    case LOG_ROVER_ATTITUDE_MSG: {
        struct log_Rover_Attitude msg;
        if(sizeof(msg) != length) {
            printf("Bad ATTITUDE length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
        break;
    }
    }
}

void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
{
    switch (type) {
    case LOG_COPTER_COMPASS_MSG: {
        struct log_Copter_Compass msg;
        if(sizeof(msg) != length) {
            printf("Bad copter COMPASS length %u expected %u\n", (unsigned)length, (unsigned)sizeof(msg));
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
        compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
        break;
    }

    case LOG_COPTER_ATTITUDE_MSG: {
        struct log_Copter_Attitude msg;
        if(sizeof(msg) != length) {
            printf("Bad ATTITUDE length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
        break;
    }

    case LOG_COPTER_NAV_TUNING_MSG: {
        struct log_Copter_Nav_Tuning msg;
        if(sizeof(msg) != length) {
            printf("Bad copter NAV_TUNING length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        inavpos = Vector3f(msg.pos_x * 0.01f, 
                           msg.pos_y * 0.01f,
                           0);
        break;
    }
    }
}

bool LogReader::set_parameter(const char *name, float value)
{
    enum ap_var_type var_type;
    AP_Param *vp = AP_Param::find(name, &var_type);
    if (vp == NULL) {
        return false;
    }
    if (var_type == AP_PARAM_FLOAT) {
        ((AP_Float *)vp)->set(value);
        ::printf("Set %s to %f\n", name, value);
    } else if (var_type == AP_PARAM_INT32) {
        ((AP_Int32 *)vp)->set(value);
        ::printf("Set %s to %d\n", name, (int)value);
    } else if (var_type == AP_PARAM_INT16) {
        ((AP_Int16 *)vp)->set(value);
        ::printf("Set %s to %d\n", name, (int)value);
    } else if (var_type == AP_PARAM_INT8) {
        ((AP_Int8 *)vp)->set(value);
        ::printf("Set %s to %d\n", name, (int)value);
    } else {
        // we don't support mavlink set on this parameter
        return false;
    }            
    return true;
}

bool LogReader::update(uint8_t &type)
{
    uint8_t hdr[3];
    if (::read(fd, hdr, 3) != 3) {
        return false;
    }
    if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
        printf("bad log header\n");
        return false;
    }

    if (hdr[2] == LOG_FORMAT_MSG) {
        struct log_Format &f = formats[num_formats];
        memcpy(&f, hdr, 3);
        if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
            return false;
        }
        if (num_formats < LOGREADER_MAX_FORMATS-1) {
            num_formats++;
        }
        type = f.type;
        return true;
    }

    uint8_t i;
    for (i=0; i<num_formats; i++) {
        if (formats[i].type == hdr[2]) break;
    }
    if (i == num_formats) {
        return false;
    }
    const struct log_Format &f = formats[i];
    
    uint8_t data[f.length];
    memcpy(data, hdr, 3);
    if (::read(fd, &data[3], f.length-3) != f.length-3) {
        return false;
    }

    switch (f.type) {
    case LOG_MESSAGE_MSG: {
        struct log_Message msg;
        if(sizeof(msg) != f.length) {
            printf("Bad MESSAGE length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
            vehicle = VEHICLE_PLANE;
            ::printf("Detected Plane\n");
            ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
            ahrs.set_fly_forward(true);
        } else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
            vehicle = VEHICLE_COPTER;
            ::printf("Detected Copter\n");
            ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
            ahrs.set_fly_forward(false);
        } else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) {
            vehicle = VEHICLE_ROVER;
            ::printf("Detected Rover\n");
            ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
            ahrs.set_fly_forward(true);
        }
        break;
    }

    case LOG_IMU_MSG: {
        struct log_IMU msg;
        if(sizeof(msg) != f.length) {
            printf("Bad IMU length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        if (gyro_mask & 1) {
            ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
        }
        if (accel_mask & 1) {
            ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
        }
        break;
    }

    case LOG_IMU2_MSG: {
        struct log_IMU msg;
        if(sizeof(msg) != f.length) {
            printf("Bad IMU2 length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        if (gyro_mask & 2) {
            ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
        }
        if (accel_mask & 2) {
            ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
        }
        break;
    }

    case LOG_GPS_MSG: {
        struct log_GPS msg;
        if(sizeof(msg) != f.length) {
            printf("Bad GPS length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.apm_time);
        Location loc;
        loc.lat = msg.latitude;
        loc.lng = msg.longitude;
        loc.alt = msg.altitude;
        loc.options = 0;

        Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
                     msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
                     msg.vel_z);
        gps.setHIL(0, (AP_GPS::GPS_Status)msg.status,
                   msg.apm_time,
                   loc,
                   vel,
                   msg.num_sats,
                   msg.hdop,
                   msg.vel_z != 0);
        if (msg.status == 3 && ground_alt_cm == 0) {
            ground_alt_cm = msg.altitude;
        }
        rel_altitude = msg.rel_altitude*0.01f;
        break;
    }

    case LOG_GPS2_MSG: {
        struct log_GPS2 msg;
        if(sizeof(msg) != f.length) {
            printf("Bad GPS2 length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.apm_time);
        Location loc;
        loc.lat = msg.latitude;
        loc.lng = msg.longitude;
        loc.alt = msg.altitude;
        loc.options = 0;

        Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
                     msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
                     msg.vel_z);
        gps.setHIL(1, (AP_GPS::GPS_Status)msg.status,
                   msg.apm_time,
                   loc,
                   vel,
                   msg.num_sats,
                   msg.hdop,
                   msg.vel_z != 0);
        if (msg.status == 3 && ground_alt_cm == 0) {
            ground_alt_cm = msg.altitude;
        }
        break;
    }

    case LOG_SIMSTATE_MSG: {
        struct log_AHRS msg;
        if(sizeof(msg) != f.length) {
            printf("Bad SIMSTATE length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.time_ms);
        sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
        break;
    }

    case LOG_BARO_MSG: {
        struct log_BARO msg;
        if(sizeof(msg) != f.length) {
            printf("Bad LOG_BARO length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        wait_timestamp(msg.timestamp);
        baro.setHIL(msg.pressure, msg.temperature*0.01f);
        break;
    }

    case LOG_PARAMETER_MSG: {
        struct log_Parameter msg;
        if(sizeof(msg) != f.length) {
            printf("Bad LOG_PARAMETER length\n");
            exit(1);
        }
        memcpy(&msg, data, sizeof(msg));
        set_parameter(msg.name, msg.value);
        break;        
    }
        

    default:
        if (vehicle == VEHICLE_PLANE) {
            process_plane(f.type, data, f.length);
        } else if (vehicle == VEHICLE_COPTER) {
            process_copter(f.type, data, f.length);
        } else if (vehicle == VEHICLE_ROVER) {
            process_rover(f.type, data, f.length);
        }
        break;
    }

    type = f.type;

    return true;
}

void LogReader::wait_timestamp(uint32_t timestamp)
{
    hal.scheduler->stop_clock(timestamp*1000UL);
}

bool LogReader::wait_type(uint8_t wtype)
{
    while (true) {
        uint8_t type;
        if (!update(type)) {
            return false;
        }
        if (wtype == type) {
            break;
        }
    }
    return true;
}