/*
   $Id: nav.c,v 1.3 2011-01-21 11:52:44 moellmer Exp $

   Copyright (C) 2003  Pascal Brisset, Antoine Drouin

   This file is part of paparazzi.

   paparazzi 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 2, or (at your option)
   any later version.

   paparazzi 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 paparazzi; see the file COPYING.  If not, write to
   the Free Software Foundation, 59 Temple Place - Suite 330,
   Boston, MA 02111-1307, USA.

*/

#define NAV_C

#include "math.h"

#include "nav.h"
#include "estimator.h"
#include "pid.h"
#include "autopilot.h"
#include "link_fbw.h"
#include "airframe.h"

uint8_t nav_stage, nav_block;
uint8_t excpt_stage; /*To save the current stage when an exception is raised */
static float last_x, last_y;
static uint8_t last_wp;
float rc_pitch;
uint16_t stage_time, block_time;
float stage_time_ds;

#define RcRoll(travel) (from_fbw.channels[ RADIO_ROLL ]* (float)travel /(float)MAX_PPRZ)

#define RcEvent1() CheckEvent(rc_event_1)
#define RcEvent2() CheckEvent(rc_event_2)
#define Block(x) case x: nav_block=x;
#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); }
#define NextBlock() { nav_block++; InitBlock(); }
#define GotoBlock(b) { nav_block=b; InitBlock(); }

#define Stage(s) case s: nav_stage=s;
#define InitStage() { last_x = estimator_x; last_y = estimator_y; stage_time = 0; stage_time_ds = 0.0; return; }
#define NextStage() { nav_stage++; InitStage() }
#define NextStageFrom(wp) { last_wp = wp; NextStage() }
#define GotoStage(s) { nav_stage = s; InitStage() }

#define Label(x) label_ ## x:
#define Goto(x) { goto label_ ## x; }

#define Exception(x) { excpt_stage = nav_stage; goto label_ ## x; }
#define ReturnFromException(_) { GotoStage(excpt_stage) }

static bool_t approaching( uint8_t );
static inline void fly_to_xy( float x, float y );
static void fly_to( uint8_t wp );
static void route_to( uint8_t last_wp, uint8_t wp );
//static void glide_to(uint8_t last_wp, uint8_t wp);

#define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))

#define DegOfRad(x) ((x) / M_PI * 180.)
#define RadOfDeg(x) ((x)/180. * M_PI)
#define NormCourse(x) { _Pragma("loopbound min 0 max 0") while (x < 0) x += 360; _Pragma("loopbound min 0 max 0") while (x >= 360) x -= 360;}

static float qdr; /* Degrees from 0 to 360 */

#define CircleXY(x,y,radius) { float alpha = atan2(estimator_y - y, estimator_x - x);float alpha_carrot = alpha + CARROT / (-radius * estimator_hspeed_mod); fly_to_xy(x+cos(alpha_carrot)*fabs(radius),y+sin(alpha_carrot)*fabs(radius)); qdr = DegOfRad(M_PI/2 - alpha_carrot); NormCourse(qdr);}

#define MAX_DIST_CARROT 250.
#define MIN_HEIGHT_CARROT 50.
#define MAX_HEIGHT_CARROT 150.

#define Goto3D(radius) { static float carrot_x, carrot_y; int16_t pitch; int16_t roll; if (pprz_mode == PPRZ_MODE_AUTO2) { int16_t yaw = from_fbw.channels[ RADIO_YAW ]; if (yaw > MIN_DX || yaw < -MIN_DX) { carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);carrot_x = Min(carrot_x, MAX_DIST_CARROT); carrot_x = Max(carrot_x, -MAX_DIST_CARROT); } pitch = from_fbw.channels[ RADIO_PITCH ]; if (pitch > MIN_DX || pitch < -MIN_DX) { carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); carrot_y = Min(carrot_y, MAX_DIST_CARROT); carrot_y = Max(carrot_y, -MAX_DIST_CARROT);} vertical_mode = VERTICAL_MODE_AUTO_ALT; roll =  from_fbw.channels[ RADIO_ROLL ]; if (roll > MIN_DX || roll < -MIN_DX) { desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT); desired_altitude = Min(desired_altitude, MAX_HEIGHT_CARROT+GROUND_ALT); } } CircleXY(carrot_x, carrot_y, radius); }

#define Circle(wp, radius) CircleXY(waypoints[ wp ].x, waypoints[ wp ].y, radius)

#define And(x, y) ((x) && (y))
#define Or(x, y) ((x) || (y))
//#define Min(x,y) (x < y ? x : y)
//#define Max(x,y) (x > y ? x : y)
#define Qdr(x) (Min(x, 350) < qdr && qdr < x+10)

