/*
 ******************************************************************************
 *
 * @file       grounddrivecontroller.cpp
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
 * @brief      Ground drive controller
 *		the required PathDesired LAND mode.
 * @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
 */

extern "C" {
#include <openpilot.h>

#include <callbackinfo.h>

#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>

#include <homelocation.h>
#include <accelstate.h>
#include <groundpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <pathsummary.h>
#include <statusgrounddrive.h>
}

// C++ includes
#include "grounddrivecontroller.h"

// Private constants

// pointer to a singleton instance
GroundDriveController *GroundDriveController::p_inst = 0;

GroundDriveController::GroundDriveController()
    : groundSettings(0), mActive(false), mMode(0)
{}

// Called when mode first engaged
void GroundDriveController::Activate(void)
{
    if (!mActive) {
        mActive = true;
        SettingsUpdated();
        controlNE.Activate();
        mMode   = pathDesired->Mode;
    }
}

uint8_t GroundDriveController::IsActive(void)
{
    return mActive;
}

uint8_t GroundDriveController::Mode(void)
{
    return mMode;
}

// Objective updated in pathdesired
void GroundDriveController::ObjectiveUpdated(void)
{}

void GroundDriveController::Deactivate(void)
{
    if (mActive) {
        mActive = false;
        controlNE.Deactivate();
    }
}


void GroundDriveController::SettingsUpdated(void)
{
    const float dT = groundSettings->UpdatePeriod / 1000.0f;

    controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP);
    controlNE.UpdateParameters(groundSettings->SpeedPI.Kp,
                               groundSettings->SpeedPI.Ki,
                               groundSettings->SpeedPI.Kd,
                               groundSettings->SpeedPI.Beta,
                               dT,
                               groundSettings->HorizontalVelMax);


    // max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse.
    controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
}

/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings)
{
    PIOS_Assert(ptr_groundSettings);

    groundSettings = ptr_groundSettings;

    return 0;
}

void GroundDriveController::UpdateAutoPilot()
{
    uint8_t result = updateAutoPilotGround();

    if (result) {
        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
    } else {
        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
    }

    PathStatusSet(pathStatus);
}

/**
 * fixed wing autopilot:
 * straight forward:
 * 1. update path velocity for limited motion crafts
 * 2. update attitude according to default fixed wing pathfollower algorithm
 */
uint8_t GroundDriveController::updateAutoPilotGround()
{
    updatePathVelocity(groundSettings->CourseFeedForward);
    return updateGroundDesiredAttitude();
}

/**
 * Compute desired velocity from the current position and path
 */
void GroundDriveController::updatePathVelocity(float kFF)
{
    PositionStateData positionState;

    PositionStateGet(&positionState);
    VelocityStateData velocityState;
    VelocityStateGet(&velocityState);
    VelocityDesiredData velocityDesired;
    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);

    // look ahead kFF seconds
    float cur[3] = { positionState.North + (velocityState.North * kFF),
                     positionState.East + (velocityState.East * kFF),
                     positionState.Down + (velocityState.Down * kFF) };
    struct path_status progress;
    path_progress(pathDesired, cur, &progress, false);

    // GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector
    // FOLLOWVECTOR:  correct_vector is distance to vector path, path_vector is the desired velocity vector

    // Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector)
    controlNE.ControlPositionWithPath(&progress);
    float north, east;
    controlNE.GetVelocityDesired(&north, &east);
    velocityDesired.North = north;
    velocityDesired.East  = east;
    velocityDesired.Down  = 0.0f;

    // update pathstatus
    pathStatus->error     = progress.error;
    pathStatus->fractional_progress  = progress.fractional_progress;
    // FOLLOWVECTOR: desired velocity vector
    pathStatus->path_direction_north = progress.path_vector[0];
    pathStatus->path_direction_east  = progress.path_vector[1];
    pathStatus->path_direction_down  = progress.path_vector[2];

    // FOLLOWVECTOR: correction distance to vector path
    pathStatus->correction_direction_north = progress.correction_vector[0];
    pathStatus->correction_direction_east  = progress.correction_vector[1];
    pathStatus->correction_direction_down  = progress.correction_vector[2];

    VelocityDesiredSet(&velocityDesired);
}

/**
 * Compute desired attitude for ground vehicles
 */
uint8_t GroundDriveController::updateGroundDesiredAttitude()
{
    StatusGroundDriveData statusGround;
    VelocityStateData velocityState;

    VelocityStateGet(&velocityState);
    AttitudeStateData attitudeState;
    AttitudeStateGet(&attitudeState);
    statusGround.State.Yaw = attitudeState.Yaw;
    statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East);

    StabilizationDesiredData stabDesired;
    StabilizationDesiredGet(&stabDesired);

    // estimate a north/east command value to control the velocity error.
    // ThrustLimits.Max(+-) limits the range.  Think of this as a command unit vector
    // of the ultimate direction to reduce lateral error and achieve the target direction (desired angle).
    float northCommand, eastCommand;
    controlNE.GetNECommand(&northCommand, &eastCommand);

    // Get current vehicle orientation
    float angle_radians  = DEG2RAD(attitudeState.Yaw); // (+-pi)
    float cos_angle      = cosf(angle_radians);
    float sine_angle     = sinf(angle_radians);

    float courseCommand  = 0.0f;
    float speedCommand   = 0.0f;
    float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
    float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
    // +ve facing correct direction, lateral command should just correct angle,
    if (forwardCommand > 0.0f) {
        // if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error

        courseCommand = boundf(lateralCommand, -1.0f, 1.0f);
        speedCommand  = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max);

        // reinstate max thrust
        controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);

        statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK;
    } else {
        // -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly.

        // Reduce steering angle based on current velocity
        float steeringReductionFactor = 1.0f;
        if (stabDesired.Thrust > 0.3f) {
            steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax;
            steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f);
        }

        // should we turn left or right?
        if (lateralCommand >= 0.1f) {
            courseCommand = 1.0f * steeringReductionFactor;
            statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT;
        } else {
            courseCommand = -1.0f * steeringReductionFactor;
            statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT;
        }

        // Impose limits to slow down.
        controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f);

        speedCommand = groundSettings->ThrustLimit.SlowForward;
    }

    stabDesired.Roll   = 0.0f;
    stabDesired.Pitch  = 0.0f;
    stabDesired.Yaw    = courseCommand;

    // Mix yaw into thrust limit TODO
    stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max);

    stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
    stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
    stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
    StabilizationDesiredSet(&stabDesired);

    statusGround.NECommand.North       = northCommand;
    statusGround.NECommand.East        = eastCommand;
    statusGround.State.Thrust          = stabDesired.Thrust;
    statusGround.BodyCommand.Forward   = forwardCommand;
    statusGround.BodyCommand.Right     = lateralCommand;
    statusGround.ControlCommand.Course = courseCommand;
    statusGround.ControlCommand.Speed  = speedCommand;
    StatusGroundDriveSet(&statusGround);

    return 1;
}