#include "ControlParams.h"
#include "assert.h"

//==================================================================
//==================================================================
//constructor, calls loadValues 
ControlParams::ControlParams()
{
  loadValues();
}
//==================================================================
//==================================================================
//opens param files for various omni types and fills structs with loaded values
void ControlParams::loadValues()
{
  //-----------------------------------------
  ifstream mainFile;
  //-----------------------------------------
  //Load 2001 Omni Params
  mainFile.open("Params/TrajectoryParams/RoboCup_2001_TrajectoryParams.txt", ios::nocreate);
  ASSERT(mainFile.fail() == 0, "CAN'T OPEN TrajectoryParams/RoboCup_2001_TrajectoryParams.txt!");
  //-------------------
  //Values which determine when to use:
  //1.) bang bang
  //2.) proportional gains
  //3.) stop robot
  READ_FLOAT_TO_OBJECT(PROP_GAIN_DISTANCE, Omni2001);
  READ_FLOAT_TO_OBJECT(STOP_DISTANCE, Omni2001);

  READ_FLOAT_TO_OBJECT(PROP_GAIN_THETA_DISTANCE, Omni2001);
  READ_FLOAT_TO_OBJECT(STOP_THETA_DISTANCE, Omni2001);
  //-------------------  
  //Default Values
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(DEFAULT.LATERAL_GAIN, Omni2001);
 
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(DEFAULT.ANGULAR_GAIN, Omni2001);
  //-------------------
  // Slow Rotate Values
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.LATERAL_GAIN, Omni2001);

  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.ANGULAR_GAIN, Omni2001);
  //-------------------  
  // Goalie Values
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(GOALIE.LATERAL_GAIN, Omni2001);
  
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(GOALIE.ANGULAR_GAIN, Omni2001);
  //-------------------
  // Spin To Kick Values
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.LATERAL_GAIN, Omni2001);

  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.ANGULAR_GAIN, Omni2001);
  //-------------------
  // Corner Speed Values
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.LATERAL_GAIN, Omni2001);

  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.ANGULAR_GAIN, Omni2001);
  //-------------------
  // Ball Possession Values
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.LATERAL_GAIN, Omni2001);
  
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.ANGULAR_GAIN, Omni2001);
  //-------------------
  // Ball Approach Values
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.LATERAL_GAIN, Omni2001);

  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.ANGULAR_GAIN, Omni2001);
  //-------------------
  // Formation Values
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_ACCELERATION, Omni2001);
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_VELOCITY, Omni2001);
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.LATERAL_GAIN, Omni2001);

  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_THETA_ACCELERATION, Omni2001);			
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_THETA_VELOCITY, Omni2001);			
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.ANGULAR_GAIN, Omni2001);
  //-------------------
  
  mainFile.close(); 
  //-----------------------------------------
  //Load 2002 Omni Params

  ///Open the paramater file:
  mainFile.open("Params/TrajectoryParams/RoboCup_2002_TrajectoryParams.txt", ios::nocreate);
  ASSERT(mainFile.fail() == 0, "CAN'T OPEN TrajectoryParams/RoboCup_2002_TrajectoryParams.txt");
  //-------------------
  //Values which determine when to use:
  //1.) bang bang
  //2.) proportional gains
  //3.) stop robot
  READ_FLOAT_TO_OBJECT(PROP_GAIN_DISTANCE, Omni2002);
  READ_FLOAT_TO_OBJECT(STOP_DISTANCE, Omni2002);

  READ_FLOAT_TO_OBJECT(PROP_GAIN_THETA_DISTANCE, Omni2002);
  READ_FLOAT_TO_OBJECT(STOP_THETA_DISTANCE, Omni2002);
  //-------------------  
  //Default Values
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(DEFAULT.LATERAL_GAIN, Omni2002);
  
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(DEFAULT.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(DEFAULT.ANGULAR_GAIN, Omni2002);
  //-------------------
  // Slow Rotate Values
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.LATERAL_GAIN, Omni2002);

  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(SLOW_ROTATE.ANGULAR_GAIN, Omni2002);
  //-------------------  
  // Goalie Values
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(GOALIE.LATERAL_GAIN, Omni2002);
  
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(GOALIE.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(GOALIE.ANGULAR_GAIN, Omni2002);
  //-------------------
  // Spin To Kick Values
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.LATERAL_GAIN, Omni2002);

  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(SPIN_TO_KICK.ANGULAR_GAIN, Omni2002);
  //-------------------
  // Corner Speed Values
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.LATERAL_GAIN, Omni2002);
  
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(CAREFUL_SPEED.ANGULAR_GAIN, Omni2002);
  //-------------------
  // Ball Possession Values
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.LATERAL_GAIN, Omni2002);
  
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(BALL_POSSESSION_SPEED.ANGULAR_GAIN, Omni2002);
  //-------------------
  // Ball Approach Values
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.LATERAL_GAIN, Omni2002);

  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(BALL_APPROACH_SPEED.ANGULAR_GAIN, Omni2002);
  //-------------------
  // Formation Values
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_ACCELERATION, Omni2002);
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_VELOCITY, Omni2002);
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.LATERAL_GAIN, Omni2002);

  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_THETA_ACCELERATION, Omni2002);			
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.MAX_THETA_VELOCITY, Omni2002);			
  READ_FLOAT_TO_OBJECT(FORMATION_SPEED.ANGULAR_GAIN, Omni2002);
  //-------------------
  mainFile.close(); 
  //-----------------------------------------
}
//==================================================================
//==================================================================
