/* 
    Robot Simulator

    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 2
    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.

    (C) 2006 Jason Hunt
    nulluser@gmail.gom 
*/



#include <stdio.h>
#include <math.h>

#include "robot.h"
#include "main.h"
#include "world.h"
#include "console.h"
#include "utility.h"

#define NUM_TIMERS 10

#define AVERAGE_SIZE    2

#define ir_fl  robot.ir[0]
#define ir_f    robot.ir[1]
#define ir_fr  robot.ir[2]

#define num_actions 3

typedef struct action_type 
{
    double pri;
    double dir;
    double speed;
};


action_type actions[num_actions];

void clamp ( double &val, double max )
{
    if (val > 0)
    {
        if (val > max) val = max;
    } else
    {
        if (val < -max) val = -max;       
    }
}


void compute_forward ( void )
{
    actions[0].dir = 0;
    actions[0].speed = 0.1;
    actions[0].pri =  0.1;

}


void compute_reverse ( void )
{
 //  double ir_fl = robot.ir[0];
  //  double ir_f =  robot.ir[1];
  //  double ir_fr = robot.ir[2];

    static double rev_bias = 0;
    static double turn_bias = 0;
    
    actions[2].dir =  turn_bias;
    actions[2].speed = -0.05;////0.1;
    actions[2].pri =  rev_bias;


    char b[300];
    sprintf(b, "ir: %f %f %f\n", ir_fl,ir_f,ir_fr);
   // write_console(b);

        // reverse needed
    if (ir_f > 1.6)
    {
        turn_bias = ir_fl - ir_fr + f_rand(-1,1);
        rev_bias = 1;
    }
    
    rev_bias /= 1.2;
    turn_bias /= 1.1;



}


void compute_avoid ( void )
{
    //double ir_fl = robot.ir[0];
   // double ir_f =  robot.ir[1];
   // double ir_fr = robot.ir[2];

    
    actions[1].dir = ir_fl - ir_fr;
    actions[1].speed = 0;//0.1;
    actions[1].pri = (ir_fl + ir_fr) ;    
}


void robot_auto_mode_test( void )
{
    
    compute_forward();
    compute_avoid();
    compute_reverse();
    
    double dir = 0;
    double speed = 0;
    
    for (int i = 0; i < num_actions; i++)
    {
        dir += actions[i].dir * actions[i].pri;
        speed += actions[i].speed * actions[i].pri;
    } 
    
    robot.speed = speed;
    robot.ddir = dir * 0.1;
    
    clamp(robot.ddir, 0.1);
    clamp(robot.speed , 0.1);
}














double nav_average[AVERAGE_SIZE] = { 0 };


/* Set new motion direction and speed for robot */
void set_motion(double speed, double dir, double speed_rand, double dir_rand)
{
    if(manual_mode) return;

    robot.speed = speed + f_rand(-speed_rand, speed_rand);

    if (robot.speed > 0.07)
        robot.speed = 0.07;
 
    robot.ddir = dir + f_rand(-dir_rand, dir_rand);
}
/* End of set_motion */



/* Setup or check a timer */
double timer_func(double v, unsigned int timer_num, double threshhold, double lim)
{
    if (timer_num >= NUM_TIMERS) return(0);

    double timer_time = 1.0;

    static bool first_run = 1;
    static double stime[NUM_TIMERS];
    static double limits[NUM_TIMERS];
    
    if (first_run)
    {
        for (int i = 0; i < NUM_TIMERS; i++)
        {
            stime[i] = 0;
            limits[NUM_TIMERS];
        }

        first_run = 0;
    }
    

    // Timer already running
    if (stime[timer_num] > 0)
    {
         double time_diff = get_time() - stime[timer_num];

        // Timer done?
        if (time_diff > timer_time)
        {            
            stime[timer_num] = 0;
            return(1);
        }

        return(limits[timer_num] * time_diff / timer_time);

    } else
    if (v >= threshhold)   // Start timer
    {
        stime[timer_num] = get_time();
        limits[timer_num] = lim;
        return(0);            
    }         
    return(0);
}
/* End of timer_func */


/* Compute neural network for automode */
void robot_auto_mode( void )
{
    
    for (int i = 0; i < AVERAGE_SIZE - 1; i++)
    {
        nav_average[i] = nav_average[i+1];
    }
    nav_average[AVERAGE_SIZE - 1] = robot.nav_off;
    
    
    double nav_ave = 0;
    for (int i = 0; i < AVERAGE_SIZE; i++)
        nav_ave += nav_average[i];

    nav_ave /=  (double)AVERAGE_SIZE;
    
    
   // char b[300];
   // sprintf(b, "ave; %f\n", nav_ave);
   // write_console(b);
    
 //   double ir_fl = robot.ir[0];
   // double ir_f =  robot.ir[1];
   // double ir_fr = robot.ir[2];
            
    
    double sl = robot.touch[0];
    double sf = robot.touch[1];
    double sr = robot.touch[2];

    double rev_sum = 0.2*ir_fl + 0.2*ir_fr + 0.4*ir_f + 0.4*sl + 0.4 * sr + 2.0 * sf;

  
    double rev_time_mod = timer_func(rev_sum, 0, 2, 2);
    double turn_time_mod = timer_func(rev_time_mod , 1, 1, f_rand(-2,2));


       double mag = robot.nav_mag;  
       if (mag < 0.5) mag = 0.5;
       
    double ms =  1 - 0.15*ir_fl - 0.15*ir_fl - 1.1*rev_time_mod;
//    double ms = 1 - sl - sr - 4*rev_time_mod;

    // 2.5 turn mag works

    double mr = 1.5* nav_ave + 0.50*ir_fl - 0.50*ir_fr - 1.0*turn_time_mod;

//    double mr = 1.5* robot.nav_off + 0.50*ir_fl - 0.50*ir_fr - 1.0*turn_time_mod;

//    double mr = sl - sr - 2*turn_time_mod;    


    robot.ddir = 0.1 * mr;
    robot.speed = 0.05 *  ms;

    if (robot.ddir < 0)
    {
        if (robot.ddir < -0.3)
            robot.ddir = -0.3;
            
    } else
    {
        if (robot.ddir > 0.3)
            robot.ddir = 0.3;

    }



    // Wrap speed
    if (robot.speed < 0)
        robot.speed = -robot.speed;

    // Clamp speed
    if (robot.speed > 0.05)
        robot.speed = 0.05;
}
/* End of robot_auto_mode */














    
