/*
	Script for automatic rotor stop when a solar panel or oxygen farm has high output.
	Version: 1.2
	Sigurd Hansen

	This program vil autostart at compile. Use argument stop or start to manually override

	*/
	#region Configuration

	// HighPwrValue - Set this to your minimum power requirement in kW per panel. (Or oxygen L/min)
	const float HighPwrValue = 119.4f;

	// NameOfSolar - Search for solar blocks or oxygen farms with this name.
	//  Maximum one item per array should be returned. Multiple arrays supported.
	const string NameOfSolar = "SolarV";

	// NameOfRotorX - Search for rotor blocks with this name. Maximum one item per array should be returned.
	//  Multiple arrays supported.
	const string NameOfRotorX = "motorV";

	// rotorspeed - Maximum speed of rotor. Will be dynamically adjusted down when close to target.
	//  Recommended value: Less that 1.0f due to ingame bug
	const float rotorspeed = 0.9f;

	// EnableTwoAxisMode - Enable dual axis mode.
	//  Recommended value: Depends on design.
	const bool EnableTwoAxisMode = false;

	// NameOfRotorY - Search for this name for Y axis. Must only find Y axis rotors.
	//  Maximum one item per array should be returned. Multiple arrays supported.
	const string NameOfRotorY = "RotorY";

	// EnableOxygenMode - Enables oxygen mode.
	const bool EnableOxygenMode = false;

	// Auto set torque and braking torque to best practice value.
	//  Recommended value: true
	const bool ForceAutoTorque = true;

	// Enable LCD Output
	const bool EnableLCD = true;
	const string NameOfLCD = "LCDSolarr";

	// Duplication
	// Applying rotor values to other rotors as well if set to true.
	//  If unsure, set all to false. Recommended value: Depends on design.
	const bool EnableDuplicateRotor1 = false;
	const bool EnableDuplicateRotor2 = false;
	const bool EnableDuplicateRotor3 = false;

	// You might want to inverse some rotors, for example on the other side of the axis.
	//  If so, set the value to true. If unsure change later if rotors lock at wrong angle.
	const bool InverseDuplicateRotor1 = false;
	const bool InverseDuplicateRotor2 = false;
	const bool InverseDuplicateRotor3 = false;

	// Enter the name of the source rotors you want to duplicate.
	const string NameOfDuplicateSource1 = "RotorY";
	const string NameOfDuplicateSource2 = "RotorY";
	const string NameOfDuplicateSource3 = "RotorF";

	// Enter the name of the destination rotors you want to duplicate.
	//  The code will search for rotors. For example:
	//  Entering RotorZ will duplicate changes to RotorZa, RotorZB, RotorZQ and so forth.
	const string SearchForDuplicateDest1 = "RotorZ";
	const string SearchForDuplicateDest2 = "RotorH";
	const string SearchForDuplicateDest3 = "RotorG";

	// Auto night mode (Beta)
	//  Turns off rotors if night is detected. Suboptimal axis towards sun might trigger night mode at daytime.
	//  Use at your own risk. Not recommended for ships.
	const bool AutoNightMode = false;

	// Trigger this timer at max power
	const bool ControlCustomTimerAtMaxPower = false;
	const string NameForCustomtriggerAtMaxPower = "Timer Lights On";

	// Trigger this timer at low power
	const bool ControlCustomTimerAtLowPower = false;
	const string NameForCustomtriggerAtLowPower = "Timer Lights Off";

	// Changes below this line not recommended.
	//------------------------------------------------------------
	#endregion
	List<IMyTerminalBlock> solarBlocks = new List<IMyTerminalBlock>();
	List<IMyTerminalBlock> rotorBlocksX = new List<IMyTerminalBlock>();
	List<IMyTerminalBlock> rotorBlocksY = new List<IMyTerminalBlock>();
	List<IMyTerminalBlock> Rotors = new List<IMyTerminalBlock>();
	List<IMyTerminalBlock> SourceRotors = new List<IMyTerminalBlock>();
	List<IMyTerminalBlock> LcdBlocks = new List<IMyTerminalBlock>();
	IMyTimerBlock timer2;
	IMyTimerBlock timer3;
	bool firstrun = true;
	bool nightmode = false;
	bool shortturn = false;
	long lCount = 0;
	int delayCount = 0;
	string textToLCD = "";
	float maxPwrValue;

