#include "mode.h"

#include "Tracker.h"

void Mode::update_auto(void)
{
    struct Tracker::NavStatus &nav_status = tracker.nav_status;

    Parameters &g = tracker.g;

    float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
    float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees

    bool direction_reversed = get_ef_yaw_direction();

    calc_angle_error(pitch, yaw, direction_reversed);

    float bf_pitch;
    float bf_yaw;
    convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);

    // only move servos if target is at least distance_min away if we  have a target
    if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !tracker.vehicle.location_valid) {
        tracker.update_pitch_servo(bf_pitch);
        tracker.update_yaw_servo(bf_yaw);
    }
}

void Mode::update_scan(void)
{
    struct Tracker::NavStatus &nav_status = tracker.nav_status;

    Parameters &g = tracker.g;

    if (!nav_status.manual_control_yaw) {
        float yaw_delta = g.scan_speed_yaw * 0.02f;
        nav_status.bearing   += yaw_delta   * (nav_status.scan_reverse_yaw?-1:1);
        if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) {
            nav_status.scan_reverse_yaw = false;
        }
        if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) {
            nav_status.scan_reverse_yaw = true;
        }
        nav_status.bearing = constrain_float(nav_status.bearing, 0, 360);
    }

    if (!nav_status.manual_control_pitch) {
        const float pitch_delta = g.scan_speed_pitch * 0.02f;
        if (nav_status.scan_reverse_pitch) {
            nav_status.pitch -= pitch_delta;
            if (nav_status.pitch < g.pitch_min) {
                nav_status.scan_reverse_pitch = false;
            }
        } else {
            nav_status.pitch += pitch_delta;
            if (nav_status.pitch > g.pitch_max) {
                nav_status.scan_reverse_pitch = true;
            }
        }
        nav_status.pitch = constrain_float(nav_status.pitch, g.pitch_min, g.pitch_max);
    }

    update_auto();
}

void Mode::calc_angle_error(float pitch, float yaw, bool direction_reversed)
{
    // Pitch angle error in centidegrees
    // Positive error means the target is above current pitch
    // Negative error means the target is below current pitch
    const AP_AHRS &ahrs = AP::ahrs();
    float ahrs_pitch = ahrs.pitch_sensor;
    int32_t ef_pitch_angle_error = pitch - ahrs_pitch;

    // Yaw angle error in centidegrees
    // Positive error means the target is right of current yaw
    // Negative error means the target is left of current yaw
    int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
    int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);
    if (direction_reversed) {
        if (ef_yaw_angle_error > 0) {
            ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;
        } else {
            ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd);
        }
    }

    // earth frame to body frame angle error conversion
    float bf_pitch_err;
    float bf_yaw_err;
    convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
    struct Tracker::NavStatus &nav_status = tracker.nav_status;
    nav_status.angle_error_pitch = bf_pitch_err;
    nav_status.angle_error_yaw = bf_yaw_err;

    // set actual and desired for logging, note we are using angles not rates
    Parameters &g = tracker.g;
    g.pidPitch2Srv.set_target_rate(pitch * 0.01);
    g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
    g.pidYaw2Srv.set_target_rate(yaw * 0.01);
    g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
}

void Mode::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
{
	// earth frame to body frame pitch and yaw conversion
    const AP_AHRS &ahrs = AP::ahrs();
    bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
    bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
}

bool Mode::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
{
    const AP_AHRS &ahrs = AP::ahrs();
    // avoid divide by zero
    if (is_zero(ahrs.cos_pitch())) {
        return false;
    }
    // convert earth frame angle or rates to body frame
    ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw;
    ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw;
    return true;
}

// return value is true if taking the long road to the target, false if normal, shortest direction should be used
bool Mode::get_ef_yaw_direction()
{
    // calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
    Parameters &g = tracker.g;
    float yaw_angle_limit_lower =   (-g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
    float yaw_angle_limit_upper =   (g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
    float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - tracker.pitch_servo_out_filt.get();
    float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - tracker.pitch_servo_out_filt.get();

    // distances to earthframe angle limits in centi-degrees
    float ef_yaw_limit_lower = yaw_angle_limit_lower;
    float ef_yaw_limit_upper = yaw_angle_limit_upper;
    float ef_pitch_limit_lower = pitch_angle_limit_lower;
    float ef_pitch_limit_upper = pitch_angle_limit_upper;
    convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
    convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);

    const AP_AHRS &ahrs = AP::ahrs();
    float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
    struct Tracker::NavStatus &nav_status = tracker.nav_status;
    float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
    float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);

    // calculate angle error to target in both directions (i.e. moving up/right or lower/left)
    float yaw_angle_error_upper;
    float yaw_angle_error_lower;
    if (ef_yaw_angle_error >= 0) {
        yaw_angle_error_upper = ef_yaw_angle_error;
        yaw_angle_error_lower = ef_yaw_angle_error - 36000;
    } else {
        yaw_angle_error_upper = 36000 + ef_yaw_angle_error;
        yaw_angle_error_lower = ef_yaw_angle_error;
    }

    // checks that the vehicle is outside the tracker's range
    if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {
        // if the tracker is trying to move clockwise to reach the vehicle,
        // but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
        if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {
            return true;
        }
        // if the tracker is trying to move counter-clockwise to reach the vehicle,
        // but the tracker could get closer to the vehicle by moving then set direction_reversed to true
        if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {
            return true;
        }
    }

    // if we get this far we can use the regular, shortest path to the target
    return false;
}