// Legal/License Information:
//
// All information contained herein is, and remains the property of Vena 
// Engineering Corporation. This source code contains proprietary 
// information of Vena Engineering Corporation. This source code is 
// intended solely for use by parties operating and/or maintaining 
// equipment manufactured by Vena Engineering. Any use of this information 
// or content outside of this purpose is strictly prohibited. Such 
// proprietary information may not be used, reproduced, or disclosed to any 
// other parties for any purpose without the expressed written permission 
// of Vena Engineering Corporation. The intellectual and technical concepts 
// contained herein are proprietary to Vena Engineering Corporation and may 
// be covered by U.S. and Foreign Patents, patents in process, and are 
// protected by trade secret or copyright law. Dissemination of this 
// information or reproduction of this material is strictly forbidden 
// unless prior written permission is obtained from Vena Engineering 
// Corporation. 
// 
// The software source is provided "as is", without warranty of any kind, 
// express or implied. In no event shall Vena Engineering Corporation be 
// liable for any claim, damages or other liability, in connection with the 
// provided software. 

#import "ViewController.h"
#import "pilib.h"

@interface ViewController ()

@end

const uint32_t MTR_TABLE_SIZE = 10;

@implementation ViewController {
	uint32_t handle;
	uint32_t encoder_cnts;
	uint32_t elec_cycle_div;
	uint32_t elec_cycle_per_sec;
	uint32_t phases;
	uint32_t poles;
	mtr_segment_t mtr_table[MTR_TABLE_SIZE];
}

- (void)viewDidLoad {
	[super viewDidLoad];

	handle = pidev_init("192.168.1.200", NULL, 0);
	
	[self configMotor];

	[NSTimer scheduledTimerWithTimeInterval:0.1
								target:self
								selector:@selector(processTimer:)
								userInfo:nil
								repeats:YES];
}

- (void)configMotor {
	phases = 3;
	poles = 4;
	encoder_cnts = 512*4;
	elec_cycle_div = 0.05 * DEFAULT_CLK_PIMOTION;
	elec_cycle_per_sec = DEFAULT_CLK_PIMOTION / elec_cycle_div;
	
	//configure the PiMotion to a motor type 3-PHASE
	mtr_set_mtr_type(handle, MTR_TYPE_3PHASE);
	mtr_set_poles(handle, poles);
	mtr_set_enc_counts_per_rot(handle, encoder_cnts);
	mtr_set_enc_polarity(handle, 0);
	mtr_set_use_aux_enc_for_servo(handle, 0);
	mtr_set_output_polarity(handle, 0);
	
	//set the pid and trajectory generator update rate
	mtr_set_elec_cycle_div(handle, elec_cycle_div);
	
	//set the pid gains (proportional, integral, derivitive)
	mtr_set_kp(handle, 4.0);
	mtr_set_ki(handle, 0.5);
	mtr_set_kd(handle, 8.0);
	mtr_set_ilim(handle, 30000.0);
	
	//motor limits and position error
	mtr_set_max_pid_output(handle, 30000);
	mtr_set_max_pos_error(handle, 0); // SERVOMODE_VELOCITY no pos_error
	
	//initial phase angle configuration parameters
	mtr_set_reset_current(handle, 18000);
	mtr_set_reset_wait_time_ms(handle, 2000);
	
	[self.rpmLabel setText:@"configMotor"];
	
}

- (void)didReceiveMemoryWarning {
	[super didReceiveMemoryWarning];
	// Dispose of any resources that can be recreated.
}

- (IBAction)resetButtonTouch:(id)sender {

	//reset the motor
	uint32_t reset_state = 999;
	
	[self setStatus:@"Resetting motor..."];
	mtr_write_phase_mem_default(handle, phases, poles, encoder_cnts);
	mtr_reset(handle);
	
	while (reset_state != 0) {
		[NSThread sleepForTimeInterval:0.1];
		mtr_get_reset_state(handle, &reset_state);
	}
	[self setStatus:@"Finished resetting."];
}

- (IBAction)rpmSliderValChanged:(UISlider *)sender {
	
	uint32_t rc = 0;
	int64_t actual_vel = 0;
	uint32_t num_segments = MTR_TABLE_SIZE;
	double cps = [self rpmToCps:sender.value];
	velocity_entry_t profile = {0.0, cps,[self rpmToCps:1000.0]};
	
	[self setStatus:[NSString stringWithFormat:@"cps = %.3f, rpm = %.3f", cps, sender.value]];

	if (rc == 0) rc = mtr_get_actual_vel(handle, &actual_vel);
	if (rc == 0) rc = mtr_create_velocity_profile(handle, (double)(actual_vel * elec_cycle_per_sec), 1, &profile, SERVOMODE_VELOCITY, elec_cycle_div, &num_segments, mtr_table);
	if (rc == 0) rc = mtr_write_table(handle, 0, num_segments, mtr_table);
	if (rc == 0) rc = mtr_start(handle, 0, TRIG_SRC_NONE);
	
	if (rc != 0) {
		[self.rpmLabel setText:[NSString stringWithFormat:@"%s", err_get_string_from_code(rc)]];
	}
}

- (void)setStatus:(NSString*) msg{
	[self.myLabel setText:msg];
}

- (void)processTimer:(NSTimer*)theTimer {
	
	int64_t actual_vel;
	double rpm;

	mtr_get_actual_vel(handle, &actual_vel);
	rpm = [self cpecToRpm:actual_vel];
	[self.rpmLabel setText:[NSString stringWithFormat:@"%.0f", rpm]];
}

- (double)cpecToRpm:(int64_t)cpec {
	
	return (cpec * elec_cycle_per_sec * 60.0) / encoder_cnts;
}

- (double)rpmToCps:(double)rpm {
	return (rpm * encoder_cnts) / 60.0;
}


@end