public Program() {
	Runtime.UpdateFrequency |= UpdateFrequency.Update100;
	if (Storage.Length > 0) {
		string[] parts = Storage.Split(';');
		float.TryParse(parts[0], out maxPwrValue);
	}
}

public void Save() {
	Storage = maxPwrValue + ";";
	//Echo("Saved: " + maxPwrValue.ToString());
}
	#region Main
	void Main(string argument)
	{
		if (firstrun) {
			// Echo = text => {}; // Uncommenting this line might increase performance on servers. Will disable echo
			GridTerminalSystem.SearchBlocksOfName(NameOfSolar, solarBlocks); // Search for Solar Blocks
			GridTerminalSystem.SearchBlocksOfName(NameOfRotorX, rotorBlocksX); // Search for Rotor Blocks
			GridTerminalSystem.SearchBlocksOfName(NameOfRotorY, rotorBlocksY); // Search for RotorY Blocks
			if (EnableLCD) GridTerminalSystem.SearchBlocksOfName(NameOfLCD, LcdBlocks); // Search for LCD Blocks
			if (solarBlocks.Count == 0) { throw new Exception("Cannot find solar panel. Check name and recompile."); }
			if (rotorBlocksX.Count == 0) { throw new Exception("Cannot find x-axis rotor blocks. Check name and recompile."); }
			if (rotorBlocksY.Count == 0 && EnableTwoAxisMode) { throw new Exception("Cannot find y-axis rotor blocks. Check name and recompile."); }
			if (rotorBlocksY.Count < rotorBlocksX.Count && EnableTwoAxisMode) {
				int diff = rotorBlocksX.Count - rotorBlocksY.Count;
				throw new Exception(diff + " Y-axis rotors missing. Fix and recompile.");
			}
			if (solarBlocks.Count > rotorBlocksX.Count) { throw new Exception("Too many solar panels found. Check solar panel names."); }
			Echo ("Init...\nX rotors: " + rotorBlocksX.Count.ToString() + "\nY rotors: " + rotorBlocksY.Count.ToString());
			Echo ("Solar panels/Oxygen farms: " + solarBlocks.Count.ToString());
			if (EnableTwoAxisMode) Echo ("Dual Axis mode enabled");
			if (EnableOxygenMode) Echo ("Oxygen Farm mode ON");
			if (EnableDuplicateRotor1 || EnableDuplicateRotor2 || EnableDuplicateRotor3) Echo ("Duplication activated.");
			if (ControlCustomTimerAtMaxPower) timer2 = GridTerminalSystem.GetBlockWithName(NameForCustomtriggerAtMaxPower) as IMyTimerBlock;
			if (ControlCustomTimerAtLowPower) timer3 = GridTerminalSystem.GetBlockWithName(NameForCustomtriggerAtLowPower) as IMyTimerBlock;

			firstrun = false;
		}
			if (argument == "start") Runtime.UpdateFrequency |= UpdateFrequency.Update100;
			else if (argument == "stop") Runtime.UpdateFrequency &= ~UpdateFrequency.Update100;
			lCount++;
			if (lCount == 3) {
				lCount = 0;
				shortturn = false;
			} else  {
				shortturn = true;
				//return;
			}
	    textToLCD = "Auto Optimize Solar Panels / Oxygen farm\n";
		bool[] rotorOn = new bool[rotorBlocksX.Count]; // Stop or start rotor
		bool[] rotorLow = new bool[rotorBlocksX.Count]; // True when power is way to low
		bool[] rotorFineTune = new bool[rotorBlocksX.Count]; // Fine Tune, very slow rotor
		bool[] reverse = new bool[rotorBlocksX.Count]; // Reverse rotor
		bool[] rotorOnY = new bool[rotorBlocksY.Count]; // Stop or start rotor
		bool containsFalse = false;
		float pwr = 0f, lastPwr = 0f; // Current and last power reading

		if (EnableOxygenMode) {
			for(int i = 0; i < solarBlocks.Count; i++) { // For each oxygen farm...
			var solar = solarBlocks[i] as IMyOxygenFarm; // Support for oxygen farm
			if(solar != null) { // Yes I am
				GetOxygen(solar, ref pwr); // Get oxygen level, return into existing pwr variable
				lastPwr = GetAndSetLastOxygen(solar, pwr); // Get and set last runs oxygen level
				reverse[i] = (lastPwr < pwr || pwr == 0) ? false : true; // Change rotor direction
				rotorOn[i] = (pwr <= HighPwrValue) ? true : false; // Turn on rotor
				rotorLow[i] = (pwr < HighPwrValue/2) ? true : false; // Slow or fast rotor
				rotorFineTune[i] = (pwr > HighPwrValue*10/11) ? true : false; // Fine tune rotor
				}
			}
		}	else {
			for(int i = 0; i < solarBlocks.Count; i++) { // For each solar panel...
			var solar = solarBlocks[i] as IMySolarPanel; // I am a Solar Panel
			if(solar != null) { // Yes I am
				GetPower(solar, ref pwr); // Get Power from solar panel, return into existing pwr variable
				lastPwr = GetAndSetLastPwr(solar, pwr); // Get and set last runs power
				reverse[i] = (lastPwr < pwr || pwr == 0) ? false : true; // Change rotor direction
				rotorOn[i] = (pwr <= HighPwrValue) ? true : false; // Turn on rotor
				rotorLow[i] = (pwr < HighPwrValue/2) ? true : false; // Slow or fast rotor
				rotorFineTune[i] = (pwr > HighPwrValue*10/11) ? true : false; // Fine tune rotor
				if (lastPwr == 0 && pwr == 0 && AutoNightMode || Me.TerminalRunArgument == "Nightmode") { // TEST
					nightmode = true;
				} else {
					nightmode = false;
				}
				}
			}
		}
		if (shortturn) return;

		if (nightmode) {
			textToLCD += "Night mode\n";
			// TODO
		}

		for(int i = 0; i < rotorBlocksX.Count; i++) { // For each rotorX...
			IMyMotorStator rotor = rotorBlocksX[i] as IMyMotorStator; // I am a Rotor
			if(rotor != null) { // Yes I am
				if (ForceAutoTorque) { // Force torque.
					rotor.BrakingTorque = float.MaxValue;
					rotor.Torque = 30000000f;
				}
				if (nightmode) {
					TriggerRotor(rotor, false, false, ref containsFalse); // Stop rotor
				} else {
					SetRotorSpeed(rotor, rotorLow[i], rotorFineTune[i]); // Dynamic rotor speed
					if (!rotorOn[i]) { // Turn off...
						TriggerRotor(rotor, false, false, ref containsFalse); // Stop rotor
					}			else if (rotorOn[i] && EnableTwoAxisMode && reverse[i]) { // Turn On, dual axis mode, and reverse
						rotorOnY[i] = CheckAndUpdateRotorName(rotor);
						if (rotorOnY[i]) {
							TriggerRotor(rotor, false, false, ref containsFalse); // Y on, therefore X off.
						}

						if (!rotorOnY[i]) {
							TriggerRotor(rotor, true, reverse[i], ref containsFalse); // Y off, therefore X on, and reverse.
						}
					}	else if (rotorOn[i] && EnableTwoAxisMode && !reverse[i]) {
						rotorOnY[i] = CheckAndUpdateRotorName(rotor);
						if (!rotorOnY[i]) {
							TriggerRotor(rotor, true, reverse[i], ref containsFalse); // Start rotor. Reverse if needed
						}
					}	else {
						TriggerRotor(rotor, true, reverse[i], ref containsFalse); // Start rotor. Reverse if needed
					}
				}
			}
		}

		if (EnableTwoAxisMode) {
			for(int i = 0; i < rotorBlocksY.Count; i++) { // For each rotorY...
				IMyMotorStator rotor = rotorBlocksY[i] as IMyMotorStator; // I am a Rotor
				if(rotor != null) { // Yes I am
					if (ForceAutoTorque) {
					rotor.BrakingTorque = float.MaxValue;
					rotor.Torque = 30000000f;
					}
					if (nightmode) {
						TriggerRotor(rotor, false, false, ref containsFalse); // Stop rotor
					} else {
						if (rotorOnY[i] == true) {
							SetRotorSpeed(rotor, rotorLow[i], rotorFineTune[i]); // Dynamic rotor speed
							TriggerRotor(rotor, true, reverse[i], ref containsFalse); // Start rotor Y. Reverse if needed
						} else {
							TriggerRotor(rotor, false, false, ref containsFalse);
						}
					}
				}
			}
		}
	textToLCD += "Max Power detected: " + maxPwrValue.ToString() + "\n";

	//textToLCD += "CurrentInstructionCount: " + Runtime.CurrentInstructionCount.ToString() + " / "  + Runtime.MaxInstructionCount.ToString()+ "\n";
	//textToLCD += "TimeSinceLastRun: " + Runtime.TimeSinceLastRun.ToString() + "\n";


	// Duplicate stuff
	if (EnableDuplicateRotor3 && !containsFalse) { // Main rotors aligned, lets duplicate changes...
			RotorDuplicate(NameOfDuplicateSource3, SearchForDuplicateDest3, InverseDuplicateRotor3);
		}
		if (EnableDuplicateRotor2 && !containsFalse) { // Main rotors aligned, lets duplicate changes...
			RotorDuplicate(NameOfDuplicateSource2, SearchForDuplicateDest2, InverseDuplicateRotor2);
		}
		if (EnableDuplicateRotor1 && !containsFalse) { // Main rotors aligned, lets duplicate changes...
			RotorDuplicate(NameOfDuplicateSource1, SearchForDuplicateDest1, InverseDuplicateRotor1);
		}
	if (!containsFalse && ControlCustomTimerAtMaxPower) { // Main rotors aligned, lets control a timer
		delayCount = 0;
		ControlTimer(timer2);
		}
	if (containsFalse && ControlCustomTimerAtLowPower){
			delayCount++;
			if (delayCount == 5) {
				delayCount = 0;
				ControlTimer(timer3);
			}
		}

	if (EnableLCD) PrintToLCD(textToLCD);
	}

	#endregion
	#region Methods

	void PrintToLCD(string text) {
		if (EnableLCD) {
			for(int i = 0; i < LcdBlocks.Count; i++) { // For each LCD...
				IMyTextPanel LCD = LcdBlocks[i] as IMyTextPanel;
				LCD.WriteText(text, false);
				LCD.SetValue("FontSize", 0.9f);
                LCD.ContentType = ContentType.TEXT_AND_IMAGE;
                //((IMyTextSurface)DisplayBlock).ContentType = ContentType.TEXT_AND_IMAGE; // instead of ShowPublicTextOnScreen()
				//LCD.ShowTextOnScreen();
			}
		}
	}
	void ControlTimer(IMyTimerBlock thistimer) {
		thistimer.ApplyAction("TriggerNow");
	}

	void TriggerRotor(IMyMotorStator rotor, bool state, bool reverse, ref bool containsFalse) {
		if (!state) {
			rotor.ApplyAction("OnOff_Off"); // Stop rotor
		}	else {
			rotor.ApplyAction("OnOff_On"); // Start rotor
			containsFalse = true;
			if (reverse) { rotor.ApplyAction("Reverse"); }
		}
	}

	void GetPower(IMySolarPanel solar, ref float pwr) {
		pwr = solar.MaxOutput * 1000;
		float test = pwr / HighPwrValue * 100f;
		decimal test2 = Math.Round((decimal)test);
		textToLCD += "Current power: " + pwr.ToString() + " kWh (" + test2.ToString() + " %)\n";
		if (!EnableLCD) Echo(pwr.ToString() + " kWh");
		if ( pwr > maxPwrValue) {
			maxPwrValue = pwr;
			//HighPwrValue = maxPwrValue;
		}
	}

	void GetOxygen(IMyOxygenFarm solar, ref float pwr) {
		pwr = solar.GetOutput()* 1.8f;
		if ( pwr > maxPwrValue) {
			maxPwrValue = pwr;
		}
		if (!EnableLCD) Echo(pwr.ToString() + " L/min");
	}

	float RotorPosition(IMyMotorStator Rotor) {
		if (Rotor == null) {
			Echo( "Rotor not found. Returning 0");
			return 0;
		}
		return (float)Math.Round(Rotor.Angle * (180.0 / Math.PI), 0);
	}

	void RotorDuplicate(string SourceRotorName, string RotorName, bool Inverse) {
		GridTerminalSystem.SearchBlocksOfName(SourceRotorName, SourceRotors); // Search for Solar Source Blocks
		IMyMotorStator SourceRotor = SourceRotors[0] as IMyMotorStator; // I am a Rotor
		float SetPosition = RotorPosition(SourceRotor);
		GridTerminalSystem.SearchBlocksOfName(RotorName, Rotors); // Search for Solar Blocks
		if (Rotors.Count == 0) {
			Echo("Cannot find any duplicate destination rotors.");
			return;
		}
		if (Inverse) {
			SetPosition = -SetPosition;
		}
		for(int i = 0; i < Rotors.Count; i++) { // For each rotor...
			IMyMotorStator Rotor = Rotors[i] as IMyMotorStator; // I am a Rotor
			float DestPosition = RotorPosition(Rotor);
			if (ForceAutoTorque) {
				Rotor.BrakingTorque = float.MaxValue;
				Rotor.Torque = 10000000f;
			}
			Rotor.SetValueFloat("LowerLimit",SetPosition);
			Rotor.SetValueFloat("UpperLimit",SetPosition);
			textToLCD += "Duplication #" + i + ": ";
			if (SetPosition == DestPosition) {
				Rotor.TargetVelocityRPM = 0f;
				Rotor.ApplyAction("OnOff_Off"); // Stop rotor
				textToLCD += "Locked\n";
			} else if (SetPosition < DestPosition)  {
				Rotor.ApplyAction("OnOff_On"); // Start rotor
				Rotor.TargetVelocityRPM = -rotorspeed;
				textToLCD += "Running\n";
			} else if (SetPosition > DestPosition)  {
				Rotor.ApplyAction("OnOff_On"); // Start rotor
				Rotor.TargetVelocityRPM = rotorspeed;
				textToLCD += "Running\n";
			}
		}
	}

	void SetRotorSpeed(IMyMotorStator rotor, bool fast, bool FineTune) {
		float SetTo = 0f;
		bool VelocityPositive = (rotor.TargetVelocityRPM > 0f) ? true : false; // True if positive velocity
		if (fast) { // Under half of required power from solar panel. Will increase speed
			SetTo = (VelocityPositive) ? rotorspeed : -rotorspeed;
		} else if (FineTune) { // Fine tune speed
				SetTo = (VelocityPositive) ? rotorspeed/3.7f : -rotorspeed/3.7f;
		} else { // Not far from required power. Lower speed for increased accuracy
			SetTo = (VelocityPositive) ? rotorspeed/1.7f : -rotorspeed/1.7f;
		}
		rotor.TargetVelocityRPM = SetTo;
	}

	float GetAndSetLastPwr(IMySolarPanel solar, float CurrentPower) {
		float OldPwr = 0f;
		string[] words = solar.CustomName.Split(':'); // Colon split words
		if (words.Length > 1) {	// If there is data after colon
			if (!float.TryParse(words[1], out OldPwr)) { OldPwr = 0f;} // Try to get data into float variable
		}
		solar.CustomName = (words[0] + ":" + CurrentPower); // Set current power in solar panel name
		return OldPwr;
	}
	float GetAndSetLastOxygen(IMyOxygenFarm solar, float CurrentPower) {
		float OldPwr = 0f;
		string[] words = solar.CustomName.Split(':'); // Colon split words
		if (words.Length > 1) {	// If there is data after colon
			if (!float.TryParse(words[1], out OldPwr)) { OldPwr = 0f;} // Try to get data into float variable
		}
		solar.CustomName = (words[0] + ":" + CurrentPower); // Set current power in solar panel name
		return OldPwr;
	}

	bool CheckAndUpdateRotorName(IMyMotorStator rotor) {
		int OldCount = 0;
		string[] words = rotor.CustomName.Split(':'); // Colon split words
		 if (words.Length > 1) {	// If there is data after colon
			if (!int.TryParse(words[1], out OldCount)) { OldCount = 0;} // Try to get data into int variable
		}
		int NewCount = OldCount + 1;
		if (OldCount > 6) {
			rotor.CustomName = (words[0] + ":0");
			return true;
			}
		else if (OldCount >= 3) {
			rotor.CustomName = (words[0] + ":" + NewCount); // Set current count in rotor name
			return true;
		} else {
			rotor.CustomName = (words[0] + ":" + NewCount); // Set current count in rotor name
			return false;
		}
	}
	#endregion