/* 
    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

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 */