// 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.
//
//  This example uses a modified custom control found here:
//  http://www.thinkandbuild.it/how-to-build-a-custom-control-in-ios/
//

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

@interface ViewController ()

@end

@implementation ViewController {
	uint32_t handle;
	uint32_t elec_cycle_div;
	uint32_t elec_cycle_per_sec;
	double stepper_step_angle;
	uint32_t microstep_mult;
	double acceleration;
	double velocity;
	TBCircularSlider *circular_slider;
}

- (void)viewDidLoad {
	[super viewDidLoad];

	stepper_step_angle = 1.8; // stepper full step angle
	microstep_mult = 1024;		// the level of microstepping for the motor
	handle = pidev_init("192.168.1.200", NULL, 0);
	acceleration = 50000;
	velocity = 200000;
	
	self.view.backgroundColor = [UIColor colorWithRed:1 green:1 blue:1 alpha:1];
	circular_slider = [[TBCircularSlider alloc]initWithFrame:CGRectMake(0, 60, TB_SLIDER_SIZE, TB_SLIDER_SIZE)];
	[circular_slider addTarget:self action:@selector(newValue:) forControlEvents:UIControlEventValueChanged];
	[self.view addSubview:circular_slider];

	[self configMotor];
}

- (void)configMotor {
	uint32_t rc = 0;
	elec_cycle_div = 0.00001 * DEFAULT_CLK_PIMOTION;
	elec_cycle_per_sec = DEFAULT_CLK_PIMOTION / elec_cycle_div;
	uint32_t drive_current = 3000;		// current to drive the stepper
	
	if (rc == 0) rc = mtr_set_mtr_type(handle, MTR_TYPE_STEPPER);
	if (rc == 0) rc = mtr_set_elec_cycle_div(handle, elec_cycle_div);
	if (rc == 0) rc = mtr_set_open_loop_drive_current(handle, drive_current);
	if (rc == 0) rc = mtr_write_phase_mem_stepper(handle, microstep_mult);
	if (rc == 0) rc = mtr_reset(handle);

	NSLog(@"Finished resetting (%s)", err_get_string_from_code(rc));
}

- (void)waitForMotorDone {
	uint32_t finished = 0;
	while (finished == 0) {
		mtr_get_profile_finished(handle, &finished);
		if (finished == 0) {
			[NSThread sleepForTimeInterval:0.1];
		}
	}
}

- (void)stopMotor {
	uint32_t rc = 0;
    velocity_entry_t entries = {0.0, 0.0,2*acceleration};
	mtr_segment_t segments[3];
	uint32_t num_segments = 3;

	if (rc == 0) rc = mtr_create_velocity_profile(handle, 10000.0, 1, &entries, SERVOMODE_VELOCITY, elec_cycle_div, &num_segments, segments);
	segments[0].time_ec = 0; // stay on this segment until we get to 0 velocity
	if (rc == 0) rc = mtr_write_table(handle, 0, num_segments, segments);
	if (rc == 0) rc = mtr_start(handle, 0, TRIG_SRC_NONE);
	[self waitForMotorDone];
}

- (void)moveMotorAngle:(int32_t)angle {
	[self stopMotor];
	
	uint32_t rc = 0;
	int64_t start_pos;
	mtr_get_target_pos(handle, &start_pos);
	
	int64_t pos = (angle * (int64_t)microstep_mult) / stepper_step_angle;
	trapezoidal_entry_t entries = {0.0, pos, velocity, acceleration, acceleration};
	mtr_segment_t segments[3];
	uint32_t num_segments = 3;
	
	NSLog(@"angle = %d, start_pos = %lld, target = %lld", angle, start_pos, pos);
	
	if (start_pos != pos) {
		if (rc == 0) rc = mtr_create_trapezoidal_profile(handle, start_pos, 1, &entries, elec_cycle_div, &num_segments, segments);
		if (rc == 0) rc = mtr_write_table(handle, 0, num_segments, segments);
		if (rc == 0) rc = mtr_start(handle, 0, TRIG_SRC_NONE);
		if (rc != 0) {
			NSLog(@"%s", err_get_string_from_code(rc));
		}
	}
}

-(void)newValue:(TBCircularSlider*)slider{
	[self moveMotorAngle:slider.sum_angle];
}

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

@end