#include "flight_plan.h"


#define MIN_DIST2_WP (15.*15.)

#define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y))

const int32_t nav_east0 = NAV_UTM_EAST0;
const int32_t nav_north0 = NAV_UTM_NORTH0;

float desired_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
float desired_x, desired_y;
uint16_t nav_desired_gaz;
float nav_pitch = NAV_PITCH;
float nav_desired_roll;

float dist2_to_wp, dist2_to_home;
bool_t too_far_from_home;
const uint8_t nb_waypoint = NB_WAYPOINT;

struct point waypoints[ NB_WAYPOINT + 1 ] = WAYPOINTS

      static float carrot;
static bool_t approaching( uint8_t wp )
{
  float pw_x = waypoints[ wp ].x - estimator_x;
  float pw_y = waypoints[ wp ].y - estimator_y;
  float scal_prod;//changes for PowerPc Fev-06

  dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
  carrot = CARROT * estimator_hspeed_mod;
  carrot = ( carrot < 40 ? 40 : carrot );
  if ( dist2_to_wp < carrot * carrot )
    return TRUE;

  scal_prod = ( waypoints[ wp ].x - last_x ) * pw_x + ( waypoints[ wp ].y - last_y ) *
              pw_y;

  return ( scal_prod < 0 );
}

static inline void fly_to_xy( float x, float y )
{
  desired_x = x;
  desired_y = y;
  desired_course = M_PI / 2. - atan2( y - estimator_y, x - estimator_x );
}

static void fly_to( uint8_t wp )
{
  fly_to_xy( waypoints[ wp ].x, waypoints[ wp ].y );
}

static float alpha, leg;
static void route_to( uint8_t _last_wp, uint8_t wp )
{
  float last_wp_x = waypoints[ _last_wp ].x;
  float last_wp_y = waypoints[ _last_wp ].y;
  float leg_x = waypoints[ wp ].x - last_wp_x;
  float leg_y = waypoints[ wp ].y - last_wp_y;
  float leg2 = leg_x * leg_x + leg_y * leg_y;
  alpha = ( ( estimator_x - last_wp_x ) * leg_x + ( estimator_y - last_wp_y ) *
            leg_y ) / leg2;
  alpha = Max( alpha, 0. );
  leg = sqrt( leg2 );
  alpha += Max( carrot / leg, 0. ); /* carrot computed in approaching() */
  alpha = Min( 1., alpha );
  fly_to_xy( last_wp_x + alpha * leg_x, last_wp_y + alpha * leg_y );
}

/*static void glide_to(uint8_t _last_wp, uint8_t wp) {
  float last_alt = waypoints[ _last_wp ].a;
  desired_altitude = last_alt + alpha * (waypoints[ wp ].a - last_alt);
  pre_climb = NOMINAL_AIRSPEED * (waypoints[ wp ].a - last_alt) / leg;
  }*/

static inline void compute_dist2_to_home( void )
{
  float ph_x = waypoints[ WP_HOME ].x - estimator_x;
  float ph_y = waypoints[ WP_HOME ].y - estimator_y;
  dist2_to_home = ph_x * ph_x + ph_y * ph_y;
  too_far_from_home = dist2_to_home > ( MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME );
}

void nav_home( void )
{
  //Circle(WP_HOME, 50); /* FIXME: radius should be defined elsewhere */
  {
    float alpha = atan2( estimator_y - waypoints[ WP_HOME ].y,
                         estimator_x - waypoints[ WP_HOME ].x );
    float alpha_carrot = alpha + CARROT / ( -50 * estimator_hspeed_mod );
    fly_to_xy( waypoints[ WP_HOME ].x + cos( alpha_carrot )*fabs( 50 ),
               waypoints[ WP_HOME ].y + sin( alpha_carrot )*fabs( 50 ) );
    qdr = DegOfRad( M_PI / 2 - alpha_carrot );
    {
      _Pragma( "loopbounds min 0 max 2" )
      while ( qdr < 0 ) qdr += 360;
      _Pragma( "loopbounds min 0 max 2" )
      while ( qdr >= 360 ) qdr -= 360;
    }
  }

  nav_pitch = 0.; /* Nominal speed */
  vertical_mode = VERTICAL_MODE_AUTO_ALT;
  desired_altitude = GROUND_ALT + 50;
  compute_dist2_to_home();
  dist2_to_wp = dist2_to_home;
}

void nav_update( void )
{
  compute_dist2_to_home();

  auto_nav();
}


void nav_init( void )
{
  nav_block = 0;
  nav_stage = 0;
}