
#include <bur/plctypes.h>

//#include "Camera/Camera.h"

#include <math.h>

#include <cstring>
#include <cstdio>

//---GAME INCLUDES---//
#include "Functions.h"

//---STRATEGY INCLUDES---//

#include "Strategy/Structs.h"
#include "Strategy/BaseStrategy.h"
#include "Strategy/StrategyB.h"
#include "Strategy/StrategyC.h"



#ifdef _DEFAULT_INCLUDES
#include <AsDefault.h>
#endif

BaseStrategy* strategy;

// amount of memory to be allocated for heap storage must be specified for every ANSI C++ program with the bur_heap_size variable
unsigned long bur_heap_size = 0xFFFF;
unsigned long timer[3] = {0};

//CameraClass camera;

void _INIT ProgramInit(void)
{
	//--Strategy--//
	outputs.lights.Top = false;
	outputs.lights.Red = false;
	outputs.lights.Blue = false;
	
	strategy = new StrategyB();
	
	//--MappView--//
	

}

void _CYCLIC ProgramCyclic(void)
{

	// CHECK ERROR
	
		
	// PROCCESS STUFF
	switch(SystemState){
		case NOTHING:
			strcpy(DEBUG_TEXT,"\0");
			
			matchActive = false;
								

			forn(i, 4){
				hmi.axes.red[i].lin = 0;
				hmi.axes.red[i].rot = 0;
			}
			
			//lastSystemState = NOTHING;
			
			break;
		case INIT:
			strcpy(DEBUG_TEXT, "Initialising axes...\0");
			
			hmi.axes.init = false;
			matchActive = true;
			motion.cmd.init = true;	
			SystemState = WAIT_FOR_INIT;
				
			break;					
		case WAIT_FOR_INIT:
			
			if(motion.status.overall.homed && motion.status.overall.ready){
				SystemState = nextSystemState;
			}
			
			
			break;
		case SERVICE:
			strcpy(DEBUG_TEXT, "Service\0");
			
			// kick
			forn(i, 4){
			
				if(hmi.axes.red[i].kick){
					hmi.axes.red[i].kickStrength++;
					hmi.axes.red[i].isSide = false;
				}
				if(hmi.axes.red[i].sidekick){
					hmi.axes.red[i].kickStrength++;
					hmi.axes.red[i].isSide = true;
				}
				
				if(hmi.axes.red[i].kickStrength > 0 && !hmi.axes.red[i].kick && !hmi.axes.red[i].sidekick){
					motion.cmd.move.rot[i].power = hmi.axes.red[i].kickStrength > 500 ? 100 : (hmi.axes.red[i].kickStrength / 5);
					hmi.axes.red[i].isSide ? motion.cmd.move.rot[i].sideShoot = 1 : motion.cmd.move.rot[i].shoot = 1;
					hmi.axes.red[i].kickStrength = 0;
				}
							
				
			}
	
			
			//move Axis from service
			
			if(!safety.error){
				forn(i, 4){
					
					if(motion.cmd.move.rot[i].shoot || motion.cmd.move.rot[i].sideShoot) continue;
					
					motion.cmd.move.lin[i].pos = hmi.axes.red[i].lin;
					motion.cmd.move.rot[i].pos = hmi.axes.red[i].rot * 10;
				}
			}
			else{
				forn(i, 4){
					
					hmi.axes.red[i].lin = motion.status.lin[i].pos;
					hmi.axes.red[i].rot = motion.status.rot[i].pos / 10;
				}
			}
			

		
		
			break;
		case PLAY:
			switch(GameState){
				case PLAYING:
					strcpy(DEBUG_TEXT, "Game : playing\0");
					
					if(camera.ball[camera.mode].newdata){
						camera.ball[camera.mode].newdata = false;
						strategy->CameraInput(camera.ball[camera.mode].X, camera.ball[camera.mode].Y);
						strategy->Process();
				
						forn(i, 4){
							if( !(motion.cmd.move.rot[i].shoot || motion.cmd.move.rot[i].sideShoot )){	
						
								motion.cmd.move.lin[i].pos = strategy->axes[i].desiredDisplacement;
												
								if(strategy->axes[i].mode == forwardShoot){
									motion.cmd.move.rot[i].shoot = true;
									motion.cmd.move.rot[i].power = 80;						
									hmi.axes.red[i].kick = 1;
								}else if(strategy->axes[i].mode == backwardShoot){
									motion.cmd.move.rot[i].sideShoot = true;
									motion.cmd.move.rot[i].power = 80;						
									hmi.axes.red[i].kick = 1;
								}else if(strategy->axes[i].mode == raised){
									motion.cmd.move.rot[i].pos = -900;
								}else if(strategy->axes[i].mode == backwardDefense){
									motion.cmd.move.rot[i].pos = 155;						
								}else if(strategy->axes[i].mode == forwardDefense){
									motion.cmd.move.rot[i].pos = -100;	
							
								}else if(motion.cmd.move.rot[i].shoot){
									motion.cmd.move.lin[i].pos = strategy->axes[i].desiredDisplacement;
								}
						
											
								strategy->axes[i].realDisplacement = motion.status.lin[i].pos;
												
							}
							if(hmi.axes.red[i].kick && !(motion.cmd.move.rot[i].shoot && motion.cmd.move.rot[i].sideShoot)){
								strategy->axes[i].mode == forwardDefense;
								hmi.axes.red[i].kick == 0;
							}					
						}
					}
				
					if(sensor.isGoal[0] || sensor.isGoal[1]){
						GameState = GOAL;
					}
					break;
				case GOAL:
					if(sensor.isGoal[0]){
						sensor.isGoal[0] = false;
						hmi.game.scoreBlu++;
						blink.red += 12;						
					}
					else if(sensor.isGoal[1]){
						sensor.isGoal[1] = false;
						hmi.game.scoreRed++;
						blink.blue += 12;						
					}
				
					if(hmi.game.scoreRed >= hmi.game.scoreCap){
						GameState = END;
						blink.top += 20;
						hmi.game.playButton.pushed = 0;
						hmi.game.pauseButton.pushed = 0;
					}
					else if(hmi.game.scoreBlu >= hmi.game.scoreCap){
						GameState = END;
						blink.top += 20;
						hmi.game.playButton.pushed = 0;
						hmi.game.pauseButton.pushed = 0;
					}else{
					
						GameState = WAIT;
					}
					
					break;
				case WAIT:
					strcpy(DEBUG_TEXT, "Game : paused\0");
					
					if(camera.mode != 2 && !hmi.game.pauseButton.pushed){
						GameState = PLAYING;
					}
					else{
					
					}
				
					break;
				case END:
					strcpy(DEBUG_TEXT, "Game : end\0");
					
					break;
					
				}
		
			break;
		case SIMU_TEST:
			strcpy(DEBUG_TEXT, "Strategy test\0");
			
			if(camera.ball[camera.mode].newdata){
				camera.ball[camera.mode].newdata = false;
				strategy->CameraInput(camera.ball[camera.mode].X, camera.ball[camera.mode].Y);
				strategy->Process();
				
				forn(i, 4){
					if( !(motion.cmd.move.rot[i].shoot || motion.cmd.move.rot[i].sideShoot )){	
						
						motion.cmd.move.lin[i].pos = strategy->axes[i].desiredDisplacement;
												
						if(strategy->axes[i].mode == forwardShoot){
							motion.cmd.move.rot[i].shoot = true;
							motion.cmd.move.rot[i].power = 80;						
							hmi.axes.red[i].kick = 1;
						}else if(strategy->axes[i].mode == backwardShoot){
							motion.cmd.move.rot[i].sideShoot = true;
							motion.cmd.move.rot[i].power = 80;						
							hmi.axes.red[i].kick = 1;
						}else if(strategy->axes[i].mode == raised){
							motion.cmd.move.rot[i].pos = -900;
						}else if(strategy->axes[i].mode == backwardDefense){
							motion.cmd.move.rot[i].pos = 100;						
						}else if(strategy->axes[i].mode == forwardDefense){
							motion.cmd.move.rot[i].pos = -100;	
							
					}else if(motion.cmd.move.rot[i].shoot){
						motion.cmd.move.lin[i].pos = strategy->axes[i].desiredDisplacement;
					}
						
											
						strategy->axes[i].realDisplacement = motion.status.lin[i].pos;
												
					}
					if(hmi.axes.red[i].kick && !(motion.cmd.move.rot[i].shoot && motion.cmd.move.rot[i].sideShoot)){
						strategy->axes[i].mode == forwardDefense;
						hmi.axes.red[i].kick == 0;
					}					
				}
			}
				
			break;
		
		case AXES_FOLLOW:
			strcpy(DEBUG_TEXT, "Follow axes\0");
			
			forn(i, 4){
				motion.cmd.move.lin[i].pos = sensor.human.pos[i];
				
				if(!sensor.human.block[i] && !motion.cmd.move.rot[i].shoot){
					motion.cmd.move.rot[i].shoot = true;
					motion.cmd.move.rot[i].power = 80;
				}
			
			}
			
			
			
			break;
		
		
			
		
		
			
	}//end switch
	
	
	
	
	
	
	
	
	
	

}

void _EXIT ProgramExit(void)
{

}
